From 92339ff916c7e17e513859a748ff8e334a83adcf Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Sat, 23 Apr 2022 21:43:08 +0900 Subject: [PATCH 1/3] minor refactoring of mission_planner Signed-off-by: Takayuki Murooka --- .../mission_planner_lanelet2.hpp | 2 +- .../mission_planner/mission_planner_base.hpp | 17 +++++---- .../lib/mission_planner_base.cpp | 38 +++++-------------- planning/mission_planner/package.xml | 1 + .../mission_planner_lanelet2.cpp | 28 +++++++------- 5 files changed, 35 insertions(+), 51 deletions(-) diff --git a/planning/mission_planner/include/mission_planner/lanelet2_impl/mission_planner_lanelet2.hpp b/planning/mission_planner/include/mission_planner/lanelet2_impl/mission_planner_lanelet2.hpp index a623f45e432ba..038c2c1c2f643 100644 --- a/planning/mission_planner/include/mission_planner/lanelet2_impl/mission_planner_lanelet2.hpp +++ b/planning/mission_planner/include/mission_planner/lanelet2_impl/mission_planner_lanelet2.hpp @@ -57,7 +57,7 @@ class MissionPlannerLanelet2 : public MissionPlanner rclcpp::Subscription::SharedPtr map_subscriber_; void mapCallback(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg); - bool isGoalValid() const; + bool isGoalValid(const geometry_msgs::msg::Pose & goal_pose) const; // virtual functions bool isRoutingGraphReady() const; diff --git a/planning/mission_planner/include/mission_planner/mission_planner_base.hpp b/planning/mission_planner/include/mission_planner/mission_planner_base.hpp index c7c56479f3f2f..a23c5be7995ca 100644 --- a/planning/mission_planner/include/mission_planner/mission_planner_base.hpp +++ b/planning/mission_planner/include/mission_planner/mission_planner_base.hpp @@ -27,6 +27,8 @@ #include // Autoware +#include + #include #include #include @@ -38,17 +40,17 @@ class MissionPlanner : public rclcpp::Node protected: MissionPlanner(const std::string & node_name, const rclcpp::NodeOptions & node_options); - geometry_msgs::msg::PoseStamped goal_pose_; - geometry_msgs::msg::PoseStamped start_pose_; - std::vector checkpoints_; + tier4_autoware_utils::SelfPoseListener self_pose_listener_{this}; - std::string base_link_frame_; std::string map_frame_; + std::vector checkpoints_; + rclcpp::Publisher::SharedPtr marker_publisher_; virtual bool isRoutingGraphReady() const = 0; virtual autoware_auto_planning_msgs::msg::HADMapRoute planRoute() = 0; + virtual void visualizeRoute( const autoware_auto_planning_msgs::msg::HADMapRoute & route) const = 0; virtual void publishRoute(const autoware_auto_planning_msgs::msg::HADMapRoute & route) const; @@ -61,12 +63,11 @@ class MissionPlanner : public rclcpp::Node tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; - bool getEgoVehiclePose(geometry_msgs::msg::PoseStamped * ego_vehicle_pose); - void goalPoseCallback(const geometry_msgs::msg::PoseStamped::ConstSharedPtr goal_msg_ptr); - void checkpointCallback(const geometry_msgs::msg::PoseStamped::ConstSharedPtr checkpoint_msg_ptr); bool transformPose( const geometry_msgs::msg::PoseStamped & input_pose, - geometry_msgs::msg::PoseStamped * output_pose, const std::string target_frame); + geometry_msgs::msg::PoseStamped * output_pose, const std::string & target_frame); + void goalPoseCallback(const geometry_msgs::msg::PoseStamped::ConstSharedPtr goal_msg_ptr); + void checkpointCallback(const geometry_msgs::msg::PoseStamped::ConstSharedPtr checkpoint_msg_ptr); }; } // namespace mission_planner diff --git a/planning/mission_planner/lib/mission_planner_base.cpp b/planning/mission_planner/lib/mission_planner_base.cpp index 2d29b655602a6..9f3f85e99988e 100644 --- a/planning/mission_planner/lib/mission_planner_base.cpp +++ b/planning/mission_planner/lib/mission_planner_base.cpp @@ -31,7 +31,6 @@ MissionPlanner::MissionPlanner( : Node(node_name, node_options), tf_buffer_(get_clock()), tf_listener_(tf_buffer_) { map_frame_ = declare_parameter("map_frame", "map"); - base_link_frame_ = declare_parameter("base_link_frame", "base_link"); using std::placeholders::_1; @@ -46,27 +45,13 @@ MissionPlanner::MissionPlanner( create_publisher("output/route", durable_qos); marker_publisher_ = create_publisher("debug/route_marker", durable_qos); -} -bool MissionPlanner::getEgoVehiclePose(geometry_msgs::msg::PoseStamped * ego_vehicle_pose) -{ - geometry_msgs::msg::PoseStamped base_link_origin; - base_link_origin.header.frame_id = base_link_frame_; - base_link_origin.pose.position.x = 0; - base_link_origin.pose.position.y = 0; - base_link_origin.pose.position.z = 0; - base_link_origin.pose.orientation.x = 0; - base_link_origin.pose.orientation.y = 0; - base_link_origin.pose.orientation.z = 0; - base_link_origin.pose.orientation.w = 1; - - // transform base_link frame origin to map_frame to get vehicle positions - return transformPose(base_link_origin, ego_vehicle_pose, map_frame_); + self_pose_listener_.waitForFirstPose(); } bool MissionPlanner::transformPose( const geometry_msgs::msg::PoseStamped & input_pose, geometry_msgs::msg::PoseStamped * output_pose, - const std::string target_frame) + const std::string & target_frame) { geometry_msgs::msg::TransformStamped transform; try { @@ -83,22 +68,19 @@ bool MissionPlanner::transformPose( void MissionPlanner::goalPoseCallback( const geometry_msgs::msg::PoseStamped::ConstSharedPtr goal_msg_ptr) { - // set start pose - if (!getEgoVehiclePose(&start_pose_)) { - RCLCPP_ERROR( - get_logger(), "Failed to get ego vehicle pose in map frame. Aborting mission planning"); - return; - } + const auto & start_pose = *self_pose_listener_.getCurrentPose(); + // set goal pose - if (!transformPose(*goal_msg_ptr, &goal_pose_, map_frame_)) { + geometry_msgs::msg::PoseStamped goal_pose; + if (!transformPose(*goal_msg_ptr, &goal_pose, map_frame_)) { RCLCPP_ERROR(get_logger(), "Failed to get goal pose in map frame. Aborting mission planning"); return; } - RCLCPP_INFO(get_logger(), "New goal pose is set. Reset checkpoints."); + checkpoints_.clear(); - checkpoints_.push_back(start_pose_); - checkpoints_.push_back(goal_pose_); + checkpoints_.push_back(start_pose); + checkpoints_.push_back(goal_pose); if (!isRoutingGraphReady()) { RCLCPP_ERROR(get_logger(), "RoutingGraph is not ready. Aborting mission planning"); @@ -107,7 +89,7 @@ void MissionPlanner::goalPoseCallback( autoware_auto_planning_msgs::msg::HADMapRoute route = planRoute(); publishRoute(route); -} // namespace mission_planner +} void MissionPlanner::checkpointCallback( const geometry_msgs::msg::PoseStamped::ConstSharedPtr checkpoint_msg_ptr) diff --git a/planning/mission_planner/package.xml b/planning/mission_planner/package.xml index 3aced14bbb08a..d70d3947ab52e 100644 --- a/planning/mission_planner/package.xml +++ b/planning/mission_planner/package.xml @@ -18,6 +18,7 @@ route_handler tf2_geometry_msgs tf2_ros + tier4_autoware_utils ament_lint_auto autoware_lint_common diff --git a/planning/mission_planner/src/mission_planner_lanelet2/mission_planner_lanelet2.cpp b/planning/mission_planner/src/mission_planner_lanelet2/mission_planner_lanelet2.cpp index 271b4a0344115..39978cae60a91 100644 --- a/planning/mission_planner/src/mission_planner_lanelet2/mission_planner_lanelet2.cpp +++ b/planning/mission_planner/src/mission_planner_lanelet2/mission_planner_lanelet2.cpp @@ -192,19 +192,17 @@ void MissionPlannerLanelet2::visualizeRoute( marker_publisher_->publish(route_marker_array); } -bool MissionPlannerLanelet2::isGoalValid() const +bool MissionPlannerLanelet2::isGoalValid(const geometry_msgs::msg::Pose & goal_pose) const { lanelet::Lanelet closest_lanelet; - if (!lanelet::utils::query::getClosestLanelet( - road_lanelets_, goal_pose_.pose, &closest_lanelet)) { + if (!lanelet::utils::query::getClosestLanelet(road_lanelets_, goal_pose, &closest_lanelet)) { return false; } - const auto goal_lanelet_pt = lanelet::utils::conversion::toLaneletPoint(goal_pose_.pose.position); + const auto goal_lanelet_pt = lanelet::utils::conversion::toLaneletPoint(goal_pose.position); if (isInLane(closest_lanelet, goal_lanelet_pt)) { - const auto lane_yaw = - lanelet::utils::getLaneletAngle(closest_lanelet, goal_pose_.pose.position); - const auto goal_yaw = tf2::getYaw(goal_pose_.pose.orientation); + const auto lane_yaw = lanelet::utils::getLaneletAngle(closest_lanelet, goal_pose.position); + const auto goal_yaw = tf2::getYaw(goal_pose.orientation); const auto angle_diff = normalizeRadian(lane_yaw - goal_yaw); constexpr double th_angle = M_PI / 4; @@ -229,14 +227,14 @@ bool MissionPlannerLanelet2::isGoalValid() const // check if goal is in shoulder lanelet lanelet::Lanelet closest_shoulder_lanelet; if (!lanelet::utils::query::getClosestLanelet( - shoulder_lanelets_, goal_pose_.pose, &closest_shoulder_lanelet)) { + shoulder_lanelets_, goal_pose, &closest_shoulder_lanelet)) { return false; } // check if goal pose is in shoulder lane if (isInLane(closest_shoulder_lanelet, goal_lanelet_pt)) { const auto lane_yaw = - lanelet::utils::getLaneletAngle(closest_shoulder_lanelet, goal_pose_.pose.position); - const auto goal_yaw = tf2::getYaw(goal_pose_.pose.orientation); + lanelet::utils::getLaneletAngle(closest_shoulder_lanelet, goal_pose.position); + const auto goal_yaw = tf2::getYaw(goal_pose.orientation); const auto angle_diff = normalizeRadian(lane_yaw - goal_yaw); constexpr double th_angle = M_PI / 4; @@ -250,19 +248,21 @@ bool MissionPlannerLanelet2::isGoalValid() const autoware_auto_planning_msgs::msg::HADMapRoute MissionPlannerLanelet2::planRoute() { + const geometry_msgs::msg::Pose goal_pose = checkpoints_.back().pose; + std::stringstream ss; for (const auto & checkpoint : checkpoints_) { ss << "x: " << checkpoint.pose.position.x << " " << "y: " << checkpoint.pose.position.y << std::endl; } RCLCPP_INFO_STREAM( - get_logger(), "start planning route with checkpoints: " << std::endl - << ss.str()); + get_logger(), "start planning route with checkpoints_: " << std::endl + << ss.str()); autoware_auto_planning_msgs::msg::HADMapRoute route_msg; RouteSections route_sections; - if (!isGoalValid()) { + if (!isGoalValid(goal_pose)) { RCLCPP_WARN(get_logger(), "Goal is not valid! Please check position and angle of goal_pose"); return route_msg; } @@ -289,7 +289,7 @@ autoware_auto_planning_msgs::msg::HADMapRoute MissionPlannerLanelet2::planRoute( route_msg.header.stamp = this->now(); route_msg.header.frame_id = map_frame_; route_msg.segments = route_sections; - route_msg.goal_pose = goal_pose_.pose; + route_msg.goal_pose = goal_pose; return route_msg; } From b057d02e486f481fb1bb33f23f4055ee364f88b8 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Sun, 24 Apr 2022 03:12:47 +0900 Subject: [PATCH 2/3] Implemented looped route in mission planning Signed-off-by: Takayuki Murooka --- .../mission_planning.launch.py | 1 + .../mission_planner_lanelet2.hpp | 7 +- .../mission_planner/mission_planner_base.hpp | 26 +++- .../launch/mission_planner.launch.xml | 2 + .../lib/mission_planner_base.cpp | 118 ++++++++++++++---- planning/mission_planner/package.xml | 1 + .../mission_planner_lanelet2.cpp | 72 +++++++++-- .../include/route_handler/route_handler.hpp | 6 +- planning/route_handler/src/route_handler.cpp | 7 ++ .../ad_service_state_monitor_node.cpp | 2 +- 10 files changed, 201 insertions(+), 41 deletions(-) diff --git a/launch/tier4_planning_launch/launch/mission_planning/mission_planning.launch.py b/launch/tier4_planning_launch/launch/mission_planning/mission_planning.launch.py index cc3952f93be96..77df3a4ad01f1 100644 --- a/launch/tier4_planning_launch/launch/mission_planning/mission_planning.launch.py +++ b/launch/tier4_planning_launch/launch/mission_planning/mission_planning.launch.py @@ -34,6 +34,7 @@ def generate_launch_description(): ("input/vector_map", "/map/vector_map"), ("input/goal_pose", "/planning/mission_planning/goal"), ("input/checkpoint", "/planning/mission_planning/checkpoint"), + ("input/loop", "/planning/mission_planning/loop"), ("output/route", "/planning/mission_planning/route"), ("debug/route_marker", "/planning/mission_planning/route_marker"), ], diff --git a/planning/mission_planner/include/mission_planner/lanelet2_impl/mission_planner_lanelet2.hpp b/planning/mission_planner/include/mission_planner/lanelet2_impl/mission_planner_lanelet2.hpp index 038c2c1c2f643..928c0b4643570 100644 --- a/planning/mission_planner/include/mission_planner/lanelet2_impl/mission_planner_lanelet2.hpp +++ b/planning/mission_planner/include/mission_planner/lanelet2_impl/mission_planner_lanelet2.hpp @@ -61,7 +61,12 @@ class MissionPlannerLanelet2 : public MissionPlanner // virtual functions bool isRoutingGraphReady() const; - autoware_auto_planning_msgs::msg::HADMapRoute planRoute(); + autoware_auto_planning_msgs::msg::HADMapRoute planRoute( + const std::vector & pass_points, const bool is_looped_route); + boost::optional getClosestRouteSectionIndex( + const autoware_auto_planning_msgs::msg::HADMapRoute & route, + const geometry_msgs::msg::PoseStamped & pose, geometry_msgs::msg::Pose & goal_pose); + void visualizeRoute(const autoware_auto_planning_msgs::msg::HADMapRoute & route) const; }; } // namespace mission_planner diff --git a/planning/mission_planner/include/mission_planner/mission_planner_base.hpp b/planning/mission_planner/include/mission_planner/mission_planner_base.hpp index a23c5be7995ca..34f7210904053 100644 --- a/planning/mission_planner/include/mission_planner/mission_planner_base.hpp +++ b/planning/mission_planner/include/mission_planner/mission_planner_base.hpp @@ -15,6 +15,8 @@ #ifndef MISSION_PLANNER__MISSION_PLANNER_BASE_HPP_ #define MISSION_PLANNER__MISSION_PLANNER_BASE_HPP_ +#include + #include #include @@ -32,6 +34,7 @@ #include #include #include +#include namespace mission_planner { @@ -44,12 +47,18 @@ class MissionPlanner : public rclcpp::Node std::string map_frame_; + boost::optional goal_pose_; std::vector checkpoints_; rclcpp::Publisher::SharedPtr marker_publisher_; virtual bool isRoutingGraphReady() const = 0; - virtual autoware_auto_planning_msgs::msg::HADMapRoute planRoute() = 0; + virtual autoware_auto_planning_msgs::msg::HADMapRoute planRoute( + const std::vector & pass_points, + const bool is_looped_route = false) = 0; + virtual boost::optional getClosestRouteSectionIndex( + const autoware_auto_planning_msgs::msg::HADMapRoute & route, + const geometry_msgs::msg::PoseStamped & pose, geometry_msgs::msg::Pose & goal_pose) = 0; virtual void visualizeRoute( const autoware_auto_planning_msgs::msg::HADMapRoute & route) const = 0; @@ -59,15 +68,24 @@ class MissionPlanner : public rclcpp::Node rclcpp::Publisher::SharedPtr route_publisher_; rclcpp::Subscription::SharedPtr goal_subscriber_; rclcpp::Subscription::SharedPtr checkpoint_subscriber_; + rclcpp::Subscription::SharedPtr loop_subscriber_; tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; + bool is_looped_route_{false}; + + autoware_auto_planning_msgs::msg::HADMapRoute route_; + boost::optional loop_idx_; + + rclcpp::TimerBase::SharedPtr timer_; + + boost::optional transformPose( + const geometry_msgs::msg::PoseStamped & input_pose, const std::string & target_frame); - bool transformPose( - const geometry_msgs::msg::PoseStamped & input_pose, - geometry_msgs::msg::PoseStamped * output_pose, const std::string & target_frame); + void run(); void goalPoseCallback(const geometry_msgs::msg::PoseStamped::ConstSharedPtr goal_msg_ptr); void checkpointCallback(const geometry_msgs::msg::PoseStamped::ConstSharedPtr checkpoint_msg_ptr); + void loopCallback(const std_msgs::msg::Bool::ConstSharedPtr msg); }; } // namespace mission_planner diff --git a/planning/mission_planner/launch/mission_planner.launch.xml b/planning/mission_planner/launch/mission_planner.launch.xml index 6c0461e9d1eeb..ef383bc96f352 100644 --- a/planning/mission_planner/launch/mission_planner.launch.xml +++ b/planning/mission_planner/launch/mission_planner.launch.xml @@ -1,6 +1,7 @@ + @@ -11,6 +12,7 @@ + diff --git a/planning/mission_planner/lib/mission_planner_base.cpp b/planning/mission_planner/lib/mission_planner_base.cpp index 9f3f85e99988e..ce1ebafffdacd 100644 --- a/planning/mission_planner/lib/mission_planner_base.cpp +++ b/planning/mission_planner/lib/mission_planner_base.cpp @@ -34,6 +34,8 @@ MissionPlanner::MissionPlanner( using std::placeholders::_1; + loop_subscriber_ = create_subscription( + "input/loop", 10, std::bind(&MissionPlanner::loopCallback, this, _1)); goal_subscriber_ = create_subscription( "input/goal_pose", 10, std::bind(&MissionPlanner::goalPoseCallback, this, _1)); checkpoint_subscriber_ = create_subscription( @@ -47,72 +49,138 @@ MissionPlanner::MissionPlanner( create_publisher("debug/route_marker", durable_qos); self_pose_listener_.waitForFirstPose(); + + const auto planning_hz = declare_parameter("planning_hz", 1); + const auto period_ns = rclcpp::Rate(planning_hz).period(); + timer_ = + rclcpp::create_timer(this, get_clock(), period_ns, std::bind(&MissionPlanner::run, this)); } -bool MissionPlanner::transformPose( - const geometry_msgs::msg::PoseStamped & input_pose, geometry_msgs::msg::PoseStamped * output_pose, - const std::string & target_frame) +boost::optional MissionPlanner::transformPose( + const geometry_msgs::msg::PoseStamped & input_pose, const std::string & target_frame) { + geometry_msgs::msg::PoseStamped output_pose; + geometry_msgs::msg::TransformStamped transform; try { transform = tf_buffer_.lookupTransform(target_frame, input_pose.header.frame_id, tf2::TimePointZero); - tf2::doTransform(input_pose, *output_pose, transform); - return true; + tf2::doTransform(input_pose, output_pose, transform); + return output_pose; } catch (tf2::TransformException & ex) { RCLCPP_WARN(get_logger(), "%s", ex.what()); - return false; + return {}; + } +} + +void MissionPlanner::run() +{ + if (!is_looped_route_ || route_.segments.empty()) { + return; + } + + const auto start_pose = *self_pose_listener_.getCurrentPose(); + + // publish new route if the ego's closest lanelet moves forward + geometry_msgs::msg::Pose goal_pose; + const auto final_idx = getClosestRouteSectionIndex(route_, start_pose, goal_pose); + if (!loop_idx_ || loop_idx_.get() != final_idx.get()) { + auto route = route_; + route.segments.clear(); + route.segments.insert( + route.segments.end(), route_.segments.begin() + final_idx.get(), route_.segments.end()); + route.segments.insert( + route.segments.end(), route_.segments.begin(), + route_.segments.end() - (route_.segments.size() - final_idx.get())); + route.goal_pose = goal_pose; + + publishRoute(route); + + loop_idx_ = final_idx.get(); } } +void MissionPlanner::loopCallback(const std_msgs::msg::Bool::ConstSharedPtr msg) +{ + if (msg->data) { + RCLCPP_INFO(get_logger(), "Route will be looped."); + } else { + RCLCPP_INFO(get_logger(), "Route will not be looped."); + } + is_looped_route_ = msg->data; +} + void MissionPlanner::goalPoseCallback( const geometry_msgs::msg::PoseStamped::ConstSharedPtr goal_msg_ptr) { - const auto & start_pose = *self_pose_listener_.getCurrentPose(); + const auto start_pose = *self_pose_listener_.getCurrentPose(); // set goal pose - geometry_msgs::msg::PoseStamped goal_pose; - if (!transformPose(*goal_msg_ptr, &goal_pose, map_frame_)) { + goal_pose_ = transformPose(*goal_msg_ptr, map_frame_); + if (!goal_pose_) { RCLCPP_ERROR(get_logger(), "Failed to get goal pose in map frame. Aborting mission planning"); return; } - RCLCPP_INFO(get_logger(), "New goal pose is set. Reset checkpoints."); + RCLCPP_INFO(get_logger(), "New goal pose is set."); - checkpoints_.clear(); - checkpoints_.push_back(start_pose); - checkpoints_.push_back(goal_pose); + std::vector pass_points; + pass_points.push_back(start_pose); + pass_points.push_back(goal_pose_.get()); if (!isRoutingGraphReady()) { RCLCPP_ERROR(get_logger(), "RoutingGraph is not ready. Aborting mission planning"); return; } - autoware_auto_planning_msgs::msg::HADMapRoute route = planRoute(); + autoware_auto_planning_msgs::msg::HADMapRoute route = planRoute(pass_points); publishRoute(route); } void MissionPlanner::checkpointCallback( const geometry_msgs::msg::PoseStamped::ConstSharedPtr checkpoint_msg_ptr) { - if (checkpoints_.size() < 2) { + const auto transformed_checkpoint = transformPose(*checkpoint_msg_ptr, map_frame_); + if (!transformed_checkpoint) { RCLCPP_ERROR( - get_logger(), - "You must set start and goal before setting checkpoints. Aborting mission planning"); + get_logger(), "Failed to get checkpoint pose in map frame. Aborting mission planning"); return; } - geometry_msgs::msg::PoseStamped transformed_checkpoint; - if (!transformPose(*checkpoint_msg_ptr, &transformed_checkpoint, map_frame_)) { - RCLCPP_ERROR( - get_logger(), "Failed to get checkpoint pose in map frame. Aborting mission planning"); + const auto start_pose = *self_pose_listener_.getCurrentPose(); + + if (!isRoutingGraphReady()) { + RCLCPP_ERROR(get_logger(), "RoutingGraph is not ready. Aborting mission planning"); return; } - // insert checkpoint before goal - checkpoints_.insert(checkpoints_.end() - 1, transformed_checkpoint); + if (is_looped_route_) { + checkpoints_.push_back(transformed_checkpoint.get()); - autoware_auto_planning_msgs::msg::HADMapRoute route = planRoute(); - publishRoute(route); + std::vector pass_points; + pass_points.push_back(start_pose); + pass_points.insert(pass_points.end(), checkpoints_.begin(), checkpoints_.end()); + pass_points.push_back(start_pose); + + route_ = planRoute(pass_points, true); + // route_.segments.pop_back(); + } else { + if (!goal_pose_) { + RCLCPP_ERROR( + get_logger(), + "You must set start and goal before setting checkpoints. Aborting mission planning"); + return; + } + + checkpoints_.push_back(transformed_checkpoint.get()); + + std::vector pass_points; + pass_points.push_back(start_pose); + pass_points.insert(pass_points.end(), checkpoints_.begin(), checkpoints_.end()); + pass_points.push_back(goal_pose_.get()); + + autoware_auto_planning_msgs::msg::HADMapRoute route = planRoute(pass_points); + publishRoute(route); + } } void MissionPlanner::publishRoute(const autoware_auto_planning_msgs::msg::HADMapRoute & route) const diff --git a/planning/mission_planner/package.xml b/planning/mission_planner/package.xml index d70d3947ab52e..c2fea3d5759e5 100644 --- a/planning/mission_planner/package.xml +++ b/planning/mission_planner/package.xml @@ -16,6 +16,7 @@ rclcpp rclcpp_components route_handler + std_msgs tf2_geometry_msgs tf2_ros tier4_autoware_utils diff --git a/planning/mission_planner/src/mission_planner_lanelet2/mission_planner_lanelet2.cpp b/planning/mission_planner/src/mission_planner_lanelet2/mission_planner_lanelet2.cpp index 39978cae60a91..46cde25eb9117 100644 --- a/planning/mission_planner/src/mission_planner_lanelet2/mission_planner_lanelet2.cpp +++ b/planning/mission_planner/src/mission_planner_lanelet2/mission_planner_lanelet2.cpp @@ -246,18 +246,19 @@ bool MissionPlannerLanelet2::isGoalValid(const geometry_msgs::msg::Pose & goal_p return false; } -autoware_auto_planning_msgs::msg::HADMapRoute MissionPlannerLanelet2::planRoute() +autoware_auto_planning_msgs::msg::HADMapRoute MissionPlannerLanelet2::planRoute( + const std::vector & pass_points, const bool is_looped_route) { - const geometry_msgs::msg::Pose goal_pose = checkpoints_.back().pose; + const geometry_msgs::msg::Pose goal_pose = pass_points.back().pose; std::stringstream ss; - for (const auto & checkpoint : checkpoints_) { + for (const auto & checkpoint : pass_points) { ss << "x: " << checkpoint.pose.position.x << " " << "y: " << checkpoint.pose.position.y << std::endl; } RCLCPP_INFO_STREAM( - get_logger(), "start planning route with checkpoints_: " << std::endl - << ss.str()); + get_logger(), "start planning route with pass_points: " << std::endl + << ss.str()); autoware_auto_planning_msgs::msg::HADMapRoute route_msg; RouteSections route_sections; @@ -267,9 +268,9 @@ autoware_auto_planning_msgs::msg::HADMapRoute MissionPlannerLanelet2::planRoute( return route_msg; } - for (std::size_t i = 1; i < checkpoints_.size(); i++) { - const auto start_checkpoint = checkpoints_.at(i - 1); - const auto goal_checkpoint = checkpoints_.at(i); + for (std::size_t i = 1; i < pass_points.size(); i++) { + const auto start_checkpoint = pass_points.at(i - 1); + const auto goal_checkpoint = pass_points.at(i); lanelet::ConstLanelets path_lanelets; if (!route_handler_.planPathLaneletsBetweenCheckpoints( start_checkpoint.pose, goal_checkpoint.pose, &path_lanelets)) { @@ -291,9 +292,64 @@ autoware_auto_planning_msgs::msg::HADMapRoute MissionPlannerLanelet2::planRoute( route_msg.segments = route_sections; route_msg.goal_pose = goal_pose; + if (is_looped_route) { + // ignore overlapped route where the initial ego pose locates + route_msg.segments.pop_back(); + + route_handler_.setRoute(route_msg); + } + return route_msg; } +boost::optional MissionPlannerLanelet2::getClosestRouteSectionIndex( + const autoware_auto_planning_msgs::msg::HADMapRoute & route, + const geometry_msgs::msg::PoseStamped & pose, geometry_msgs::msg::Pose & goal_pose) +{ + if (!route_handler_.isHandlerReady()) { + RCLCPP_ERROR_STREAM(rclcpp::get_logger("po"), "Route handler is not ready."); + return {}; + } + + lanelet::ConstLanelet lanelet; + if (!route_handler_.getClosestPreferredLanelet(pose.pose, &lanelet)) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("po"), "Closest preferred lanelet to ego was not found."); + return {}; + } + + // set closest index + for (size_t i = 0; i < route.segments.size(); ++i) { + if (lanelet.id() == route.segments.at(i).preferred_primitive_id) { + // set goal pose + const size_t goal_idx = (i + route.segments.size() - 3) % + route.segments.size(); // TODO(murooka) not too close to ego + lanelet::ConstLanelet goal_lanelet = + route_handler_.getLaneletsFromId(route.segments.at(goal_idx).primitives.front().id); + const Eigen::Vector2d back_pose = (goal_lanelet.rightBound().back().basicPoint2d() + + goal_lanelet.leftBound().back().basicPoint2d()) / + 2.0; + goal_pose.position.x = back_pose.x(); + goal_pose.position.y = back_pose.y(); + + const size_t size = goal_lanelet.leftBound().size(); + if (size > 1) { + const Eigen::Vector2d front_pose = (goal_lanelet.rightBound()[size - 2].basicPoint2d() + + goal_lanelet.leftBound()[size - 2].basicPoint2d()) / + 2.0; + goal_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw( + std::atan2(back_pose.y() - front_pose.y(), back_pose.x() - front_pose.x())); + } + + // return index + return i; + } + } + + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("po"), "Closest preferred lanelet was not found in route."); + return {}; +} } // namespace mission_planner #include diff --git a/planning/route_handler/include/route_handler/route_handler.hpp b/planning/route_handler/include/route_handler/route_handler.hpp index 8b6f35b86b5a9..a36364215909c 100644 --- a/planning/route_handler/include/route_handler/route_handler.hpp +++ b/planning/route_handler/include/route_handler/route_handler.hpp @@ -96,8 +96,6 @@ class RouteHandler std::vector getLanesAfterGoal(const double vehicle_length) const; // for lanelet - bool getPreviousLaneletsWithinRoute( - const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelets * prev_lanelets) const; bool isDeadEndLanelet(const lanelet::ConstLanelet & lanelet) const; lanelet::ConstLanelets getLaneletsFromPoint(const lanelet::ConstPoint3d & point) const; @@ -121,6 +119,8 @@ class RouteHandler boost::optional getLeftLanelet( const lanelet::ConstLanelet & lanelet) const; lanelet::ConstLanelets getNextLanelets(const lanelet::ConstLanelet & lanelet) const; + bool getPreviousLaneletsWithinRoute( + const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelets * prev_lanelets) const; /** * @brief Check if opposite-direction lane is available at the right side of the lanelet @@ -219,6 +219,8 @@ class RouteHandler int getNumLaneToPreferredLane(const lanelet::ConstLanelet & lanelet) const; bool getClosestLaneletWithinRoute( const Pose & search_pose, lanelet::ConstLanelet * closest_lanelet) const; + bool getClosestPreferredLanelet( + const Pose & search_pose, lanelet::ConstLanelet * closest_lanelet) const; lanelet::ConstLanelet getLaneletsFromId(const lanelet::Id id) const; lanelet::ConstLanelets getLaneletsFromIds(const lanelet::Ids ids) const; lanelet::ConstLanelets getLaneletSequence( diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp index 406e070f31c4b..ec3f96755bc36 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -747,6 +747,13 @@ bool RouteHandler::getClosestLaneletWithinRoute( return lanelet::utils::query::getClosestLanelet(route_lanelets_, search_pose, closest_lanelet); } +bool RouteHandler::getClosestPreferredLanelet( + const Pose & search_pose, lanelet::ConstLanelet * closest_lanelet) const +{ + return lanelet::utils::query::getClosestLanelet( + preferred_lanelets_, search_pose, closest_lanelet); +} + bool RouteHandler::getNextLaneletWithinRoute( const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * next_lanelet) const { diff --git a/system/ad_service_state_monitor/src/ad_service_state_monitor_node/ad_service_state_monitor_node.cpp b/system/ad_service_state_monitor/src/ad_service_state_monitor_node/ad_service_state_monitor_node.cpp index cbfb469a72fa2..507c3a3ffe982 100644 --- a/system/ad_service_state_monitor/src/ad_service_state_monitor_node/ad_service_state_monitor_node.cpp +++ b/system/ad_service_state_monitor/src/ad_service_state_monitor_node/ad_service_state_monitor_node.cpp @@ -101,7 +101,7 @@ void AutowareStateMonitorNode::onRoute( if (disengage_on_route_ && isEngaged()) { RCLCPP_INFO(this->get_logger(), "new route received and disengage Autoware"); - setDisengage(); + // setDisengage(); } } From 7c3dedc6f9f54e49f4828ded751f51c7cfc1e7a7 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Sun, 24 Apr 2022 11:37:32 +0900 Subject: [PATCH 3/3] reset looped_route when posing the goal Signed-off-by: Takayuki Murooka --- planning/mission_planner/lib/mission_planner_base.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/planning/mission_planner/lib/mission_planner_base.cpp b/planning/mission_planner/lib/mission_planner_base.cpp index ce1ebafffdacd..d09f903ca07f8 100644 --- a/planning/mission_planner/lib/mission_planner_base.cpp +++ b/planning/mission_planner/lib/mission_planner_base.cpp @@ -113,6 +113,8 @@ void MissionPlanner::loopCallback(const std_msgs::msg::Bool::ConstSharedPtr msg) void MissionPlanner::goalPoseCallback( const geometry_msgs::msg::PoseStamped::ConstSharedPtr goal_msg_ptr) { + is_looped_route_ = false; + const auto start_pose = *self_pose_listener_.getCurrentPose(); // set goal pose