From d913da2dde8623f7c083bf2853f5a50b7e7265bf Mon Sep 17 00:00:00 2001 From: Aswin Vijay Date: Wed, 3 Apr 2024 11:03:36 +0900 Subject: [PATCH 1/8] Initial Auto-Parking Implementation --- planning/auto_parking/CMakeLists.txt | 19 + .../config/auto_parking.param.yaml | 31 ++ .../auto_parking/auto_parking_node.hpp | 149 ++++++ .../launch/auto_parking.launch.xml | 26 + planning/auto_parking/package.xml | 32 ++ .../src/auto_parking/auto_parking_node.cpp | 498 ++++++++++++++++++ 6 files changed, 755 insertions(+) create mode 100644 planning/auto_parking/CMakeLists.txt create mode 100644 planning/auto_parking/config/auto_parking.param.yaml create mode 100644 planning/auto_parking/include/auto_parking/auto_parking_node.hpp create mode 100644 planning/auto_parking/launch/auto_parking.launch.xml create mode 100644 planning/auto_parking/package.xml create mode 100644 planning/auto_parking/src/auto_parking/auto_parking_node.cpp diff --git a/planning/auto_parking/CMakeLists.txt b/planning/auto_parking/CMakeLists.txt new file mode 100644 index 0000000000000..7937a424b2a8c --- /dev/null +++ b/planning/auto_parking/CMakeLists.txt @@ -0,0 +1,19 @@ +cmake_minimum_required(VERSION 3.14) +project(auto_parking) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_library(auto_parking_node SHARED + src/auto_parking/auto_parking_node.cpp +) + +rclcpp_components_register_node(auto_parking_node + PLUGIN "auto_parking::AutoParkingNode" + EXECUTABLE auto_parking) + +ament_auto_package( + INSTALL_TO_SHARE + launch + config +) diff --git a/planning/auto_parking/config/auto_parking.param.yaml b/planning/auto_parking/config/auto_parking.param.yaml new file mode 100644 index 0000000000000..3886ac28824bd --- /dev/null +++ b/planning/auto_parking/config/auto_parking.param.yaml @@ -0,0 +1,31 @@ +/**: + ros__parameters: + # -- Auto Park Node Configurations -- + th_arrived_distance_m: 1.0 + th_parking_space_distance_m: 10.0 + update_rate: 5.0 + vehicle_shape_margin_m: 0.2 + + # -- Configurations common to the all planners -- + # base configs + time_limit: 30000.0 + minimum_turning_radius: 9.0 + maximum_turning_radius: 9.0 + turning_radius_size: 1 + # search configs + theta_size: 144 + angle_goal_range: 6.0 + curve_weight: 1.2 + reverse_weight: 2.0 + lateral_goal_range: 0.5 + longitudinal_goal_range: 2.0 + # costmap configs + obstacle_threshold: 100 + + # -- A* search Configurations -- + astar: + only_behind_solutions: false + use_back: true + use_curve_weight: false + use_complete_astar: true + distance_heuristic_weight: 1.0 \ No newline at end of file diff --git a/planning/auto_parking/include/auto_parking/auto_parking_node.hpp b/planning/auto_parking/include/auto_parking/auto_parking_node.hpp new file mode 100644 index 0000000000000..1852251c5e122 --- /dev/null +++ b/planning/auto_parking/include/auto_parking/auto_parking_node.hpp @@ -0,0 +1,149 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/* + * Copyright 2018-2019 Autoware Foundation. All rights reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef AUTO_PARKING__AUTO_PARKING_NODE_HPP_ +#define AUTO_PARKING__AUTO_PARKING_NODE_HPP_ + +#include +#include +#include +#include +#include "tier4_autoware_utils/ros/logger_level_configure.hpp" + +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include + +using Pose = geometry_msgs::msg::Pose; +using PoseStamped = geometry_msgs::msg::PoseStamped; +using geometry_msgs::msg::TransformStamped; + +using EngageMsg = autoware_auto_vehicle_msgs::msg::Engage; +using EngageSrv = tier4_external_api_msgs::srv::Engage; + +using freespace_planning_algorithms::AbstractPlanningAlgorithm; +using freespace_planning_algorithms::AstarParam; +using freespace_planning_algorithms::AstarSearch; +using freespace_planning_algorithms::PlannerCommonParam; +using freespace_planning_algorithms::VehicleShape; +using nav_msgs::msg::OccupancyGrid; + +namespace auto_parking +{ + +struct AutoParkParam +{ + double th_arrived_distance_m; // threshold to check arrival at goal 1.0 + double th_parking_space_distance_m; // search for parking spaces within this radius 10 + double update_rate; // Timer() update rate 2 + double vehicle_shape_margin_m; // margin to add to vehicle dimensions for astar collision check, collision margin 0.2 +}; + +class AutoParkingNode : public rclcpp::Node +{ +public: + explicit AutoParkingNode(const rclcpp::NodeOptions & node_options); + + void onMap(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg); + void onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg); + void onOccupancyGrid(const OccupancyGrid::ConstSharedPtr msg); + void goalPublisher(const PoseStamped msg); + void reset(); + void onTimer(); + void filterGoalPoseinParkingLot(const lanelet::ConstLineString3d center_line, Pose& goal); + bool findParkingSpace(); + //void getGoalToParkingSpace(); + + bool isInParkingLot(); + bool initAutoParking(); + bool isArrived(const Pose& goal); + + void onEngage(EngageMsg::ConstSharedPtr msg); + void engageAutonomous(); + + // functions used in the fpa constructor + PlannerCommonParam getPlannerCommonParam(); + TransformStamped getTransform(const std::string & from, const std::string & to); + +private: + // ros + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Publisher::SharedPtr goal_pose_pub_; + + nav_msgs::msg::Odometry::ConstSharedPtr odom_; + + rclcpp::Subscription::SharedPtr sub_odom_; + rclcpp::Subscription::SharedPtr sub_lanelet_map_; + rclcpp::Subscription::SharedPtr engage_sub_; + rclcpp::Subscription::SharedPtr occupancy_grid_sub_; + rclcpp::Client::SharedPtr client_engage_; + + std::shared_ptr lanelet_map_ptr_; + std::shared_ptr nearest_parking_lot_; + lanelet::traffic_rules::TrafficRulesPtr traffic_rules_ptr_; + lanelet::routing::RoutingGraphPtr routing_graph_ptr_; + lanelet::ConstLineStrings3d nearest_parking_spaces_; + lanelet::ConstLanelets parking_lot_lanelets_; + OccupancyGrid::ConstSharedPtr occupancy_grid_; + + // fpa algo + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + std::unique_ptr algo_; + + // params + AutoParkParam node_param_; + VehicleShape vehicle_shape_; + + std::unique_ptr logger_configure_; + + PoseStamped current_goal_; + PoseStamped current_pose_; + lanelet::ConstLanelet current_lanelet_; + std::unique_ptr stop_checker_; + Pose parking_goal_; + bool set_parking_lot_goal_; + bool set_parking_space_goal_; + + bool is_engaged_; +}; +} // namespace auto_parking +#endif // AUTO_PARKING__AUTO_PARKING_NODE_HPP_ \ No newline at end of file diff --git a/planning/auto_parking/launch/auto_parking.launch.xml b/planning/auto_parking/launch/auto_parking.launch.xml new file mode 100644 index 0000000000000..794b046edcb2f --- /dev/null +++ b/planning/auto_parking/launch/auto_parking.launch.xml @@ -0,0 +1,26 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/planning/auto_parking/package.xml b/planning/auto_parking/package.xml new file mode 100644 index 0000000000000..34f57153e04e1 --- /dev/null +++ b/planning/auto_parking/package.xml @@ -0,0 +1,32 @@ + + + + auto_parking + 0.0.0 + The auto_parking package + aswinvijay + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + geometry_msgs + rclcpp + rclcpp_components + tier4_autoware_utils + lanelet2_extension + motion_utils + tier4_external_api_msgs + autoware_auto_vehicle_msgs + freespace_planning_algorithms + nav_msgs + tf2_ros + tf2 + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/auto_parking/src/auto_parking/auto_parking_node.cpp b/planning/auto_parking/src/auto_parking/auto_parking_node.cpp new file mode 100644 index 0000000000000..f73eb4f00ed5e --- /dev/null +++ b/planning/auto_parking/src/auto_parking/auto_parking_node.cpp @@ -0,0 +1,498 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/* + * Copyright 2015-2019 Autoware Foundation. All rights reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "auto_parking/auto_parking_node.hpp" + +#include +#include + +#include +#include +#include +#include +#include + +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + +using lanelet::utils::getId; +using lanelet::utils::to2D; +using Pose = geometry_msgs::msg::Pose; +using PoseStamped = geometry_msgs::msg::PoseStamped; + +namespace +{ + +std::shared_ptr filterNearestParkinglot( + const std::shared_ptr & lanelet_map_ptr, + const lanelet::BasicPoint2d & current_position) +{ + const auto all_parking_lots = lanelet::utils::query::getAllParkingLots(lanelet_map_ptr); + const auto linked_parking_lot = std::make_shared(); + *linked_parking_lot = *std::min_element(all_parking_lots.begin(), all_parking_lots.end(), + [&](const lanelet::ConstPolygon3d& p1, const lanelet::ConstPolygon3d& p2) { + const double dist_p1 = boost::geometry::distance(current_position, to2D(p1).basicPolygon()); + const double dist_p2 = boost::geometry::distance(current_position, to2D(p2).basicPolygon()); + return dist_p1 < dist_p2; + }); + if (linked_parking_lot) { + return linked_parking_lot; + } else { + return {}; + } +} + +// bool isInLane( +// const std::shared_ptr & lanelet_map_ptr, +// const geometry_msgs::msg::Point & current_pos) +// { +// const auto & p = current_pos; +// const lanelet::Point3d search_point(lanelet::InvalId, p.x, p.y, p.z); + +// std::vector> nearest_lanelets = +// lanelet::geometry::findNearest(lanelet_map_ptr->laneletLayer, search_point.basicPoint2d(), 1); + +// if (nearest_lanelets.empty()) { +// return false; +// } + +// const auto nearest_lanelet = nearest_lanelets.front().second; + +// return lanelet::geometry::within(search_point, nearest_lanelet.polygon3d()); +// } + +Pose transformPose(const Pose & pose, const TransformStamped & transform) +{ + PoseStamped transformed_pose; + PoseStamped orig_pose; + orig_pose.pose = pose; + tf2::doTransform(orig_pose, transformed_pose, transform); + + return transformed_pose.pose; +} +} // namespace + +namespace auto_parking +{ +void AutoParkingNode::goalPublisher(const PoseStamped msg) +{ + goal_pose_pub_->publish(msg); +} + +TransformStamped AutoParkingNode::getTransform( + const std::string & from, const std::string & to) +{ + TransformStamped tf; + try { + tf = + tf_buffer_->lookupTransform(from, to, rclcpp::Time(0), rclcpp::Duration::from_seconds(1.0)); + } catch (const tf2::TransformException & ex) { + RCLCPP_ERROR(get_logger(), "%s", ex.what()); + } + return tf; +} + +void AutoParkingNode::onMap( + const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg) +{ + lanelet_map_ptr_ = std::make_shared(); + lanelet::utils::conversion::fromBinMsg( + *msg, lanelet_map_ptr_, &traffic_rules_ptr_, &routing_graph_ptr_); +} + +void AutoParkingNode::onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg) +{ + odom_ = msg; + + geometry_msgs::msg::TwistStamped twist; + twist.header = msg->header; + twist.twist = msg->twist.twist; + stop_checker_->addTwist(twist); +} + +void AutoParkingNode::onOccupancyGrid(const OccupancyGrid::ConstSharedPtr msg) +{ + occupancy_grid_ = msg; +} + +PlannerCommonParam AutoParkingNode::getPlannerCommonParam() +{ + PlannerCommonParam p; + + // search configs + p.time_limit = declare_parameter("time_limit"); + p.minimum_turning_radius = declare_parameter("minimum_turning_radius"); + p.maximum_turning_radius = declare_parameter("maximum_turning_radius"); + p.turning_radius_size = declare_parameter("turning_radius_size"); + p.maximum_turning_radius = std::max(p.maximum_turning_radius, p.minimum_turning_radius); + p.turning_radius_size = std::max(p.turning_radius_size, 1); + + p.theta_size = declare_parameter("theta_size"); + p.angle_goal_range = declare_parameter("angle_goal_range"); + p.curve_weight = declare_parameter("curve_weight"); + p.reverse_weight = declare_parameter("reverse_weight"); + p.lateral_goal_range = declare_parameter("lateral_goal_range"); + p.longitudinal_goal_range = declare_parameter("longitudinal_goal_range"); + + // costmap configs + p.obstacle_threshold = declare_parameter("obstacle_threshold"); + + return p; +} + +bool AutoParkingNode::isArrived(const Pose& goal) +{ + // Check distance to current goal + if (node_param_.th_arrived_distance_m < tier4_autoware_utils::calcDistance2d(current_pose_.pose, goal)) { + return false; + } + return true; +} + +void AutoParkingNode::filterGoalPoseinParkingLot(const lanelet::ConstLineString3d center_line, + Pose& goal) +{ + for(size_t i = 0; i < center_line.size(); i++){ + const lanelet::Point3d search_point(lanelet::InvalId, center_line[i].x(), center_line[i].y(), 0.0); + if(lanelet::geometry::within(search_point, nearest_parking_lot_->basicPolygon())){ + goal.position.x = center_line[i].x(); + goal.position.y = center_line[i].y(); + goal.position.z = 0.0; + const auto yaw = std::atan2( + center_line[i+1].y() - center_line[i].y(), center_line[i+1].x() - center_line[i].x()); + tf2::Quaternion q; + q.setRPY(0, 0, yaw); + goal.orientation = tf2::toMsg(q); + return; + } + } +} + +bool AutoParkingNode::isInParkingLot() +{ + const auto & p = current_pose_.pose.position; + const lanelet::Point3d search_point(lanelet::InvalId, p.x, p.y, p.z); + if(lanelet::geometry::within(search_point, nearest_parking_lot_->basicPolygon())){ + return true; + } + return false; +} + +bool AutoParkingNode::initAutoParking() +{ + const auto & p = current_pose_.pose.position; + const lanelet::Point3d search_point(lanelet::InvalId, p.x, p.y, p.z); + + nearest_parking_lot_ = + filterNearestParkinglot(lanelet_map_ptr_, search_point.basicPoint2d()); + if(!nearest_parking_lot_){ + RCLCPP_INFO(get_logger(), "No parking lot found!!"); + return false; + } + + const auto & all_parking_spaces_ = + lanelet::utils::query::getAllParkingSpaces(lanelet_map_ptr_); + + nearest_parking_spaces_ = + lanelet::utils::query::getLinkedParkingSpaces(*nearest_parking_lot_, all_parking_spaces_); + + const auto & all_lanelets = lanelet::utils::query::laneletLayer(lanelet_map_ptr_); + const auto & all_road_lanelets = lanelet::utils::query::roadLanelets(all_lanelets); + + parking_lot_lanelets_ = + lanelet::utils::query::getLinkedLanelets(*nearest_parking_lot_, all_road_lanelets); + + // Get closest lanelet to ego-vehicle + if(!lanelet::utils::query::getClosestLanelet(all_road_lanelets, + current_pose_.pose, ¤t_lanelet_)){ + RCLCPP_INFO(get_logger(), "No close lanelet to vehicle found!!"); + return false; + } + + // Order parking lot lanelets by route length + // - Sort lanelets + std::sort(parking_lot_lanelets_.begin(), parking_lot_lanelets_.end(), + [&](const lanelet::ConstLanelet& l1, const lanelet::ConstLanelet& l2){ + const auto l1_length = routing_graph_ptr_->shortestPath(current_lanelet_, l1, {}, false)->size(); + const auto l2_length = routing_graph_ptr_->shortestPath(current_lanelet_, l2, {}, false)->size(); + return ( l1_length < l2_length); + }); + + // Set parking lot goal, goal at exit of parking lot + // Assuming parking lot has one entry, longest route is from + // entry to exit. + lanelet::ConstLineString3d exit_llt_cline = + parking_lot_lanelets_.back().centerline(); // final lanelet from sorted lanelets + filterGoalPoseinParkingLot(exit_llt_cline, parking_goal_); + + // print out sortet lanelet ID's for DEBUG. + // for (const auto & lanelet : parking_lot_lanelets_) { + // // check if parking space is close to lanelet + // RCLCPP_INFO(get_logger(),"LaneId: %li \n", getId(lanelet)); + // } + + return true; +} + +bool AutoParkingNode::findParkingSpace() +{ + const auto & p = current_pose_.pose.position; + const lanelet::Point2d search_point(lanelet::InvalId, p.x, p.y); + + // Set occupancy map and current pose + algo_->setMap(*occupancy_grid_); + const auto current_pose_in_costmap_frame = transformPose( + current_pose_.pose, + getTransform(occupancy_grid_->header.frame_id, current_pose_.header.frame_id)); + + // Cycle through parking spaces + for(const auto & parking_space : nearest_parking_spaces_){ + const double dist = boost::geometry::distance(to2D(parking_space).basicLineString(), search_point); + // Check if parking space is nearby + if(node_param_.th_parking_space_distance_m < dist){ + continue; + } + + // Compute two goal poses for each parking space + double distance_thresh = boost::geometry::distance(parking_space.front().basicPoint(), + parking_space.back().basicPoint()) / 4; + lanelet::ConstPoints3d p_space_fw_bk; + p_space_fw_bk.push_back(parking_space.back()); + p_space_fw_bk.push_back(parking_space.front()); + + // Compute parking space poses, orientations i = 0 , 1 + // Check if freespace trajectory available then set parking space goal + // Check back to front pose first then opposite + + for(auto i = 0; i < 2; i++){ + + auto in1 = i % 2; + auto in2 = (i + 1) % 2; + + Eigen::Vector3d direction = + p_space_fw_bk[in2].basicPoint() - p_space_fw_bk[in1].basicPoint(); + direction.normalize(); + + // Set pose + const Eigen::Vector3d goal_pose = p_space_fw_bk[in1].basicPoint() + direction * distance_thresh; + const auto yaw = std::atan2(direction.y(), direction.x()); + tf2::Quaternion q; + q.setRPY(0, 0, yaw); + + Pose goal; + goal.position.x = goal_pose.x(); + goal.position.y = goal_pose.y(); + goal.position.z = goal_pose.z(); + goal.orientation = tf2::toMsg(q); + + current_goal_.pose = goal; + current_goal_.header.frame_id = odom_->header.frame_id; + current_goal_.header.stamp = rclcpp::Time(); + + const auto goal_pose_in_costmap_frame = transformPose( + current_goal_.pose, + getTransform(occupancy_grid_->header.frame_id, current_goal_.header.frame_id)); + bool result = algo_->makePlan(current_pose_in_costmap_frame, goal_pose_in_costmap_frame); + + if(result){ + RCLCPP_INFO(get_logger(), "Found free parking space!"); + return true; + } + + continue; + } + } + + // No free parking spaces found + return false; +} + +void AutoParkingNode::onEngage(EngageMsg::ConstSharedPtr msg) +{ + is_engaged_ = msg->engage; +} + +void AutoParkingNode::engageAutonomous() +{ + // engage + auto req = std::make_shared(); + req->engage = true; + RCLCPP_INFO(get_logger(), "auto-engage client request"); + if (!client_engage_->service_is_ready()) { + RCLCPP_INFO(get_logger(), "auto-engage client is unavailable"); + return; + } + client_engage_->async_send_request( + req, []([[maybe_unused]] rclcpp::Client::SharedFuture result) {}); +} + +void AutoParkingNode::reset() +{ + set_parking_lot_goal_ = false; + set_parking_space_goal_ = false; +} + +void AutoParkingNode::onTimer() +{ + // Check all inputs are ready + if (!odom_ || !lanelet_map_ptr_ || !routing_graph_ptr_ || + !engage_sub_ || !client_engage_) { + return; + } + + current_pose_.pose = odom_->pose.pose; + current_pose_.header = odom_->header; + + if (current_pose_.header.frame_id == "") { + return; + } + + // Publish parking lot entrance goal + if(!set_parking_lot_goal_){ + if(!initAutoParking()){ return; } + current_goal_.header.frame_id = odom_->header.frame_id; + current_goal_.header.stamp = rclcpp::Time(); + current_goal_.pose = parking_goal_; + goalPublisher(current_goal_); + RCLCPP_INFO(get_logger(), "Publishing parking lot goal"); + set_parking_lot_goal_ = true; + } + + // If stopped replan: TODO + //const auto is_vehicle_stopped = stop_checker_->isVehicleStopped(1.0); + + // Arrived at parking lot exit without finding parking space + if(set_parking_lot_goal_ && isArrived(parking_goal_)){ + RCLCPP_INFO(get_logger(), "Auto-parking: failed to find parking space"); + return; + } + + // Search parking spaces if inside parking lot + if(!set_parking_space_goal_ && + isInParkingLot() && + occupancy_grid_) + { + RCLCPP_INFO(get_logger(), "Searching..."); + if(findParkingSpace()){ + goalPublisher(current_goal_); + RCLCPP_INFO(get_logger(), "Publishing parking space goal"); + set_parking_space_goal_ = true; + } + } + + // Engage autonomous once a goal is set + // For smooth transition remove vehicle stopping after publishing goal pose in freespace planner node + if(!is_engaged_ && !isArrived(current_goal_.pose)){ + engageAutonomous(); + } +} + +AutoParkingNode::AutoParkingNode(const rclcpp::NodeOptions & node_options) +: Node("auto_parking", node_options) +{ + + // Auto-park params + { + auto & p = node_param_; + p.th_arrived_distance_m = declare_parameter("th_arrived_distance_m"); + p.th_parking_space_distance_m = declare_parameter("th_parking_space_distance_m"); + p.update_rate = declare_parameter("update_rate"); + p.vehicle_shape_margin_m = declare_parameter("vehicle_shape_margin_m"); + } + + // set vehicle_info for astar + { + const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); + vehicle_shape_.length = vehicle_info.vehicle_length_m + node_param_.vehicle_shape_margin_m; + vehicle_shape_.width = vehicle_info.vehicle_width_m + node_param_.vehicle_shape_margin_m; + vehicle_shape_.base2back = vehicle_info.rear_overhang_m + node_param_.vehicle_shape_margin_m / 2; + } + + // Subscribers + { + sub_lanelet_map_ = this->create_subscription( + "~/input/lanelet_map_bin", rclcpp::QoS{10}.transient_local(), + std::bind(&AutoParkingNode::onMap, this, std::placeholders::_1)); + + sub_odom_ = this->create_subscription( + "~/input/odometry", rclcpp::QoS{100}, + std::bind(&AutoParkingNode::onOdometry, this, std::placeholders::_1)); + + engage_sub_ = create_subscription( + "~/input/engage", rclcpp::QoS{5}, std::bind(&AutoParkingNode::onEngage, this, std::placeholders::_1)); + + occupancy_grid_sub_ = create_subscription( + "~/input/occupancy_grid", 10, std::bind(&AutoParkingNode::onOccupancyGrid, this, std::placeholders::_1)); + } + + // Publishers + { + rclcpp::QoS qos{1}; + qos.transient_local(); // latch + goal_pose_pub_ = this->create_publisher("~/output/fixed_goal", qos); + } + + // Client + { + client_engage_ = this->create_client("~/service/engage"); + } + + // TF + { + tf_buffer_ = std::make_shared(get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); + } + + // Timer + { + const auto period_ns = rclcpp::Rate(node_param_.update_rate).period(); + timer_ = rclcpp::create_timer( + this, get_clock(), period_ns, std::bind(&AutoParkingNode::onTimer, this)); + } + + // Initialize Freespace planning algorithm - astar + { + const auto planner_common_param = getPlannerCommonParam(); + algo_ = std::make_unique(planner_common_param, vehicle_shape_, *this); + } + + reset(); + stop_checker_ = std::make_unique(this, 1.0 + 1.0); + logger_configure_ = std::make_unique(this); +} +} + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(auto_parking::AutoParkingNode) From 33f76938e4c97789b869ec732a177dbeffab5607 Mon Sep 17 00:00:00 2001 From: Aswin Vijay Date: Wed, 3 Apr 2024 13:08:37 +0900 Subject: [PATCH 2/8] add check to see if active --- .../config/auto_parking.param.yaml | 4 +- .../auto_parking/auto_parking_node.hpp | 1 + .../src/auto_parking/auto_parking_node.cpp | 44 +++++++------------ 3 files changed, 19 insertions(+), 30 deletions(-) diff --git a/planning/auto_parking/config/auto_parking.param.yaml b/planning/auto_parking/config/auto_parking.param.yaml index 3886ac28824bd..6d7812f4c3518 100644 --- a/planning/auto_parking/config/auto_parking.param.yaml +++ b/planning/auto_parking/config/auto_parking.param.yaml @@ -9,9 +9,9 @@ # -- Configurations common to the all planners -- # base configs time_limit: 30000.0 - minimum_turning_radius: 9.0 + minimum_turning_radius: 5.0 maximum_turning_radius: 9.0 - turning_radius_size: 1 + turning_radius_size: 3 # search configs theta_size: 144 angle_goal_range: 6.0 diff --git a/planning/auto_parking/include/auto_parking/auto_parking_node.hpp b/planning/auto_parking/include/auto_parking/auto_parking_node.hpp index 1852251c5e122..1d9dfa1aa463c 100644 --- a/planning/auto_parking/include/auto_parking/auto_parking_node.hpp +++ b/planning/auto_parking/include/auto_parking/auto_parking_node.hpp @@ -142,6 +142,7 @@ class AutoParkingNode : public rclcpp::Node Pose parking_goal_; bool set_parking_lot_goal_; bool set_parking_space_goal_; + bool active_; bool is_engaged_; }; diff --git a/planning/auto_parking/src/auto_parking/auto_parking_node.cpp b/planning/auto_parking/src/auto_parking/auto_parking_node.cpp index f73eb4f00ed5e..5055ada500ac8 100644 --- a/planning/auto_parking/src/auto_parking/auto_parking_node.cpp +++ b/planning/auto_parking/src/auto_parking/auto_parking_node.cpp @@ -74,25 +74,6 @@ std::shared_ptr filterNearestParkinglot( } } -// bool isInLane( -// const std::shared_ptr & lanelet_map_ptr, -// const geometry_msgs::msg::Point & current_pos) -// { -// const auto & p = current_pos; -// const lanelet::Point3d search_point(lanelet::InvalId, p.x, p.y, p.z); - -// std::vector> nearest_lanelets = -// lanelet::geometry::findNearest(lanelet_map_ptr->laneletLayer, search_point.basicPoint2d(), 1); - -// if (nearest_lanelets.empty()) { -// return false; -// } - -// const auto nearest_lanelet = nearest_lanelets.front().second; - -// return lanelet::geometry::within(search_point, nearest_lanelet.polygon3d()); -// } - Pose transformPose(const Pose & pose, const TransformStamped & transform) { PoseStamped transformed_pose; @@ -327,10 +308,9 @@ bool AutoParkingNode::findParkingSpace() bool result = algo_->makePlan(current_pose_in_costmap_frame, goal_pose_in_costmap_frame); if(result){ - RCLCPP_INFO(get_logger(), "Found free parking space!"); + RCLCPP_INFO(get_logger(), "Auto-parking: Found free parking space!"); return true; } - continue; } } @@ -349,9 +329,9 @@ void AutoParkingNode::engageAutonomous() // engage auto req = std::make_shared(); req->engage = true; - RCLCPP_INFO(get_logger(), "auto-engage client request"); + RCLCPP_INFO(get_logger(), "Auto-parking: auto-engage client request"); if (!client_engage_->service_is_ready()) { - RCLCPP_INFO(get_logger(), "auto-engage client is unavailable"); + RCLCPP_INFO(get_logger(), "Auto-parking: auto-engage client is unavailable"); return; } client_engage_->async_send_request( @@ -362,13 +342,14 @@ void AutoParkingNode::reset() { set_parking_lot_goal_ = false; set_parking_space_goal_ = false; + active_ = true; } void AutoParkingNode::onTimer() { // Check all inputs are ready if (!odom_ || !lanelet_map_ptr_ || !routing_graph_ptr_ || - !engage_sub_ || !client_engage_) { + !engage_sub_ || !client_engage_ || !active_) { return; } @@ -386,7 +367,7 @@ void AutoParkingNode::onTimer() current_goal_.header.stamp = rclcpp::Time(); current_goal_.pose = parking_goal_; goalPublisher(current_goal_); - RCLCPP_INFO(get_logger(), "Publishing parking lot goal"); + RCLCPP_INFO(get_logger(), "Auto-parking: Publishing parking lot goal"); set_parking_lot_goal_ = true; } @@ -395,7 +376,7 @@ void AutoParkingNode::onTimer() // Arrived at parking lot exit without finding parking space if(set_parking_lot_goal_ && isArrived(parking_goal_)){ - RCLCPP_INFO(get_logger(), "Auto-parking: failed to find parking space"); + RCLCPP_INFO(get_logger(), "Auto-parking: Failed to find parking space"); return; } @@ -404,14 +385,21 @@ void AutoParkingNode::onTimer() isInParkingLot() && occupancy_grid_) { - RCLCPP_INFO(get_logger(), "Searching..."); + RCLCPP_INFO(get_logger(), "Auto-parking: Searching..."); if(findParkingSpace()){ goalPublisher(current_goal_); - RCLCPP_INFO(get_logger(), "Publishing parking space goal"); + RCLCPP_INFO(get_logger(), "Auto-parking: Publishing parking space goal"); set_parking_space_goal_ = true; } } + // Arrived at parking space goal + if(set_parking_space_goal_ && isArrived(current_goal_.pose)){ + RCLCPP_INFO(get_logger(), "Auto-parking: Complete!"); + active_ = false; + return; + } + // Engage autonomous once a goal is set // For smooth transition remove vehicle stopping after publishing goal pose in freespace planner node if(!is_engaged_ && !isArrived(current_goal_.pose)){ From ffe35a023774176b5829258e8ab8edd287a1766b Mon Sep 17 00:00:00 2001 From: Aswin Vijay Date: Tue, 9 Apr 2024 11:36:56 +0900 Subject: [PATCH 3/8] rviz integration auto_park --- .../auto_parking/auto_parking_node.hpp | 22 +++-- .../launch/auto_parking.launch.xml | 6 +- planning/auto_parking/package.xml | 3 +- .../src/auto_parking/auto_parking_node.cpp | 85 ++++++++++++++++--- 4 files changed, 97 insertions(+), 19 deletions(-) diff --git a/planning/auto_parking/include/auto_parking/auto_parking_node.hpp b/planning/auto_parking/include/auto_parking/auto_parking_node.hpp index 1d9dfa1aa463c..ed7d0c7242f1e 100644 --- a/planning/auto_parking/include/auto_parking/auto_parking_node.hpp +++ b/planning/auto_parking/include/auto_parking/auto_parking_node.hpp @@ -33,6 +33,8 @@ #include #include +#include +#include #include #include #include "tier4_autoware_utils/ros/logger_level_configure.hpp" @@ -90,7 +92,6 @@ class AutoParkingNode : public rclcpp::Node void onTimer(); void filterGoalPoseinParkingLot(const lanelet::ConstLineString3d center_line, Pose& goal); bool findParkingSpace(); - //void getGoalToParkingSpace(); bool isInParkingLot(); bool initAutoParking(); @@ -98,6 +99,9 @@ class AutoParkingNode : public rclcpp::Node void onEngage(EngageMsg::ConstSharedPtr msg); void engageAutonomous(); + void onSetActiveStatus( + const std_srvs::srv::SetBool::Request::SharedPtr req, + std_srvs::srv::SetBool::Response::SharedPtr res); // functions used in the fpa constructor PlannerCommonParam getPlannerCommonParam(); @@ -106,16 +110,20 @@ class AutoParkingNode : public rclcpp::Node private: // ros rclcpp::TimerBase::SharedPtr timer_; - rclcpp::Publisher::SharedPtr goal_pose_pub_; - nav_msgs::msg::Odometry::ConstSharedPtr odom_; + + rclcpp::Publisher::SharedPtr goal_pose_pub_; + rclcpp::Publisher::SharedPtr active_status_pub_; - rclcpp::Subscription::SharedPtr sub_odom_; - rclcpp::Subscription::SharedPtr sub_lanelet_map_; + rclcpp::Subscription::SharedPtr odom_sub_; + rclcpp::Subscription::SharedPtr lanelet_map_sub_; rclcpp::Subscription::SharedPtr engage_sub_; rclcpp::Subscription::SharedPtr occupancy_grid_sub_; + rclcpp::Client::SharedPtr client_engage_; + rclcpp::Service::SharedPtr srv_set_active_; + std::shared_ptr lanelet_map_ptr_; std::shared_ptr nearest_parking_lot_; lanelet::traffic_rules::TrafficRulesPtr traffic_rules_ptr_; @@ -124,7 +132,7 @@ class AutoParkingNode : public rclcpp::Node lanelet::ConstLanelets parking_lot_lanelets_; OccupancyGrid::ConstSharedPtr occupancy_grid_; - // fpa algo + // fpa algo vars std::shared_ptr tf_buffer_; std::shared_ptr tf_listener_; std::unique_ptr algo_; @@ -143,8 +151,8 @@ class AutoParkingNode : public rclcpp::Node bool set_parking_lot_goal_; bool set_parking_space_goal_; bool active_; - bool is_engaged_; + }; } // namespace auto_parking #endif // AUTO_PARKING__AUTO_PARKING_NODE_HPP_ \ No newline at end of file diff --git a/planning/auto_parking/launch/auto_parking.launch.xml b/planning/auto_parking/launch/auto_parking.launch.xml index 794b046edcb2f..27e772ce231be 100644 --- a/planning/auto_parking/launch/auto_parking.launch.xml +++ b/planning/auto_parking/launch/auto_parking.launch.xml @@ -2,11 +2,13 @@ - + + + @@ -20,6 +22,8 @@ + + diff --git a/planning/auto_parking/package.xml b/planning/auto_parking/package.xml index 34f57153e04e1..c73a6f269ff17 100644 --- a/planning/auto_parking/package.xml +++ b/planning/auto_parking/package.xml @@ -4,7 +4,7 @@ auto_parking 0.0.0 The auto_parking package - aswinvijay + Aswin Vijay Apache License 2.0 ament_cmake_auto @@ -22,6 +22,7 @@ nav_msgs tf2_ros tf2 + std_srvs ament_lint_auto autoware_lint_common diff --git a/planning/auto_parking/src/auto_parking/auto_parking_node.cpp b/planning/auto_parking/src/auto_parking/auto_parking_node.cpp index 5055ada500ac8..d526b1c330b7a 100644 --- a/planning/auto_parking/src/auto_parking/auto_parking_node.cpp +++ b/planning/auto_parking/src/auto_parking/auto_parking_node.cpp @@ -54,7 +54,7 @@ using PoseStamped = geometry_msgs::msg::PoseStamped; namespace { - +// get nearest parking lot by distance std::shared_ptr filterNearestParkinglot( const std::shared_ptr & lanelet_map_ptr, const lanelet::BasicPoint2d & current_position) @@ -128,6 +128,20 @@ void AutoParkingNode::onOccupancyGrid(const OccupancyGrid::ConstSharedPtr msg) occupancy_grid_ = msg; } +void AutoParkingNode::onSetActiveStatus( + const std_srvs::srv::SetBool::Request::SharedPtr req, + std_srvs::srv::SetBool::Response::SharedPtr res) +{ + if(req->data) { + reset(); + active_ = true; + } else { + active_ = false; + } + res->success = true; + return; +} + PlannerCommonParam AutoParkingNode::getPlannerCommonParam() { PlannerCommonParam p; @@ -342,14 +356,24 @@ void AutoParkingNode::reset() { set_parking_lot_goal_ = false; set_parking_space_goal_ = false; - active_ = true; + active_ = false; } void AutoParkingNode::onTimer() { + // Publish active status message + std_msgs::msg::Bool is_active_msg; + is_active_msg.data = active_; + active_status_pub_->publish(is_active_msg); + + if (!active_){ + return; + } + // Check all inputs are ready if (!odom_ || !lanelet_map_ptr_ || !routing_graph_ptr_ || - !engage_sub_ || !client_engage_ || !active_) { + !engage_sub_ || !client_engage_) { + active_ = false; return; } @@ -357,12 +381,18 @@ void AutoParkingNode::onTimer() current_pose_.header = odom_->header; if (current_pose_.header.frame_id == "") { + active_ = false; return; } // Publish parking lot entrance goal if(!set_parking_lot_goal_){ - if(!initAutoParking()){ return; } + // Initialize variables + if(!initAutoParking()){ + RCLCPP_INFO(get_logger(), "Auto-parking: Initialization failed!"); + active_ = false; + return; + } current_goal_.header.frame_id = odom_->header.frame_id; current_goal_.header.stamp = rclcpp::Time(); current_goal_.pose = parking_goal_; @@ -371,12 +401,10 @@ void AutoParkingNode::onTimer() set_parking_lot_goal_ = true; } - // If stopped replan: TODO - //const auto is_vehicle_stopped = stop_checker_->isVehicleStopped(1.0); - // Arrived at parking lot exit without finding parking space if(set_parking_lot_goal_ && isArrived(parking_goal_)){ RCLCPP_INFO(get_logger(), "Auto-parking: Failed to find parking space"); + active_ = false; return; } @@ -393,6 +421,30 @@ void AutoParkingNode::onTimer() } } + // Check if astar goal is still valid + // else replan + if(set_parking_space_goal_ && + isInParkingLot() && + occupancy_grid_) + { + // Set occupancy map and current pose + algo_->setMap(*occupancy_grid_); + const auto current_pose_in_costmap_frame = transformPose( + current_pose_.pose, + getTransform(occupancy_grid_->header.frame_id, current_pose_.header.frame_id)); + const auto goal_pose_in_costmap_frame = transformPose( + current_goal_.pose, + getTransform(occupancy_grid_->header.frame_id, current_goal_.header.frame_id)); + bool result = algo_->makePlan(current_pose_in_costmap_frame, goal_pose_in_costmap_frame); + if(!result) { + set_parking_space_goal_ = false; + current_goal_.pose = parking_goal_; + goalPublisher(current_goal_); + set_parking_lot_goal_ = true; + return; + } + } + // Arrived at parking space goal if(set_parking_space_goal_ && isArrived(current_goal_.pose)){ RCLCPP_INFO(get_logger(), "Auto-parking: Complete!"); @@ -400,8 +452,12 @@ void AutoParkingNode::onTimer() return; } + // If ego stopped/stuck replan: TODO + // const auto is_vehicle_stopped = stop_checker_->isVehicleStopped(1.0); + // Engage autonomous once a goal is set - // For smooth transition remove vehicle stopping after publishing goal pose in freespace planner node + // For smooth transition remove vehicle stopping after + // publishing goal pose in freespace planner node if(!is_engaged_ && !isArrived(current_goal_.pose)){ engageAutonomous(); } @@ -430,11 +486,11 @@ AutoParkingNode::AutoParkingNode(const rclcpp::NodeOptions & node_options) // Subscribers { - sub_lanelet_map_ = this->create_subscription( + lanelet_map_sub_ = this->create_subscription( "~/input/lanelet_map_bin", rclcpp::QoS{10}.transient_local(), std::bind(&AutoParkingNode::onMap, this, std::placeholders::_1)); - sub_odom_ = this->create_subscription( + odom_sub_ = this->create_subscription( "~/input/odometry", rclcpp::QoS{100}, std::bind(&AutoParkingNode::onOdometry, this, std::placeholders::_1)); @@ -450,6 +506,15 @@ AutoParkingNode::AutoParkingNode(const rclcpp::NodeOptions & node_options) rclcpp::QoS qos{1}; qos.transient_local(); // latch goal_pose_pub_ = this->create_publisher("~/output/fixed_goal", qos); + active_status_pub_ = this->create_publisher("~/output/active_status", qos); + } + + // Service + { + srv_set_active_ = create_service( + "~/service/set_active", + std::bind( + &AutoParkingNode::onSetActiveStatus, this, std::placeholders::_1, std::placeholders::_2)); } // Client From 692c09550b4db9574c30a4bff861cd7abbf5207f Mon Sep 17 00:00:00 2001 From: Aswin Vijay Date: Tue, 9 Apr 2024 11:52:45 +0900 Subject: [PATCH 4/8] rviz integration and autoware launch implementation --- common/tier4_state_rviz_plugin/package.xml | 1 + .../src/autoware_state_panel.cpp | 76 +++++++++++++++++++ .../src/autoware_state_panel.hpp | 13 ++++ .../scenario_planning/parking.launch.xml | 22 +++++- 4 files changed, 111 insertions(+), 1 deletion(-) diff --git a/common/tier4_state_rviz_plugin/package.xml b/common/tier4_state_rviz_plugin/package.xml index 06d57bd947af3..556e1a635fa35 100644 --- a/common/tier4_state_rviz_plugin/package.xml +++ b/common/tier4_state_rviz_plugin/package.xml @@ -23,6 +23,7 @@ tier4_control_msgs tier4_external_api_msgs tier4_planning_msgs + std_srvs ament_lint_auto autoware_lint_common diff --git a/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp b/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp index aa1dcfcd1651d..daa516eb29f2d 100644 --- a/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp +++ b/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp @@ -61,6 +61,7 @@ AutowareStatePanel::AutowareStatePanel(QWidget * parent) : rviz_common::Panel(pa auto * velocity_limit_layout = new QHBoxLayout(); v_layout->addWidget(makeOperationModeGroup()); v_layout->addWidget(makeControlModeGroup()); + v_layout->addWidget(makeAutoParkModeGroup()); { auto * h_layout = new QHBoxLayout(); h_layout->addWidget(makeRoutingGroup()); @@ -137,6 +138,25 @@ QGroupBox * AutowareStatePanel::makeControlModeGroup() return group; } +QGroupBox * AutowareStatePanel::makeAutoParkModeGroup() +{ + auto * group = new QGroupBox("AutoParking"); + auto * grid = new QGridLayout; + + park_mode_label_ptr_ = new QLabel("INIT"); + park_mode_label_ptr_->setAlignment(Qt::AlignCenter); + park_mode_label_ptr_->setStyleSheet("border:1px solid black;"); + grid->addWidget(park_mode_label_ptr_, 0, 0); + + p_enable_button_ptr_ = new QPushButton("Enable"); + p_enable_button_ptr_->setCheckable(true); + connect(p_enable_button_ptr_, SIGNAL(clicked()), SLOT(onClickEngageAutoPark())); + grid->addWidget(p_enable_button_ptr_, 0, 1); + + group->setLayout(grid); + return group; +} + QGroupBox * AutowareStatePanel::makeRoutingGroup() { auto * group = new QGroupBox("Routing"); @@ -222,6 +242,9 @@ void AutowareStatePanel::onInitialize() sub_operation_mode_ = raw_node_->create_subscription( "/api/operation_mode/state", rclcpp::QoS{1}.transient_local(), std::bind(&AutowareStatePanel::onOperationMode, this, _1)); + sub_auto_park_status_ = raw_node_->create_subscription( + "/planning/auto_parking/status", rclcpp::QoS{1}.transient_local(), + std::bind(&AutowareStatePanel::onAutoParkStatus, this, _1)); client_change_to_autonomous_ = raw_node_->create_client("/api/operation_mode/change_to_autonomous"); @@ -241,6 +264,9 @@ void AutowareStatePanel::onInitialize() client_enable_direct_control_ = raw_node_->create_client("/api/operation_mode/disable_autoware_control"); + client_auto_park_ = + raw_node_->create_client("/planning/auto_parking/set_status"); + // Routing sub_route_ = raw_node_->create_subscription( "/api/routing/state", rclcpp::QoS{1}.transient_local(), @@ -352,6 +378,27 @@ void AutowareStatePanel::onOperationMode(const OperationModeState::ConstSharedPt changeButtonState(disable_button_ptr_, msg->is_autoware_control_enabled); } +void AutowareStatePanel::onAutoParkStatus(const std_msgs::msg::Bool::ConstSharedPtr msg) +{ + auto_park_running_ = msg->data; + auto changeButtonState = [this]( + QPushButton * button, const bool is_desired_mode_available, + const uint8_t current_mode = OperationModeState::UNKNOWN, + const uint8_t desired_mode = OperationModeState::STOP) { + if (is_desired_mode_available && current_mode != desired_mode) { + activateButton(button); + } else { + deactivateButton(button); + } + }; + if (auto_park_running_) { + updateLabel(park_mode_label_ptr_, "Enable", "background-color: #00FF00;"); // green + } else { + updateLabel(park_mode_label_ptr_, "Disable", "background-color: #FFFF00;"); // yellow + } + changeButtonState(p_enable_button_ptr_, !auto_park_running_); +} + void AutowareStatePanel::onRoute(const RouteState::ConstSharedPtr msg) { QString text = ""; @@ -571,6 +618,9 @@ void AutowareStatePanel::onClickAutonomous() void AutowareStatePanel::onClickStop() { callServiceWithoutResponse(client_change_to_stop_); + if(auto_park_running_){ + onClickDisengageAutoPark(); + } } void AutowareStatePanel::onClickLocal() { @@ -584,6 +634,32 @@ void AutowareStatePanel::onClickAutowareControl() { callServiceWithoutResponse(client_enable_autoware_control_); } +void AutowareStatePanel::onClickEngageAutoPark() +{ + auto req = std::make_shared(); + req->data = true; + RCLCPP_DEBUG(raw_node_->get_logger(), "client request"); + if (!client_auto_park_->service_is_ready()) { + RCLCPP_DEBUG(raw_node_->get_logger(), "client is unavailable"); + return; + } + client_auto_park_->async_send_request( + req, + [this]([[maybe_unused]] rclcpp::Client::SharedFuture result) {}); +} +void AutowareStatePanel::onClickDisengageAutoPark() +{ + auto req = std::make_shared(); + req->data = false; + RCLCPP_DEBUG(raw_node_->get_logger(), "client request"); + if (!client_auto_park_->service_is_ready()) { + RCLCPP_DEBUG(raw_node_->get_logger(), "client is unavailable"); + return; + } + client_auto_park_->async_send_request( + req, + [this]([[maybe_unused]] rclcpp::Client::SharedFuture result) {}); +} void AutowareStatePanel::onClickDirectControl() { callServiceWithoutResponse(client_enable_direct_control_); diff --git a/common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp b/common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp index 8f67a90215bd1..246a7328c8905 100644 --- a/common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp +++ b/common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp @@ -25,6 +25,8 @@ #include #include #include +#include +#include #include #include @@ -69,6 +71,8 @@ public Q_SLOTS: // NOLINT for Qt void onClickLocal(); void onClickRemote(); void onClickAutowareControl(); + void onClickEngageAutoPark(); + void onClickDisengageAutoPark(); void onClickDirectControl(); void onClickClearRoute(); void onClickInitByGnss(); @@ -80,6 +84,7 @@ public Q_SLOTS: // NOLINT for Qt // Layout QGroupBox * makeOperationModeGroup(); QGroupBox * makeControlModeGroup(); + QGroupBox * makeAutoParkModeGroup(); QGroupBox * makeRoutingGroup(); QGroupBox * makeLocalizationGroup(); QGroupBox * makeMotionGroup(); @@ -106,6 +111,7 @@ public Q_SLOTS: // NOLINT for Qt QPushButton * remote_button_ptr_{nullptr}; rclcpp::Subscription::SharedPtr sub_operation_mode_; + rclcpp::Subscription::SharedPtr sub_auto_park_status_; rclcpp::Client::SharedPtr client_change_to_autonomous_; rclcpp::Client::SharedPtr client_change_to_stop_; rclcpp::Client::SharedPtr client_change_to_local_; @@ -118,6 +124,13 @@ public Q_SLOTS: // NOLINT for Qt rclcpp::Client::SharedPtr client_enable_autoware_control_; rclcpp::Client::SharedPtr client_enable_direct_control_; + //// Auto Parking Mode + QLabel * park_mode_label_ptr_{nullptr}; + QPushButton * p_enable_button_ptr_{nullptr}; + rclcpp::Client::SharedPtr client_auto_park_; + bool auto_park_running_; + void onAutoParkStatus(const std_msgs::msg::Bool::ConstSharedPtr msg); + //// Functions void onOperationMode(const OperationModeState::ConstSharedPtr msg); void changeOperationMode(const rclcpp::Client::SharedPtr client); diff --git a/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.xml index 98315919b540a..088b10aecfc4f 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.xml @@ -31,8 +31,28 @@ - + + + + + + + + + + + + + + + + + + + + + From 60f8ebfa3973a713c5ff5789cc4438136620bb4d Mon Sep 17 00:00:00 2001 From: Aswin Vijay Date: Tue, 9 Apr 2024 14:52:07 +0900 Subject: [PATCH 5/8] :add readme and schema --- planning/auto_parking/README.md | 110 ++++++++++++++++++ .../schema/auto_parking.schema.json | 54 +++++++++ 2 files changed, 164 insertions(+) create mode 100644 planning/auto_parking/README.md create mode 100644 planning/auto_parking/schema/auto_parking.schema.json diff --git a/planning/auto_parking/README.md b/planning/auto_parking/README.md new file mode 100644 index 0000000000000..0387c002ba31c --- /dev/null +++ b/planning/auto_parking/README.md @@ -0,0 +1,110 @@ +# auto_parking + +## auto_parking_node + +`auto_parking_node` is a node that enables automated valet parking by publishing appropriate goal poses towards an empty parking space in the nearest parking lot. + +The auto parking is done in the following two phases. + +Phase 1: +Depending on initial pose when the auto parking feature is engaged, a goal pose is set at the exit of the nearest parking lot. This pose is found by first getting nearest parking lot, the parking spaces inside and lanelets inside the parking lot from the lanelet map. By ordering the lanelets based on routing distance from current position, the exit lanelet is found. Ideally the auto_parking feature should be engaged from outside parking lot so the ordering is proper. The ego vehicle then drives to parking lot exit lanelets, passing through the parking lot using either LANEDRIVING or PARKING scenario based on start pose. + +Phase 2: +Once inside the parking lot, while driving to exit lanelet goal, the node searches for a free parking space using the astar algorithm. Once a free space is found a new goal pose is published and trajectory is generated by the freespace_planner as scenario is PARKING. + +### Input topics + +| Name | Type | Description | +| -------------------------------- | --------------------------------------- | --------------------------------------| +| `~/input/engage` | autoware_auto_vehicle_msgs::msg::Engage | status of autoware AUTONOMOUS | +| `~/input/lanelet_map_bin` | autoware_auto_mapping_msgs::HADMapBin | vector map, for drivable areas | +| `~/input/occupancy_grid` | nav_msgs::msg::OccupancyGrid | occupancy grid from costmap generator | +| `~input/odometry` | nav_msgs::Odometry | for getting current pose | + +### Output topics + +| Name | Type | Description | +| ------------------------ | --------------------------------------- | ---------------------------------------------- | +| `~/output/fixed_goal` | geometry_msgs::msg::PoseStamped | current goal published by auto_parking | +| `~/output/active_status` | std_msgs::msg::Bool | active status of auto_park node | + +### Service + +| Name | Type | Description | +| ------------------------ | --------------------------------------- | --------------------------------- | +| `~/service/set_active` | std_srvs::srv::SetBool | set active status of node | + +### Client + +| Name | Type | Description | +| -------------------- | --------------------------------------- | ---------------------------------------------- | +| `~/service/engage` | tier4_external_api_msgs::srv::Engage | client to engage AUTONOMOUS mode | + +### How to launch + +1. Write your remapping info in `auto-parking.launch.xml` or add args when executing `roslaunch` +2. Launch node using `ros2 launch auto-parking auto-parking.launch.xml` +3. Call service to start auto parking after placing ego on map `ros2 service call /planning/auto_parking/set_status std_srvs/srv/SetBool "{data: True}"` +3. To stop/reset call with False arg `ros2 service call /planning/auto_parking/set_status std_srvs/srv/SetBool "{data: False}"` + +### Parameters + +{{ json_to_markdown("schema/auto_parking.schema.json") }} + + +### Flowchart + +```plantuml +@startuml +title onTimer +start + +if (active?) then (yes) +else (no) + stop +endif + +if (all input data are ready?) then (yes) +else (no) + :set active false; + stop +endif + +:get current pose; + +if (parking lot goal not set and set AutoParking init?) then (yes): + :publish parking lot goal; +else (no) + stop +endif + +if (parking lot goal set and Arrived?) then (yes): + :set active false; + stop +endif + +if (parking space goal not set and inside ParkingLot?) then (yes): + :findParkingSapce; + :publish parking space goal; +endif + +if (parking space goal set and inside ParkingLot?) then (yes): + :check if parking space goal still valid; + if (not valid) then (yes): + :publish parking lot goal; + :continue search; + endif +endif + +if (parking space goal set and Arrived?) then (yes): + stop +endif + +if (autonomous not engaged?) then (yes): + :engage autonomous; +endif + +stop +@enduml +``` + diff --git a/planning/auto_parking/schema/auto_parking.schema.json b/planning/auto_parking/schema/auto_parking.schema.json new file mode 100644 index 0000000000000..020ebc58adfcd --- /dev/null +++ b/planning/auto_parking/schema/auto_parking.schema.json @@ -0,0 +1,54 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Auto Parking Node", + "type": "object", + "definitions": { + "auto_parking": { + "type": "object", + "properties": { + "update_rate": { + "type": "number", + "description": "timer's update rate", + "default": "5.0", + "minimum": 0.0 + }, + "th_arrived_distance_m": { + "type": "number", + "description": "threshold distance to check if vehicle has arrived at the trajectory's endpoint", + "default": "1.0", + "minimum": 0.0 + }, + "th_parking_space_distance_m": { + "type": "number", + "description": "threshold radius within which to search parking spaces", + "default": "10.0", + "minimum": 0.0 + }, + "vehicle_shape_margin_m": { + "type": "number", + "description": "vehcile margin for astar search", + "default": "0.2", + "minimum": 0.0 + } + }, + "required": [ + "update_rate", + "th_arrived_distance_m", + "th_parking_space_distance_m", + "vehicle_shape_margin_m" + ] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/auto_parking" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} From 799466fbd820716b1c866acb5f196c23a8111660 Mon Sep 17 00:00:00 2001 From: Aswin Vijay Date: Wed, 10 Apr 2024 14:51:41 +0900 Subject: [PATCH 6/8] timer optimizations --- .../src/auto_parking/auto_parking_node.cpp | 76 +++++++++---------- 1 file changed, 35 insertions(+), 41 deletions(-) diff --git a/planning/auto_parking/src/auto_parking/auto_parking_node.cpp b/planning/auto_parking/src/auto_parking/auto_parking_node.cpp index d526b1c330b7a..6ca21d99837a9 100644 --- a/planning/auto_parking/src/auto_parking/auto_parking_node.cpp +++ b/planning/auto_parking/src/auto_parking/auto_parking_node.cpp @@ -325,6 +325,7 @@ bool AutoParkingNode::findParkingSpace() RCLCPP_INFO(get_logger(), "Auto-parking: Found free parking space!"); return true; } + continue; } } @@ -370,17 +371,13 @@ void AutoParkingNode::onTimer() return; } - // Check all inputs are ready - if (!odom_ || !lanelet_map_ptr_ || !routing_graph_ptr_ || - !engage_sub_ || !client_engage_) { - active_ = false; - return; - } - current_pose_.pose = odom_->pose.pose; current_pose_.header = odom_->header; - if (current_pose_.header.frame_id == "") { + // Check all inputs are ready + if (!odom_ || !lanelet_map_ptr_ || !routing_graph_ptr_ || + !engage_sub_ || !client_engage_ || + current_pose_.header.frame_id == "") { active_ = false; return; } @@ -408,40 +405,37 @@ void AutoParkingNode::onTimer() return; } - // Search parking spaces if inside parking lot - if(!set_parking_space_goal_ && - isInParkingLot() && - occupancy_grid_) - { - RCLCPP_INFO(get_logger(), "Auto-parking: Searching..."); - if(findParkingSpace()){ - goalPublisher(current_goal_); - RCLCPP_INFO(get_logger(), "Auto-parking: Publishing parking space goal"); - set_parking_space_goal_ = true; - } - } - - // Check if astar goal is still valid - // else replan - if(set_parking_space_goal_ && - isInParkingLot() && - occupancy_grid_) + if(isInParkingLot() && occupancy_grid_) { - // Set occupancy map and current pose - algo_->setMap(*occupancy_grid_); - const auto current_pose_in_costmap_frame = transformPose( - current_pose_.pose, - getTransform(occupancy_grid_->header.frame_id, current_pose_.header.frame_id)); - const auto goal_pose_in_costmap_frame = transformPose( - current_goal_.pose, - getTransform(occupancy_grid_->header.frame_id, current_goal_.header.frame_id)); - bool result = algo_->makePlan(current_pose_in_costmap_frame, goal_pose_in_costmap_frame); - if(!result) { - set_parking_space_goal_ = false; - current_goal_.pose = parking_goal_; - goalPublisher(current_goal_); - set_parking_lot_goal_ = true; - return; + // Search parking spaces if inside parking lot + if(!set_parking_space_goal_) + { + RCLCPP_INFO_THROTTLE(get_logger(), *this->get_clock(), 2000, "Auto-parking: Searching..."); + if(findParkingSpace()){ + goalPublisher(current_goal_); + RCLCPP_INFO(get_logger(), "Auto-parking: Publishing parking space goal"); + set_parking_space_goal_ = true; + } + } + // if parking space goal set + // check if astar goal is still valid, else replan + else + { + // Set occupancy map and current pose + algo_->setMap(*occupancy_grid_); + const auto current_pose_in_costmap_frame = transformPose( + current_pose_.pose, + getTransform(occupancy_grid_->header.frame_id, current_pose_.header.frame_id)); + const auto goal_pose_in_costmap_frame = transformPose( + current_goal_.pose, + getTransform(occupancy_grid_->header.frame_id, current_goal_.header.frame_id)); + bool result = algo_->makePlan(current_pose_in_costmap_frame, goal_pose_in_costmap_frame); + if(!result) { + set_parking_space_goal_ = false; + current_goal_.pose = parking_goal_; + goalPublisher(current_goal_); + return; + } } } From a2defdd7e2327c0b4f4fcc23482e0048f77de0dc Mon Sep 17 00:00:00 2001 From: Aswin Vijay Date: Wed, 10 Apr 2024 16:46:24 +0900 Subject: [PATCH 7/8] fix copyright name --- .../auto_parking/include/auto_parking/auto_parking_node.hpp | 2 +- planning/auto_parking/src/auto_parking/auto_parking_node.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/planning/auto_parking/include/auto_parking/auto_parking_node.hpp b/planning/auto_parking/include/auto_parking/auto_parking_node.hpp index ed7d0c7242f1e..953b4321efc06 100644 --- a/planning/auto_parking/include/auto_parking/auto_parking_node.hpp +++ b/planning/auto_parking/include/auto_parking/auto_parking_node.hpp @@ -1,4 +1,4 @@ -// Copyright 2024 Tier IV, Inc. +// Copyright 2024 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/planning/auto_parking/src/auto_parking/auto_parking_node.cpp b/planning/auto_parking/src/auto_parking/auto_parking_node.cpp index 6ca21d99837a9..a1c39f69dc9c9 100644 --- a/planning/auto_parking/src/auto_parking/auto_parking_node.cpp +++ b/planning/auto_parking/src/auto_parking/auto_parking_node.cpp @@ -1,4 +1,4 @@ -// Copyright 2024 Tier IV, Inc. +// Copyright 2024 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. From 7042549af8f4b405179ab7d1e94b6e87462f7f7e Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 10 Apr 2024 07:49:39 +0000 Subject: [PATCH 8/8] style(pre-commit): autofix --- common/tier4_state_rviz_plugin/package.xml | 2 +- .../src/autoware_state_panel.cpp | 12 +- .../src/autoware_state_panel.hpp | 4 +- .../scenario_planning/parking.launch.xml | 32 +-- planning/auto_parking/README.md | 36 ++-- .../config/auto_parking.param.yaml | 2 +- .../auto_parking/auto_parking_node.hpp | 49 +++-- .../launch/auto_parking.launch.xml | 3 +- planning/auto_parking/package.xml | 16 +- .../src/auto_parking/auto_parking_node.cpp | 201 +++++++++--------- 10 files changed, 177 insertions(+), 180 deletions(-) diff --git a/common/tier4_state_rviz_plugin/package.xml b/common/tier4_state_rviz_plugin/package.xml index 556e1a635fa35..c8bd0e96c1730 100644 --- a/common/tier4_state_rviz_plugin/package.xml +++ b/common/tier4_state_rviz_plugin/package.xml @@ -20,10 +20,10 @@ qtbase5-dev rclcpp rviz_common + std_srvs tier4_control_msgs tier4_external_api_msgs tier4_planning_msgs - std_srvs ament_lint_auto autoware_lint_common diff --git a/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp b/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp index daa516eb29f2d..ab81cdb6eadcb 100644 --- a/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp +++ b/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp @@ -264,7 +264,7 @@ void AutowareStatePanel::onInitialize() client_enable_direct_control_ = raw_node_->create_client("/api/operation_mode/disable_autoware_control"); - client_auto_park_ = + client_auto_park_ = raw_node_->create_client("/planning/auto_parking/set_status"); // Routing @@ -618,8 +618,8 @@ void AutowareStatePanel::onClickAutonomous() void AutowareStatePanel::onClickStop() { callServiceWithoutResponse(client_change_to_stop_); - if(auto_park_running_){ - onClickDisengageAutoPark(); + if (auto_park_running_) { + onClickDisengageAutoPark(); } } void AutowareStatePanel::onClickLocal() @@ -644,8 +644,7 @@ void AutowareStatePanel::onClickEngageAutoPark() return; } client_auto_park_->async_send_request( - req, - [this]([[maybe_unused]] rclcpp::Client::SharedFuture result) {}); + req, [this]([[maybe_unused]] rclcpp::Client::SharedFuture result) {}); } void AutowareStatePanel::onClickDisengageAutoPark() { @@ -657,8 +656,7 @@ void AutowareStatePanel::onClickDisengageAutoPark() return; } client_auto_park_->async_send_request( - req, - [this]([[maybe_unused]] rclcpp::Client::SharedFuture result) {}); + req, [this]([[maybe_unused]] rclcpp::Client::SharedFuture result) {}); } void AutowareStatePanel::onClickDirectControl() { diff --git a/common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp b/common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp index 246a7328c8905..5f28a39c1cd64 100644 --- a/common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp +++ b/common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp @@ -25,8 +25,6 @@ #include #include #include -#include -#include #include #include @@ -38,6 +36,8 @@ #include #include #include +#include +#include #include #include #include diff --git a/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.xml index 088b10aecfc4f..df656f0ac5d2a 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.xml @@ -38,21 +38,21 @@ - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + diff --git a/planning/auto_parking/README.md b/planning/auto_parking/README.md index 0387c002ba31c..50a73fad4fd25 100644 --- a/planning/auto_parking/README.md +++ b/planning/auto_parking/README.md @@ -14,44 +14,43 @@ Once inside the parking lot, while driving to exit lanelet goal, the node search ### Input topics -| Name | Type | Description | -| -------------------------------- | --------------------------------------- | --------------------------------------| -| `~/input/engage` | autoware_auto_vehicle_msgs::msg::Engage | status of autoware AUTONOMOUS | -| `~/input/lanelet_map_bin` | autoware_auto_mapping_msgs::HADMapBin | vector map, for drivable areas | -| `~/input/occupancy_grid` | nav_msgs::msg::OccupancyGrid | occupancy grid from costmap generator | -| `~input/odometry` | nav_msgs::Odometry | for getting current pose | +| Name | Type | Description | +| ------------------------- | --------------------------------------- | ------------------------------------- | +| `~/input/engage` | autoware_auto_vehicle_msgs::msg::Engage | status of autoware AUTONOMOUS | +| `~/input/lanelet_map_bin` | autoware_auto_mapping_msgs::HADMapBin | vector map, for drivable areas | +| `~/input/occupancy_grid` | nav_msgs::msg::OccupancyGrid | occupancy grid from costmap generator | +| `~input/odometry` | nav_msgs::Odometry | for getting current pose | ### Output topics -| Name | Type | Description | -| ------------------------ | --------------------------------------- | ---------------------------------------------- | -| `~/output/fixed_goal` | geometry_msgs::msg::PoseStamped | current goal published by auto_parking | -| `~/output/active_status` | std_msgs::msg::Bool | active status of auto_park node | +| Name | Type | Description | +| ------------------------ | ------------------------------- | -------------------------------------- | +| `~/output/fixed_goal` | geometry_msgs::msg::PoseStamped | current goal published by auto_parking | +| `~/output/active_status` | std_msgs::msg::Bool | active status of auto_park node | ### Service -| Name | Type | Description | -| ------------------------ | --------------------------------------- | --------------------------------- | -| `~/service/set_active` | std_srvs::srv::SetBool | set active status of node | +| Name | Type | Description | +| ---------------------- | ---------------------- | ------------------------- | +| `~/service/set_active` | std_srvs::srv::SetBool | set active status of node | ### Client -| Name | Type | Description | -| -------------------- | --------------------------------------- | ---------------------------------------------- | -| `~/service/engage` | tier4_external_api_msgs::srv::Engage | client to engage AUTONOMOUS mode | +| Name | Type | Description | +| ------------------ | ------------------------------------ | -------------------------------- | +| `~/service/engage` | tier4_external_api_msgs::srv::Engage | client to engage AUTONOMOUS mode | ### How to launch 1. Write your remapping info in `auto-parking.launch.xml` or add args when executing `roslaunch` 2. Launch node using `ros2 launch auto-parking auto-parking.launch.xml` 3. Call service to start auto parking after placing ego on map `ros2 service call /planning/auto_parking/set_status std_srvs/srv/SetBool "{data: True}"` -3. To stop/reset call with False arg `ros2 service call /planning/auto_parking/set_status std_srvs/srv/SetBool "{data: False}"` +4. To stop/reset call with False arg `ros2 service call /planning/auto_parking/set_status std_srvs/srv/SetBool "{data: False}"` ### Parameters {{ json_to_markdown("schema/auto_parking.schema.json") }} - ### Flowchart ```plantuml @@ -107,4 +106,3 @@ endif stop @enduml ``` - diff --git a/planning/auto_parking/config/auto_parking.param.yaml b/planning/auto_parking/config/auto_parking.param.yaml index 6d7812f4c3518..c2487b84ea2ac 100644 --- a/planning/auto_parking/config/auto_parking.param.yaml +++ b/planning/auto_parking/config/auto_parking.param.yaml @@ -28,4 +28,4 @@ use_back: true use_curve_weight: false use_complete_astar: true - distance_heuristic_weight: 1.0 \ No newline at end of file + distance_heuristic_weight: 1.0 diff --git a/planning/auto_parking/include/auto_parking/auto_parking_node.hpp b/planning/auto_parking/include/auto_parking/auto_parking_node.hpp index 953b4321efc06..3ef929d3b6c99 100644 --- a/planning/auto_parking/include/auto_parking/auto_parking_node.hpp +++ b/planning/auto_parking/include/auto_parking/auto_parking_node.hpp @@ -31,28 +31,27 @@ #ifndef AUTO_PARKING__AUTO_PARKING_NODE_HPP_ #define AUTO_PARKING__AUTO_PARKING_NODE_HPP_ -#include -#include -#include -#include -#include -#include #include "tier4_autoware_utils/ros/logger_level_configure.hpp" #include +#include +#include + +#include +#include +#include +#include #include -#include -#include +#include +#include +#include +#include #include #include #include - -#include -#include - -#include -#include +#include +#include using Pose = geometry_msgs::msg::Pose; using PoseStamped = geometry_msgs::msg::PoseStamped; @@ -73,10 +72,11 @@ namespace auto_parking struct AutoParkParam { - double th_arrived_distance_m; // threshold to check arrival at goal 1.0 - double th_parking_space_distance_m; // search for parking spaces within this radius 10 - double update_rate; // Timer() update rate 2 - double vehicle_shape_margin_m; // margin to add to vehicle dimensions for astar collision check, collision margin 0.2 + double th_arrived_distance_m; // threshold to check arrival at goal 1.0 + double th_parking_space_distance_m; // search for parking spaces within this radius 10 + double update_rate; // Timer() update rate 2 + double vehicle_shape_margin_m; // margin to add to vehicle dimensions for astar collision check, + // collision margin 0.2 }; class AutoParkingNode : public rclcpp::Node @@ -90,17 +90,17 @@ class AutoParkingNode : public rclcpp::Node void goalPublisher(const PoseStamped msg); void reset(); void onTimer(); - void filterGoalPoseinParkingLot(const lanelet::ConstLineString3d center_line, Pose& goal); + void filterGoalPoseinParkingLot(const lanelet::ConstLineString3d center_line, Pose & goal); bool findParkingSpace(); - + bool isInParkingLot(); bool initAutoParking(); - bool isArrived(const Pose& goal); + bool isArrived(const Pose & goal); void onEngage(EngageMsg::ConstSharedPtr msg); void engageAutonomous(); void onSetActiveStatus( - const std_srvs::srv::SetBool::Request::SharedPtr req, + const std_srvs::srv::SetBool::Request::SharedPtr req, std_srvs::srv::SetBool::Response::SharedPtr res); // functions used in the fpa constructor @@ -114,7 +114,7 @@ class AutoParkingNode : public rclcpp::Node rclcpp::Publisher::SharedPtr goal_pose_pub_; rclcpp::Publisher::SharedPtr active_status_pub_; - + rclcpp::Subscription::SharedPtr odom_sub_; rclcpp::Subscription::SharedPtr lanelet_map_sub_; rclcpp::Subscription::SharedPtr engage_sub_; @@ -152,7 +152,6 @@ class AutoParkingNode : public rclcpp::Node bool set_parking_space_goal_; bool active_; bool is_engaged_; - }; } // namespace auto_parking -#endif // AUTO_PARKING__AUTO_PARKING_NODE_HPP_ \ No newline at end of file +#endif // AUTO_PARKING__AUTO_PARKING_NODE_HPP_ diff --git a/planning/auto_parking/launch/auto_parking.launch.xml b/planning/auto_parking/launch/auto_parking.launch.xml index 27e772ce231be..6a1f266e58234 100644 --- a/planning/auto_parking/launch/auto_parking.launch.xml +++ b/planning/auto_parking/launch/auto_parking.launch.xml @@ -9,7 +9,6 @@ - @@ -27,4 +26,4 @@ - \ No newline at end of file + diff --git a/planning/auto_parking/package.xml b/planning/auto_parking/package.xml index c73a6f269ff17..01d5bdb2b746d 100644 --- a/planning/auto_parking/package.xml +++ b/planning/auto_parking/package.xml @@ -10,19 +10,19 @@ ament_cmake_auto autoware_cmake + autoware_auto_vehicle_msgs + freespace_planning_algorithms geometry_msgs - rclcpp - rclcpp_components - tier4_autoware_utils lanelet2_extension motion_utils - tier4_external_api_msgs - autoware_auto_vehicle_msgs - freespace_planning_algorithms nav_msgs - tf2_ros - tf2 + rclcpp + rclcpp_components std_srvs + tf2 + tf2_ros + tier4_autoware_utils + tier4_external_api_msgs ament_lint_auto autoware_lint_common diff --git a/planning/auto_parking/src/auto_parking/auto_parking_node.cpp b/planning/auto_parking/src/auto_parking/auto_parking_node.cpp index a1c39f69dc9c9..d1dcb23bcdc66 100644 --- a/planning/auto_parking/src/auto_parking/auto_parking_node.cpp +++ b/planning/auto_parking/src/auto_parking/auto_parking_node.cpp @@ -32,6 +32,7 @@ #include #include +#include #include #include @@ -39,8 +40,6 @@ #include #include -#include - #ifdef ROS_DISTRO_GALACTIC #include #else @@ -52,7 +51,7 @@ using lanelet::utils::to2D; using Pose = geometry_msgs::msg::Pose; using PoseStamped = geometry_msgs::msg::PoseStamped; -namespace +namespace { // get nearest parking lot by distance std::shared_ptr filterNearestParkinglot( @@ -61,12 +60,13 @@ std::shared_ptr filterNearestParkinglot( { const auto all_parking_lots = lanelet::utils::query::getAllParkingLots(lanelet_map_ptr); const auto linked_parking_lot = std::make_shared(); - *linked_parking_lot = *std::min_element(all_parking_lots.begin(), all_parking_lots.end(), - [&](const lanelet::ConstPolygon3d& p1, const lanelet::ConstPolygon3d& p2) { - const double dist_p1 = boost::geometry::distance(current_position, to2D(p1).basicPolygon()); - const double dist_p2 = boost::geometry::distance(current_position, to2D(p2).basicPolygon()); - return dist_p1 < dist_p2; - }); + *linked_parking_lot = *std::min_element( + all_parking_lots.begin(), all_parking_lots.end(), + [&](const lanelet::ConstPolygon3d & p1, const lanelet::ConstPolygon3d & p2) { + const double dist_p1 = boost::geometry::distance(current_position, to2D(p1).basicPolygon()); + const double dist_p2 = boost::geometry::distance(current_position, to2D(p2).basicPolygon()); + return dist_p1 < dist_p2; + }); if (linked_parking_lot) { return linked_parking_lot; } else { @@ -92,8 +92,7 @@ void AutoParkingNode::goalPublisher(const PoseStamped msg) goal_pose_pub_->publish(msg); } -TransformStamped AutoParkingNode::getTransform( - const std::string & from, const std::string & to) +TransformStamped AutoParkingNode::getTransform(const std::string & from, const std::string & to) { TransformStamped tf; try { @@ -105,8 +104,7 @@ TransformStamped AutoParkingNode::getTransform( return tf; } -void AutoParkingNode::onMap( - const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg) +void AutoParkingNode::onMap(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg) { lanelet_map_ptr_ = std::make_shared(); lanelet::utils::conversion::fromBinMsg( @@ -132,7 +130,7 @@ void AutoParkingNode::onSetActiveStatus( const std_srvs::srv::SetBool::Request::SharedPtr req, std_srvs::srv::SetBool::Response::SharedPtr res) { - if(req->data) { + if (req->data) { reset(); active_ = true; } else { @@ -167,26 +165,29 @@ PlannerCommonParam AutoParkingNode::getPlannerCommonParam() return p; } -bool AutoParkingNode::isArrived(const Pose& goal) +bool AutoParkingNode::isArrived(const Pose & goal) { // Check distance to current goal - if (node_param_.th_arrived_distance_m < tier4_autoware_utils::calcDistance2d(current_pose_.pose, goal)) { + if ( + node_param_.th_arrived_distance_m < + tier4_autoware_utils::calcDistance2d(current_pose_.pose, goal)) { return false; } return true; } -void AutoParkingNode::filterGoalPoseinParkingLot(const lanelet::ConstLineString3d center_line, - Pose& goal) +void AutoParkingNode::filterGoalPoseinParkingLot( + const lanelet::ConstLineString3d center_line, Pose & goal) { - for(size_t i = 0; i < center_line.size(); i++){ - const lanelet::Point3d search_point(lanelet::InvalId, center_line[i].x(), center_line[i].y(), 0.0); - if(lanelet::geometry::within(search_point, nearest_parking_lot_->basicPolygon())){ + for (size_t i = 0; i < center_line.size(); i++) { + const lanelet::Point3d search_point( + lanelet::InvalId, center_line[i].x(), center_line[i].y(), 0.0); + if (lanelet::geometry::within(search_point, nearest_parking_lot_->basicPolygon())) { goal.position.x = center_line[i].x(); goal.position.y = center_line[i].y(); goal.position.z = 0.0; const auto yaw = std::atan2( - center_line[i+1].y() - center_line[i].y(), center_line[i+1].x() - center_line[i].x()); + center_line[i + 1].y() - center_line[i].y(), center_line[i + 1].x() - center_line[i].x()); tf2::Quaternion q; q.setRPY(0, 0, yaw); goal.orientation = tf2::toMsg(q); @@ -199,7 +200,7 @@ bool AutoParkingNode::isInParkingLot() { const auto & p = current_pose_.pose.position; const lanelet::Point3d search_point(lanelet::InvalId, p.x, p.y, p.z); - if(lanelet::geometry::within(search_point, nearest_parking_lot_->basicPolygon())){ + if (lanelet::geometry::within(search_point, nearest_parking_lot_->basicPolygon())) { return true; } return false; @@ -210,48 +211,49 @@ bool AutoParkingNode::initAutoParking() const auto & p = current_pose_.pose.position; const lanelet::Point3d search_point(lanelet::InvalId, p.x, p.y, p.z); - nearest_parking_lot_ = - filterNearestParkinglot(lanelet_map_ptr_, search_point.basicPoint2d()); - if(!nearest_parking_lot_){ + nearest_parking_lot_ = filterNearestParkinglot(lanelet_map_ptr_, search_point.basicPoint2d()); + if (!nearest_parking_lot_) { RCLCPP_INFO(get_logger(), "No parking lot found!!"); return false; } - const auto & all_parking_spaces_ = - lanelet::utils::query::getAllParkingSpaces(lanelet_map_ptr_); + const auto & all_parking_spaces_ = lanelet::utils::query::getAllParkingSpaces(lanelet_map_ptr_); - nearest_parking_spaces_ = + nearest_parking_spaces_ = lanelet::utils::query::getLinkedParkingSpaces(*nearest_parking_lot_, all_parking_spaces_); const auto & all_lanelets = lanelet::utils::query::laneletLayer(lanelet_map_ptr_); const auto & all_road_lanelets = lanelet::utils::query::roadLanelets(all_lanelets); - parking_lot_lanelets_ = + parking_lot_lanelets_ = lanelet::utils::query::getLinkedLanelets(*nearest_parking_lot_, all_road_lanelets); // Get closest lanelet to ego-vehicle - if(!lanelet::utils::query::getClosestLanelet(all_road_lanelets, - current_pose_.pose, ¤t_lanelet_)){ + if (!lanelet::utils::query::getClosestLanelet( + all_road_lanelets, current_pose_.pose, ¤t_lanelet_)) { RCLCPP_INFO(get_logger(), "No close lanelet to vehicle found!!"); return false; } - // Order parking lot lanelets by route length + // Order parking lot lanelets by route length // - Sort lanelets - std::sort(parking_lot_lanelets_.begin(), parking_lot_lanelets_.end(), - [&](const lanelet::ConstLanelet& l1, const lanelet::ConstLanelet& l2){ - const auto l1_length = routing_graph_ptr_->shortestPath(current_lanelet_, l1, {}, false)->size(); - const auto l2_length = routing_graph_ptr_->shortestPath(current_lanelet_, l2, {}, false)->size(); - return ( l1_length < l2_length); - }); + std::sort( + parking_lot_lanelets_.begin(), parking_lot_lanelets_.end(), + [&](const lanelet::ConstLanelet & l1, const lanelet::ConstLanelet & l2) { + const auto l1_length = + routing_graph_ptr_->shortestPath(current_lanelet_, l1, {}, false)->size(); + const auto l2_length = + routing_graph_ptr_->shortestPath(current_lanelet_, l2, {}, false)->size(); + return (l1_length < l2_length); + }); // Set parking lot goal, goal at exit of parking lot // Assuming parking lot has one entry, longest route is from // entry to exit. - lanelet::ConstLineString3d exit_llt_cline = - parking_lot_lanelets_.back().centerline(); // final lanelet from sorted lanelets + lanelet::ConstLineString3d exit_llt_cline = + parking_lot_lanelets_.back().centerline(); // final lanelet from sorted lanelets filterGoalPoseinParkingLot(exit_llt_cline, parking_goal_); - + // print out sortet lanelet ID's for DEBUG. // for (const auto & lanelet : parking_lot_lanelets_) { // // check if parking space is close to lanelet @@ -269,43 +271,45 @@ bool AutoParkingNode::findParkingSpace() // Set occupancy map and current pose algo_->setMap(*occupancy_grid_); const auto current_pose_in_costmap_frame = transformPose( - current_pose_.pose, - getTransform(occupancy_grid_->header.frame_id, current_pose_.header.frame_id)); + current_pose_.pose, + getTransform(occupancy_grid_->header.frame_id, current_pose_.header.frame_id)); // Cycle through parking spaces - for(const auto & parking_space : nearest_parking_spaces_){ - const double dist = boost::geometry::distance(to2D(parking_space).basicLineString(), search_point); - // Check if parking space is nearby - if(node_param_.th_parking_space_distance_m < dist){ + for (const auto & parking_space : nearest_parking_spaces_) { + const double dist = + boost::geometry::distance(to2D(parking_space).basicLineString(), search_point); + // Check if parking space is nearby + if (node_param_.th_parking_space_distance_m < dist) { continue; } // Compute two goal poses for each parking space - double distance_thresh = boost::geometry::distance(parking_space.front().basicPoint(), - parking_space.back().basicPoint()) / 4; + double distance_thresh = + boost::geometry::distance( + parking_space.front().basicPoint(), parking_space.back().basicPoint()) / + 4; lanelet::ConstPoints3d p_space_fw_bk; p_space_fw_bk.push_back(parking_space.back()); p_space_fw_bk.push_back(parking_space.front()); - // Compute parking space poses, orientations i = 0 , 1 + // Compute parking space poses, orientations i = 0 , 1 // Check if freespace trajectory available then set parking space goal // Check back to front pose first then opposite - for(auto i = 0; i < 2; i++){ - + for (auto i = 0; i < 2; i++) { auto in1 = i % 2; auto in2 = (i + 1) % 2; - Eigen::Vector3d direction = - p_space_fw_bk[in2].basicPoint() - p_space_fw_bk[in1].basicPoint(); + Eigen::Vector3d direction = p_space_fw_bk[in2].basicPoint() - p_space_fw_bk[in1].basicPoint(); direction.normalize(); - // Set pose - const Eigen::Vector3d goal_pose = p_space_fw_bk[in1].basicPoint() + direction * distance_thresh; + // Set pose + const Eigen::Vector3d goal_pose = + p_space_fw_bk[in1].basicPoint() + direction * distance_thresh; const auto yaw = std::atan2(direction.y(), direction.x()); tf2::Quaternion q; q.setRPY(0, 0, yaw); - + Pose goal; goal.position.x = goal_pose.x(); goal.position.y = goal_pose.y(); @@ -317,11 +321,11 @@ bool AutoParkingNode::findParkingSpace() current_goal_.header.stamp = rclcpp::Time(); const auto goal_pose_in_costmap_frame = transformPose( - current_goal_.pose, - getTransform(occupancy_grid_->header.frame_id, current_goal_.header.frame_id)); + current_goal_.pose, + getTransform(occupancy_grid_->header.frame_id, current_goal_.header.frame_id)); bool result = algo_->makePlan(current_pose_in_costmap_frame, goal_pose_in_costmap_frame); - if(result){ + if (result) { RCLCPP_INFO(get_logger(), "Auto-parking: Found free parking space!"); return true; } @@ -367,7 +371,7 @@ void AutoParkingNode::onTimer() is_active_msg.data = active_; active_status_pub_->publish(is_active_msg); - if (!active_){ + if (!active_) { return; } @@ -375,20 +379,20 @@ void AutoParkingNode::onTimer() current_pose_.header = odom_->header; // Check all inputs are ready - if (!odom_ || !lanelet_map_ptr_ || !routing_graph_ptr_ || - !engage_sub_ || !client_engage_ || - current_pose_.header.frame_id == "") { + if ( + !odom_ || !lanelet_map_ptr_ || !routing_graph_ptr_ || !engage_sub_ || !client_engage_ || + current_pose_.header.frame_id == "") { active_ = false; return; } // Publish parking lot entrance goal - if(!set_parking_lot_goal_){ + if (!set_parking_lot_goal_) { // Initialize variables - if(!initAutoParking()){ + if (!initAutoParking()) { RCLCPP_INFO(get_logger(), "Auto-parking: Initialization failed!"); active_ = false; - return; + return; } current_goal_.header.frame_id = odom_->header.frame_id; current_goal_.header.stamp = rclcpp::Time(); @@ -399,19 +403,17 @@ void AutoParkingNode::onTimer() } // Arrived at parking lot exit without finding parking space - if(set_parking_lot_goal_ && isArrived(parking_goal_)){ + if (set_parking_lot_goal_ && isArrived(parking_goal_)) { RCLCPP_INFO(get_logger(), "Auto-parking: Failed to find parking space"); active_ = false; return; } - - if(isInParkingLot() && occupancy_grid_) - { + + if (isInParkingLot() && occupancy_grid_) { // Search parking spaces if inside parking lot - if(!set_parking_space_goal_) - { + if (!set_parking_space_goal_) { RCLCPP_INFO_THROTTLE(get_logger(), *this->get_clock(), 2000, "Auto-parking: Searching..."); - if(findParkingSpace()){ + if (findParkingSpace()) { goalPublisher(current_goal_); RCLCPP_INFO(get_logger(), "Auto-parking: Publishing parking space goal"); set_parking_space_goal_ = true; @@ -419,18 +421,17 @@ void AutoParkingNode::onTimer() } // if parking space goal set // check if astar goal is still valid, else replan - else - { + else { // Set occupancy map and current pose algo_->setMap(*occupancy_grid_); const auto current_pose_in_costmap_frame = transformPose( - current_pose_.pose, - getTransform(occupancy_grid_->header.frame_id, current_pose_.header.frame_id)); + current_pose_.pose, + getTransform(occupancy_grid_->header.frame_id, current_pose_.header.frame_id)); const auto goal_pose_in_costmap_frame = transformPose( - current_goal_.pose, - getTransform(occupancy_grid_->header.frame_id, current_goal_.header.frame_id)); + current_goal_.pose, + getTransform(occupancy_grid_->header.frame_id, current_goal_.header.frame_id)); bool result = algo_->makePlan(current_pose_in_costmap_frame, goal_pose_in_costmap_frame); - if(!result) { + if (!result) { set_parking_space_goal_ = false; current_goal_.pose = parking_goal_; goalPublisher(current_goal_); @@ -440,7 +441,7 @@ void AutoParkingNode::onTimer() } // Arrived at parking space goal - if(set_parking_space_goal_ && isArrived(current_goal_.pose)){ + if (set_parking_space_goal_ && isArrived(current_goal_.pose)) { RCLCPP_INFO(get_logger(), "Auto-parking: Complete!"); active_ = false; return; @@ -450,9 +451,9 @@ void AutoParkingNode::onTimer() // const auto is_vehicle_stopped = stop_checker_->isVehicleStopped(1.0); // Engage autonomous once a goal is set - // For smooth transition remove vehicle stopping after + // For smooth transition remove vehicle stopping after // publishing goal pose in freespace planner node - if(!is_engaged_ && !isArrived(current_goal_.pose)){ + if (!is_engaged_ && !isArrived(current_goal_.pose)) { engageAutonomous(); } } @@ -460,7 +461,6 @@ void AutoParkingNode::onTimer() AutoParkingNode::AutoParkingNode(const rclcpp::NodeOptions & node_options) : Node("auto_parking", node_options) { - // Auto-park params { auto & p = node_param_; @@ -475,26 +475,29 @@ AutoParkingNode::AutoParkingNode(const rclcpp::NodeOptions & node_options) const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); vehicle_shape_.length = vehicle_info.vehicle_length_m + node_param_.vehicle_shape_margin_m; vehicle_shape_.width = vehicle_info.vehicle_width_m + node_param_.vehicle_shape_margin_m; - vehicle_shape_.base2back = vehicle_info.rear_overhang_m + node_param_.vehicle_shape_margin_m / 2; + vehicle_shape_.base2back = + vehicle_info.rear_overhang_m + node_param_.vehicle_shape_margin_m / 2; } // Subscribers { lanelet_map_sub_ = this->create_subscription( - "~/input/lanelet_map_bin", rclcpp::QoS{10}.transient_local(), - std::bind(&AutoParkingNode::onMap, this, std::placeholders::_1)); + "~/input/lanelet_map_bin", rclcpp::QoS{10}.transient_local(), + std::bind(&AutoParkingNode::onMap, this, std::placeholders::_1)); odom_sub_ = this->create_subscription( - "~/input/odometry", rclcpp::QoS{100}, - std::bind(&AutoParkingNode::onOdometry, this, std::placeholders::_1)); + "~/input/odometry", rclcpp::QoS{100}, + std::bind(&AutoParkingNode::onOdometry, this, std::placeholders::_1)); engage_sub_ = create_subscription( - "~/input/engage", rclcpp::QoS{5}, std::bind(&AutoParkingNode::onEngage, this, std::placeholders::_1)); + "~/input/engage", rclcpp::QoS{5}, + std::bind(&AutoParkingNode::onEngage, this, std::placeholders::_1)); occupancy_grid_sub_ = create_subscription( - "~/input/occupancy_grid", 10, std::bind(&AutoParkingNode::onOccupancyGrid, this, std::placeholders::_1)); + "~/input/occupancy_grid", 10, + std::bind(&AutoParkingNode::onOccupancyGrid, this, std::placeholders::_1)); } - + // Publishers { rclcpp::QoS qos{1}; @@ -506,11 +509,11 @@ AutoParkingNode::AutoParkingNode(const rclcpp::NodeOptions & node_options) // Service { srv_set_active_ = create_service( - "~/service/set_active", - std::bind( - &AutoParkingNode::onSetActiveStatus, this, std::placeholders::_1, std::placeholders::_2)); + "~/service/set_active", + std::bind( + &AutoParkingNode::onSetActiveStatus, this, std::placeholders::_1, std::placeholders::_2)); } - + // Client { client_engage_ = this->create_client("~/service/engage"); @@ -539,7 +542,7 @@ AutoParkingNode::AutoParkingNode(const rclcpp::NodeOptions & node_options) stop_checker_ = std::make_unique(this, 1.0 + 1.0); logger_configure_ = std::make_unique(this); } -} +} // namespace auto_parking #include RCLCPP_COMPONENTS_REGISTER_NODE(auto_parking::AutoParkingNode)