feat(mission_planner): add route updater

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

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>

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

planning/mission_planner/src/mission_planner/mission_planner.cpp

@@ -134,6 +134,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)