feat(mission_planner): add reroute interface for mrm (autowarefoundat…

…ion#3299) * feat(mission_planner): add reroute interface for mrm Signed-off-by: Takagi, Isamu <[email protected]> * feat: fix readme Signed-off-by: Takagi, Isamu <[email protected]> * feat: rename mrm interface Signed-off-by: Takagi, Isamu <[email protected]> * feat: rename mrm interface Signed-off-by: Takagi, Isamu <[email protected]> * feat: change route service type Signed-off-by: Takagi, Isamu <[email protected]> --------- Signed-off-by: Takagi, Isamu <[email protected]>
brkay54 · Apr 18, 2023 · cab563a · cab563a
1 parent 218a340
commit cab563a
Show file tree

Hide file tree

Showing 5 changed files with 53 additions and 6 deletions.
diff --git a/planning/mission_planner/README.md b/planning/mission_planner/README.md
@@ -32,6 +32,8 @@ In current Autoware.universe, only Lanelet2 map format is supported.
 | `/planning/mission_planning/set_route`           | autoware_adapi_v1_msgs/srv/SetRoute       | route request with lanelet waypoints        |
 | `/planning/mission_planning/change_route_points` | autoware_adapi_v1_msgs/srv/SetRoutePoints | route change request with pose waypoints    |
 | `/planning/mission_planning/change_route`        | autoware_adapi_v1_msgs/srv/SetRoute       | route change request with lanelet waypoints |
+| `~/srv/set_mrm_route`                            | autoware_adapi_v1_msgs/srv/SetRoutePoints | set emergency route                         |
+| `~/srv/clear_mrm_route`                          | std_srvs/srv/Trigger                      | clear emergency route                       |
 
 ### Subscriptions
 
@@ -42,12 +44,12 @@ In current Autoware.universe, only Lanelet2 map format is supported.
 
 ### Publications
 
-| Name                            | Type                                  | Description              |
-| ------------------------------- | ------------------------------------- | ------------------------ |
-| `/planning/routing/route_state` | autoware_adapi_v1_msgs/msg/RouteState | route state              |
-| `/planning/routing/route`       | autoware_planning_msgs/LaneletRoute   | route                    |
-| `debug/route_marker`            | visualization_msgs/msg/MarkerArray    | route marker for debug   |
-| `debug/goal_footprint`          | visualization_msgs/msg/MarkerArray    | goal footprint for debug |
+| Name                                     | Type                                  | Description              |
+| ---------------------------------------- | ------------------------------------- | ------------------------ |
+| `/planning/mission_planning/route_state` | autoware_adapi_v1_msgs/msg/RouteState | route state              |
+| `/planning/mission_planning/route`       | autoware_planning_msgs/LaneletRoute   | route                    |
+| `debug/route_marker`                     | visualization_msgs/msg/MarkerArray    | route marker for debug   |
+| `debug/goal_footprint`                   | visualization_msgs/msg/MarkerArray    | goal footprint for debug |
 
 ## Route section
 

diff --git a/planning/mission_planner/include/mission_planner/mission_planner_interface.hpp b/planning/mission_planner/include/mission_planner/mission_planner_interface.hpp
@@ -17,11 +17,25 @@
 
 #include <rclcpp/qos.hpp>
 
+#include <autoware_adapi_v1_msgs/srv/set_route_points.hpp>
 #include <autoware_planning_msgs/msg/pose_with_uuid_stamped.hpp>
+#include <std_srvs/srv/trigger.hpp>
 
 namespace mission_planner
 {
 
+struct SetMrmRoute
+{
+  using Service = autoware_adapi_v1_msgs::srv::SetRoutePoints;
+  static constexpr char name[] = "~/srv/set_mrm_route";
+};
+
+struct ClearMrmRoute
+{
+  using Service = std_srvs::srv::Trigger;
+  static constexpr char name[] = "~/srv/clear_mrm_route";
+};
+
 struct ModifiedGoal
 {
   using Message = autoware_planning_msgs::msg::PoseWithUuidStamped;

diff --git a/planning/mission_planner/package.xml b/planning/mission_planner/package.xml
@@ -30,6 +30,7 @@
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
   <depend>route_handler</depend>
+  <depend>std_srvs</depend>
   <depend>tf2_geometry_msgs</depend>
   <depend>tf2_ros</depend>
   <depend>tier4_autoware_utils</depend>

diff --git a/planning/mission_planner/src/mission_planner/mission_planner.cpp b/planning/mission_planner/src/mission_planner/mission_planner.cpp
@@ -90,6 +90,8 @@ MissionPlanner::MissionPlanner(const rclcpp::NodeOptions & options)
   adaptor.init_srv(srv_set_route_points_, this, &MissionPlanner::on_set_route_points);
   adaptor.init_srv(srv_change_route_, this, &MissionPlanner::on_change_route);
   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);
@@ -249,6 +251,25 @@ void MissionPlanner::on_set_route_points(
 }
 
 // NOTE: The route interface should be mutually exclusive by callback group.
+void MissionPlanner::on_set_mrm_route(
+  const SetMrmRoute::Service::Request::SharedPtr req,
+  const SetMrmRoute::Service::Response::SharedPtr res)
+{
+  // TODO(Yutaka Shimizu): reroute for MRM
+  (void)req;
+  (void)res;
+}
+
+// NOTE: The route interface should be mutually exclusive by callback group.
+void MissionPlanner::on_clear_mrm_route(
+  const ClearMrmRoute::Service::Request::SharedPtr req,
+  const ClearMrmRoute::Service::Response::SharedPtr res)
+{
+  // TODO(Yutaka Shimizu): reroute for MRM
+  (void)req;
+  (void)res;
+}
+
 void MissionPlanner::on_modified_goal(const ModifiedGoal::Message::ConstSharedPtr msg)
 {
   // TODO(Yutaka Shimizu): reroute if the goal is outside the lane.

diff --git a/planning/mission_planner/src/mission_planner/mission_planner.hpp b/planning/mission_planner/src/mission_planner/mission_planner.hpp
@@ -93,6 +93,15 @@ class MissionPlanner : public rclcpp::Node
     const SetRoutePoints::Service::Request::SharedPtr req,
     const SetRoutePoints::Service::Response::SharedPtr res);
 
+  component_interface_utils::Service<SetMrmRoute>::SharedPtr srv_set_mrm_route_;
+  component_interface_utils::Service<ClearMrmRoute>::SharedPtr srv_clear_mrm_route_;
+  void on_set_mrm_route(
+    const SetMrmRoute::Service::Request::SharedPtr req,
+    const SetMrmRoute::Service::Response::SharedPtr res);
+  void on_clear_mrm_route(
+    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(