Implemented looped route in mission planning

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

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

@@ -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<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>
 
@@ -32,6 +34,7 @@
 #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
 {
@@ -44,12 +47,18 @@ class MissionPlanner : public rclcpp::Node
 
   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;
@@ -59,15 +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 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

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

@@ -34,6 +34,8 @@ MissionPlanner::MissionPlanner(
 
   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>(
@@ -47,72 +49,138 @@ MissionPlanner::MissionPlanner(
     create_publisher<visualization_msgs::msg::MarkerArray>("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<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::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<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);
 }
 
 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,6 +16,7 @@
   <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>

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<geometry_msgs::msg::PoseStamped> & 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<size_t> 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 <rclcpp_components/register_node_macro.hpp>