feat(mission_planner): add the interface to reroute while driving

common/component_interface_specs/include/component_interface_specs/planning.hpp

@@ -39,6 +39,18 @@ struct SetRoute
   static constexpr char name[] = "/planning/mission_planning/set_route";
 };
 
+struct ChangeRoutePoints
+{
+  using Service = autoware_adapi_v1_msgs::srv::SetRoutePoints;
+  static constexpr char name[] = "/planning/mission_planning/change_route_points";
+};
+
+struct ChangeRoute
+{
+  using Service = autoware_planning_msgs::srv::SetRoute;
+  static constexpr char name[] = "/planning/mission_planning/change_route";
+};
+
 struct ClearRoute
 {
   using Service = autoware_adapi_v1_msgs::srv::ClearRoute;

planning/mission_planner/README.md

@@ -30,13 +30,15 @@ In current Autoware.universe, only Lanelet2 map format is supported.
 | `/planning/routing/clear_route`      | autoware_adapi_v1_msgs::srv::ClearRoute     | route clear request               |
 | `/planning/routing/set_route_points` | autoware_adapi_v1_msgs::srv::SetRoutePoints | route request with pose waypoints |
 | `/planning/routing/set_route`        | autoware_planning_msgs::srv::SetRoute       | route request with HAD map format |
+| `~/srv/clear_mrm`                    | std_srvs::srv::Trigger                      | clear emergency operation         |
 
 ### Subscriptions
 
-| Name                  | Type                                 | Description                 |
-| --------------------- | ------------------------------------ | --------------------------- |
-| `input/vector_map`    | autoware_auto_mapping_msgs/HADMapBin | vector map of Lanelet2      |
-| `input/modified_goal` | geometry_msgs/PoseStamped            | goal pose for arrival check |
+| Name               | Type                                 | Description                                |
+| ------------------ | ------------------------------------ | ------------------------------------------ |
+| `input/vector_map` | autoware_auto_mapping_msgs/HADMapBin | vector map of Lanelet2                     |
+| `~/sub/new_goal`   | geometry_msgs/PoseWithUuidStamped    | modified goal pose                         |
+| `~/sub/mrm_goal`   | geometry_msgs/PoseWithUuidStamped    | modified goal pose for emergency operation |
 
 ### Publications
 

planning/mission_planner/include/mission_planner/mission_planner_interface.hpp

@@ -0,0 +1,52 @@
+// Copyright 2023 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef MISSION_PLANNER__MISSION_PLANNER_INTERFACE_HPP_
+#define MISSION_PLANNER__MISSION_PLANNER_INTERFACE_HPP_
+
+#include <rclcpp/qos.hpp>
+
+#include <autoware_planning_msgs/msg/pose_with_uuid_stamped.hpp>
+#include <std_srvs/srv/trigger.hpp>
+
+namespace mission_planner
+{
+
+struct NewGoal
+{
+  using Message = autoware_planning_msgs::msg::PoseWithUuidStamped;
+  static constexpr char name[] = "~/sub/new_goal";
+  static constexpr size_t depth = 1;
+  static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
+  static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_VOLATILE;
+};
+
+struct MrmGoal
+{
+  using Message = autoware_planning_msgs::msg::PoseWithUuidStamped;
+  static constexpr char name[] = "~/sub/mrm_goal";
+  static constexpr size_t depth = 1;
+  static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
+  static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_VOLATILE;
+};
+
+struct ClearMrm
+{
+  using Service = std_srvs::srv::Trigger;
+  static constexpr char name[] = "~/srv/clear_mrm";
+};
+
+}  // namespace mission_planner
+
+#endif  // MISSION_PLANNER__MISSION_PLANNER_INTERFACE_HPP_

planning/mission_planner/launch/mission_planner.launch.xml

@@ -6,7 +6,7 @@
 
   <node pkg="mission_planner" exec="mission_planner" name="mission_planner" output="screen">
     <param from="$(var mission_planner_param_path)"/>
-    <remap from="input/modified_goal" to="$(var modified_goal_topic_name)"/>
+    <remap from="~/sub/new_goal" to="$(var modified_goal_topic_name)"/>
     <remap from="input/vector_map" to="$(var map_topic_name)"/>
     <remap from="debug/route_marker" to="$(var visualization_topic_name)"/>
   </node>

planning/mission_planner/package.xml

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

planning/mission_planner/src/mission_planner/arrival_checker.cpp

@@ -27,10 +27,6 @@ ArrivalChecker::ArrivalChecker(rclcpp::Node * node) : vehicle_stop_checker_(node
   angle_ = tier4_autoware_utils::deg2rad(angle_deg);
   distance_ = node->declare_parameter<double>("arrival_check_distance");
   duration_ = node->declare_parameter<double>("arrival_check_duration");
-
-  sub_goal_ = node->create_subscription<PoseWithUuidStamped>(
-    "input/modified_goal", 1,
-    [this](const PoseWithUuidStamped::ConstSharedPtr msg) { modify_goal(*msg); });
 }
 
 void ArrivalChecker::set_goal()

planning/mission_planner/src/mission_planner/arrival_checker.hpp

@@ -33,6 +33,7 @@ class ArrivalChecker
   explicit ArrivalChecker(rclcpp::Node * node);
   void set_goal();
   void set_goal(const PoseWithUuidStamped & goal);
+  void modify_goal(const PoseWithUuidStamped & modified_goal);
   bool is_arrived(const PoseStamped & pose) const;
 
 private:
@@ -42,7 +43,6 @@ class ArrivalChecker
   std::optional<PoseWithUuidStamped> goal_with_uuid_;
   rclcpp::Subscription<PoseWithUuidStamped>::SharedPtr sub_goal_;
   motion_utils::VehicleStopChecker vehicle_stop_checker_;
-  void modify_goal(const PoseWithUuidStamped & modified_goal);
 };
 
 }  // namespace mission_planner

