feat(mission_planner): support looped route

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"),
                 ],

planning/mission_planner/include/mission_planner/lanelet2_impl/mission_planner_lanelet2.hpp

@@ -57,11 +57,16 @@ class MissionPlannerLanelet2 : public MissionPlanner
   rclcpp::Subscription<autoware_auto_mapping_msgs::msg::HADMapBin>::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;
-  autoware_auto_planning_msgs::msg::HADMapRoute planRoute();
+  autoware_auto_planning_msgs::msg::HADMapRoute planRoute(
+    const std::vector<geometry_msgs::msg::PoseStamped> & pass_points, const bool is_looped_route);
+  boost::optional<size_t> 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

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 <boost/optional.hpp>
+
 #include <string>
 #include <vector>
 
@@ -27,9 +29,12 @@
 #include <tf2_ros/transform_listener.h>
 
 // Autoware
+#include <tier4_autoware_utils/ros/self_pose_listener.hpp>
+
 #include <autoware_auto_planning_msgs/msg/had_map_route.hpp>
 #include <geometry_msgs/msg/pose_stamped.hpp>
 #include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
+#include <std_msgs/msg/bool.hpp>
 
 namespace mission_planner
 {
@@ -38,17 +43,23 @@ 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<geometry_msgs::msg::PoseStamped> checkpoints_;
+  tier4_autoware_utils::SelfPoseListener self_pose_listener_{this};
 
-  std::string base_link_frame_;
   std::string map_frame_;
 
+  boost::optional<geometry_msgs::msg::PoseStamped> goal_pose_;
+  std::vector<geometry_msgs::msg::PoseStamped> checkpoints_;
+
   rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::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<geometry_msgs::msg::PoseStamped> & pass_points,
+    const bool is_looped_route = false) = 0;
+  virtual boost::optional<size_t> 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;
   virtual void publishRoute(const autoware_auto_planning_msgs::msg::HADMapRoute & route) const;
@@ -57,16 +68,24 @@ class MissionPlanner : public rclcpp::Node
   rclcpp::Publisher<autoware_auto_planning_msgs::msg::HADMapRoute>::SharedPtr route_publisher_;
   rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr goal_subscriber_;
   rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr checkpoint_subscriber_;
+  rclcpp::Subscription<std_msgs::msg::Bool>::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<size_t> loop_idx_;
+
+  rclcpp::TimerBase::SharedPtr timer_;
+
+  boost::optional<geometry_msgs::msg::PoseStamped> transformPose(
+    const geometry_msgs::msg::PoseStamped & input_pose, const std::string & target_frame);
 
-  bool getEgoVehiclePose(geometry_msgs::msg::PoseStamped * ego_vehicle_pose);
+  void run();
   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);
+  void loopCallback(const std_msgs::msg::Bool::ConstSharedPtr msg);
 };
 
 }  // namespace mission_planner

planning/mission_planner/launch/mission_planner.launch.xml

@@ -1,6 +1,7 @@
 <launch>
   <arg name="goal_topic_name" default="/planning/mission_planning/goal"/>
   <arg name="checkpoint_topic_name" default="/planning/mission_planning/checkpoint"/>
+  <arg name="loop_topic_name" default="/planning/mission_planning/loop"/>
   <arg name="rout_topic_name" default="/planning/mission_planning/route"/>
   <arg name="map_topic_name" default="/map/vector_map"/>
   <arg name="visualization_topic_name" default="/planning/mission_planning/route_marker"/>
@@ -11,6 +12,7 @@
     <remap from="input/vector_map" to="$(var map_topic_name)"/>
     <remap from="input/goal_pose" to="$(var goal_topic_name)"/>
     <remap from="input/checkpoint" to="$(var checkpoint_topic_name)"/>
+    <remap from="input/loop" to="$(var loop_topic_name)"/>
     <remap from="output/route" to="$(var rout_topic_name)"/>
     <remap from="debug/route_marker" to="$(var visualization_topic_name)"/>
   </node>

planning/mission_planner/lib/mission_planner_base.cpp

@@ -31,10 +31,11 @@ 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;
 
+  loop_subscriber_ = create_subscription<std_msgs::msg::Bool>(
+    "input/loop", 10, std::bind(&MissionPlanner::loopCallback, this, _1));
   goal_subscriber_ = create_subscription<geometry_msgs::msg::PoseStamped>(
     "input/goal_pose", 10, std::bind(&MissionPlanner::goalPoseCallback, this, _1));
   checkpoint_subscriber_ = create_subscription<geometry_msgs::msg::PoseStamped>(
@@ -46,91 +47,142 @@ MissionPlanner::MissionPlanner(
     create_publisher<autoware_auto_planning_msgs::msg::HADMapRoute>("output/route", durable_qos);
   marker_publisher_ =
     create_publisher<visualization_msgs::msg::MarkerArray>("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();
+
+  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<geometry_msgs::msg::PoseStamped> 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::goalPoseCallback(
-  const geometry_msgs::msg::PoseStamped::ConstSharedPtr goal_msg_ptr)
+void MissionPlanner::run()
 {
-  // set start pose
-  if (!getEgoVehiclePose(&start_pose_)) {
-    RCLCPP_ERROR(
-      get_logger(), "Failed to get ego vehicle pose in map frame. Aborting mission planning");
+  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)
+{
+  is_looped_route_ = false;
+
+  const auto start_pose = *self_pose_listener_.getCurrentPose();
+
   // set 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.");
 
-  RCLCPP_INFO(get_logger(), "New goal pose is set. Reset checkpoints.");
-  checkpoints_.clear();
-  checkpoints_.push_back(start_pose_);
-  checkpoints_.push_back(goal_pose_);
+  std::vector<geometry_msgs::msg::PoseStamped> 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);
-}  // namespace mission_planner
+}
 
 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<geometry_msgs::msg::PoseStamped> 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<geometry_msgs::msg::PoseStamped> 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

planning/mission_planner/package.xml

@@ -16,8 +16,10 @@
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
   <depend>route_handler</depend>
+  <depend>std_msgs</depend>
   <depend>tf2_geometry_msgs</depend>
   <depend>tf2_ros</depend>
+  <depend>tier4_autoware_utils</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>autoware_lint_common</test_depend>