fix(mission_planner/behavior_path_planner): use transient local for m…

…odified goal (#5012) fix(mission_planner/behavior_path_planner): use trasient local for modified goal Signed-off-by: kosuke55 <[email protected]>
autowarefoundation · Sep 19, 2023 · 7080d7e · 7080d7e
1 parent cca5fee
commit 7080d7e
Show file tree

Hide file tree

Showing 3 changed files with 11 additions and 7 deletions.
diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp
@@ -66,7 +66,9 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
   turn_signal_publisher_ =
     create_publisher<TurnIndicatorsCommand>("~/output/turn_indicators_cmd", 1);
   hazard_signal_publisher_ = create_publisher<HazardLightsCommand>("~/output/hazard_lights_cmd", 1);
-  modified_goal_publisher_ = create_publisher<PoseWithUuidStamped>("~/output/modified_goal", 1);
+  const auto durable_qos = rclcpp::QoS(1).transient_local();
+  modified_goal_publisher_ =
+    create_publisher<PoseWithUuidStamped>("~/output/modified_goal", durable_qos);
   stop_reason_publisher_ = create_publisher<StopReasonArray>("~/output/stop_reasons", 1);
   reroute_availability_publisher_ =
     create_publisher<RerouteAvailability>("~/output/is_reroute_available", 1);

diff --git a/planning/mission_planner/src/mission_planner/mission_planner.cpp b/planning/mission_planner/src/mission_planner/mission_planner.cpp
@@ -99,12 +99,14 @@ MissionPlanner::MissionPlanner(const rclcpp::NodeOptions & options)
     rclcpp::QoS(1),
     std::bind(&MissionPlanner::on_reroute_availability, this, std::placeholders::_1));
 
-  auto qos_transient_local = rclcpp::QoS{1}.transient_local();
+  const auto durable_qos = rclcpp::QoS(1).transient_local();
   sub_vector_map_ = create_subscription<HADMapBin>(
-    "input/vector_map", qos_transient_local,
+    "input/vector_map", durable_qos,
     std::bind(&MissionPlanner::on_map, this, std::placeholders::_1));
+  sub_modified_goal_ = create_subscription<PoseWithUuidStamped>(
+    "input/modified_goal", durable_qos,
+    std::bind(&MissionPlanner::on_modified_goal, this, std::placeholders::_1));
 
-  const auto durable_qos = rclcpp::QoS(1).transient_local();
   pub_marker_ = create_publisher<MarkerArray>("debug/route_marker", durable_qos);
 
   const auto adaptor = component_interface_utils::NodeAdaptor(this);
@@ -119,7 +121,6 @@ MissionPlanner::MissionPlanner(const rclcpp::NodeOptions & options)
   adaptor.init_srv(srv_change_route_points_, this, &MissionPlanner::on_change_route_points);
   adaptor.init_srv(srv_set_mrm_route_, this, &MissionPlanner::on_set_mrm_route);
   adaptor.init_srv(srv_clear_mrm_route_, this, &MissionPlanner::on_clear_mrm_route);
-  adaptor.init_sub(sub_modified_goal_, this, &MissionPlanner::on_modified_goal);
 
   change_state(RouteState::Message::UNSET);
 }

diff --git a/planning/mission_planner/src/mission_planner/mission_planner.hpp b/planning/mission_planner/src/mission_planner/mission_planner.hpp
@@ -75,12 +75,15 @@ class MissionPlanner : public rclcpp::Node
   rclcpp::Subscription<Odometry>::SharedPtr sub_odometry_;
   rclcpp::Subscription<HADMapBin>::SharedPtr sub_vector_map_;
   rclcpp::Subscription<RerouteAvailability>::SharedPtr sub_reroute_availability_;
+  rclcpp::Subscription<PoseWithUuidStamped>::SharedPtr sub_modified_goal_;
+
   Odometry::ConstSharedPtr odometry_;
   HADMapBin::ConstSharedPtr map_ptr_;
   RerouteAvailability::ConstSharedPtr reroute_availability_;
   void on_odometry(const Odometry::ConstSharedPtr msg);
   void on_map(const HADMapBin::ConstSharedPtr msg);
   void on_reroute_availability(const RerouteAvailability::ConstSharedPtr msg);
+  void on_modified_goal(const PoseWithUuidStamped::ConstSharedPtr msg);
 
   rclcpp::Publisher<MarkerArray>::SharedPtr pub_marker_;
   void clear_route();
@@ -128,8 +131,6 @@ class MissionPlanner : public rclcpp::Node
     const ClearMrmRoute::Service::Request::SharedPtr req,
     const ClearMrmRoute::Service::Response::SharedPtr res);
 
-  component_interface_utils::Subscription<ModifiedGoal>::SharedPtr sub_modified_goal_;
-  void on_modified_goal(const ModifiedGoal::Message::ConstSharedPtr msg);
   void on_change_route(
     const SetRoute::Service::Request::SharedPtr req,
     const SetRoute::Service::Response::SharedPtr res);