planning/mission_planner/src/mission_planner/mission_planner.cpp

@@ -85,6 +85,11 @@ MissionPlanner::MissionPlanner(const rclcpp::NodeOptions & options)
   adaptor.init_srv(srv_clear_route_, this, &MissionPlanner::on_clear_route);
   adaptor.init_srv(srv_set_route_, this, &MissionPlanner::on_set_route);
   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_sub(sub_new_goal_, this, &MissionPlanner::on_new_goal);
+  adaptor.init_sub(sub_mrm_goal_, this, &MissionPlanner::on_mrm_goal);
+  adaptor.init_srv(srv_clear_mrm_, this, &MissionPlanner::on_clear_mrm);
 
   change_state(RouteState::Message::UNSET);
 }
@@ -142,7 +147,7 @@ void MissionPlanner::change_state(RouteState::Message::_state_type state)
   pub_state_->publish(state_);
 }
 
-// NOTE: The route services should be mutually exclusive by callback group.
+// NOTE: The route interface should be mutually exclusive by callback group.
 void MissionPlanner::on_clear_route(
   const ClearRoute::Service::Request::SharedPtr, const ClearRoute::Service::Response::SharedPtr res)
 {
@@ -151,7 +156,7 @@ void MissionPlanner::on_clear_route(
   res->status.success = true;
 }
 
-// NOTE: The route services should be mutually exclusive by callback group.
+// NOTE: The route interface should be mutually exclusive by callback group.
 void MissionPlanner::on_set_route(
   const SetRoute::Service::Request::SharedPtr req, const SetRoute::Service::Response::SharedPtr res)
 {
@@ -188,7 +193,7 @@ void MissionPlanner::on_set_route(
   res->status.success = true;
 }
 
-// NOTE: The route services should be mutually exclusive by callback group.
+// NOTE: The route interface should be mutually exclusive by callback group.
 void MissionPlanner::on_set_route_points(
   const SetRoutePoints::Service::Request::SharedPtr req,
   const SetRoutePoints::Service::Response::SharedPtr res)
@@ -238,6 +243,48 @@ void MissionPlanner::on_set_route_points(
   res->status.success = true;
 }
 
+// NOTE: The route interface should be mutually exclusive by callback group.
+void MissionPlanner::on_change_route(
+  const SetRoute::Service::Request::SharedPtr req, const SetRoute::Service::Response::SharedPtr res)
+{
+  // TODO(Yutaka Shimizu): reroute
+  (void)req;
+  (void)res;
+}
+
+// NOTE: The route interface should be mutually exclusive by callback group.
+void MissionPlanner::on_change_route_points(
+  const SetRoutePoints::Service::Request::SharedPtr req,
+  const SetRoutePoints::Service::Response::SharedPtr res)
+{
+  // TODO(Yutaka Shimizu): reroute
+  (void)req;
+  (void)res;
+}
+
+// NOTE: The route interface should be mutually exclusive by callback group.
+void MissionPlanner::on_new_goal(const NewGoal::Message::ConstSharedPtr msg)
+{
+  // TODO(Yutaka Shimizu): reroute if the goal is outside the lane.
+  arrival_checker_.modify_goal(*msg);
+}
+
+// NOTE: The route interface should be mutually exclusive by callback group.
+void MissionPlanner::on_mrm_goal(const MrmGoal::Message::ConstSharedPtr msg)
+{
+  // TODO(Yutaka Shimizu): reroute for MRM
+  (void)msg;
+}
+
+// NOTE: The route interface should be mutually exclusive by callback group.
+void MissionPlanner::on_clear_mrm(
+  const ClearMrm::Service::Request::SharedPtr req, const ClearMrm::Service::Response::SharedPtr res)
+{
+  // TODO(Yutaka Shimizu): reroute for MRM
+  (void)req;
+  (void)res;
+}
+
 }  // namespace mission_planner
 
 #include <rclcpp_components/register_node_macro.hpp>

planning/mission_planner/src/mission_planner/mission_planner.hpp

@@ -19,6 +19,7 @@
 
 #include <component_interface_specs/planning.hpp>
 #include <component_interface_utils/rclcpp.hpp>
+#include <mission_planner/mission_planner_interface.hpp>
 #include <mission_planner/mission_planner_plugin.hpp>
 #include <pluginlib/class_loader.hpp>
 #include <rclcpp/rclcpp.hpp>
@@ -41,9 +42,11 @@ using PoseStamped = geometry_msgs::msg::PoseStamped;
 using PoseWithUuidStamped = autoware_planning_msgs::msg::PoseWithUuidStamped;
 using LaneletRoute = autoware_planning_msgs::msg::LaneletRoute;
 using MarkerArray = visualization_msgs::msg::MarkerArray;
+using ClearRoute = planning_interface::ClearRoute;
 using SetRoutePoints = planning_interface::SetRoutePoints;
 using SetRoute = planning_interface::SetRoute;
-using ClearRoute = planning_interface::ClearRoute;
+using ChangeRoutePoints = planning_interface::ChangeRoutePoints;
+using ChangeRoute = planning_interface::ChangeRoute;
 using Route = planning_interface::Route;
 using RouteState = planning_interface::RouteState;
 using Odometry = nav_msgs::msg::Odometry;
@@ -78,6 +81,8 @@ class MissionPlanner : public rclcpp::Node
   component_interface_utils::Service<ClearRoute>::SharedPtr srv_clear_route_;
   component_interface_utils::Service<SetRoute>::SharedPtr srv_set_route_;
   component_interface_utils::Service<SetRoutePoints>::SharedPtr srv_set_route_points_;
+  component_interface_utils::Service<ChangeRoute>::SharedPtr srv_change_route_;
+  component_interface_utils::Service<ChangeRoutePoints>::SharedPtr srv_change_route_points_;
   void on_clear_route(
     const ClearRoute::Service::Request::SharedPtr req,
     const ClearRoute::Service::Response::SharedPtr res);
@@ -87,6 +92,21 @@ class MissionPlanner : public rclcpp::Node
   void on_set_route_points(
     const SetRoutePoints::Service::Request::SharedPtr req,
     const SetRoutePoints::Service::Response::SharedPtr res);
+  void on_change_route(
+    const SetRoute::Service::Request::SharedPtr req,
+    const SetRoute::Service::Response::SharedPtr res);
+  void on_change_route_points(
+    const SetRoutePoints::Service::Request::SharedPtr req,
+    const SetRoutePoints::Service::Response::SharedPtr res);
+
+  component_interface_utils::Subscription<NewGoal>::SharedPtr sub_new_goal_;
+  component_interface_utils::Subscription<MrmGoal>::SharedPtr sub_mrm_goal_;
+  component_interface_utils::Service<ClearMrm>::SharedPtr srv_clear_mrm_;
+  void on_new_goal(const NewGoal::Message::ConstSharedPtr msg);
+  void on_mrm_goal(const MrmGoal::Message::ConstSharedPtr msg);
+  void on_clear_mrm(
+    const ClearMrm::Service::Request::SharedPtr req,
+    const ClearMrm::Service::Response::SharedPtr res);
 };
 
 }  // namespace mission_planner