feat(mission_planner): add route updater (#3334)

autowarefoundation · Apr 10, 2023 · fbe8870 · fbe8870
1 parent 90ac960
commit fbe8870
Show file tree

Hide file tree

Showing 4 changed files with 17 additions and 0 deletions.
diff --git a/planning/mission_planner/include/mission_planner/mission_planner_plugin.hpp b/planning/mission_planner/include/mission_planner/mission_planner_plugin.hpp
@@ -41,6 +41,8 @@ class PlannerPlugin
   virtual bool ready() const = 0;
   virtual LaneletRoute plan(const RoutePoints & points) = 0;
   virtual MarkerArray visualize(const LaneletRoute & route) const = 0;
+  virtual void updateRoute(const LaneletRoute & route) = 0;
+  virtual void clearRoute() = 0;
 };
 
 }  // namespace mission_planner

diff --git a/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp b/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp
@@ -412,6 +412,7 @@ PlannerPlugin::LaneletRoute DefaultPlanner::plan(const RoutePoints & points)
     const auto local_route_sections = route_handler_.createMapSegments(path_lanelets);
     route_sections = combine_consecutive_route_sections(route_sections, local_route_sections);
   }
+  route_handler_.setRouteLanelets(all_route_lanelets);
 
   auto goal_pose = points.back();
   if (param_.enable_correct_goal_pose) {
@@ -452,6 +453,16 @@ geometry_msgs::msg::Pose DefaultPlanner::refine_goal_height(
   return refined_goal;
 }
 
+void DefaultPlanner::updateRoute(const PlannerPlugin::LaneletRoute & route)
+{
+  route_handler_.setRoute(route);
+}
+
+void DefaultPlanner::clearRoute()
+{
+  route_handler_.clearRoute();
+}
+
 }  // namespace mission_planner::lanelet2
 
 #include <pluginlib/class_list_macros.hpp>

diff --git a/planning/mission_planner/src/lanelet2_plugins/default_planner.hpp b/planning/mission_planner/src/lanelet2_plugins/default_planner.hpp
@@ -76,6 +76,8 @@ class DefaultPlanner : public mission_planner::PlannerPlugin
     const double search_margin = 2.0);
   bool is_goal_valid(const geometry_msgs::msg::Pose & goal, lanelet::ConstLanelets path_lanelets);
   Pose refine_goal_height(const Pose & goal, const RouteSections & route_sections);
+  void updateRoute(const PlannerPlugin::LaneletRoute & route);
+  void clearRoute();
 };
 
 }  // namespace mission_planner::lanelet2

diff --git a/planning/mission_planner/src/mission_planner/mission_planner.cpp b/planning/mission_planner/src/mission_planner/mission_planner.cpp
@@ -121,6 +121,7 @@ PoseStamped MissionPlanner::transform_pose(const PoseStamped & input)
 void MissionPlanner::change_route()
 {
   arrival_checker_.set_goal();
+  planner_->clearRoute();
   // TODO(Takagi, Isamu): publish an empty route here
 }
 
@@ -134,6 +135,7 @@ void MissionPlanner::change_route(const LaneletRoute & route)
   arrival_checker_.set_goal(goal);
   pub_route_->publish(route);
   pub_marker_->publish(planner_->visualize(route));
+  planner_->updateRoute(route);
 }
 
 void MissionPlanner::change_state(RouteState::Message::_state_type state)