feat(emergency_goal_manager, mrm_pull_over_manager): add mrm pull over

launch/tier4_system_launch/launch/system.launch.xml

@@ -105,6 +105,12 @@
         <arg name="config_file" value="$(var mrm_emergency_stop_operator_param_path)"/>
       </include>
     </group>
+    <group>
+      <include file="$(find-pkg-share mrm_pull_over_manager)/launch/mrm_pull_over_manager.launch.xml"/>
+    </group>
+    <group>
+      <include file="$(find-pkg-share emergency_goal_manager)/launch/emergency_goal_manager.launch.xml"/>
+    </group>
 
     <!-- MRM Handler -->
     <group if="$(var use_diagnostic_graph)">

system/emergency_goal_manager/CMakeLists.txt

@@ -0,0 +1,15 @@
+cmake_minimum_required(VERSION 3.14)
+project(emergency_goal_manager)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+ament_auto_add_executable(emergency_goal_manager
+  src/emergency_goal_manager_node.cpp
+  src/emergency_goal_manager_core.cpp
+)
+
+ament_auto_package(INSTALL_TO_SHARE
+  launch
+  config
+)

system/emergency_goal_manager/README.md

@@ -0,0 +1,33 @@
+# emergency_goal_manager
+
+## Purpose
+
+The Emergency goal manager is responsible for coordinating the goal poses for emergency rerouting and communicating it to the mission planner.
+
+## Inner-workings / Algorithms
+
+TBD.
+
+## Inputs / Outputs
+
+### Input
+
+| Name                                    | Type                                                 | Description                   |
+| --------------------------------------- | ---------------------------------------------------- | ----------------------------- |
+| `~/input/emergency_goals`               | `tier4_system_msgs::msg::EmergencyGoalsStamped`      | Candidates for emergency goal |
+| `~/input/emergency_goals_clear_command` | `tier4_system_msgs::msg::EmergencyGoalsClearCommand` | Clear command                 |
+
+### Output
+
+| Name                                                             | Type                                          | Description              |
+| ---------------------------------------------------------------- | --------------------------------------------- | ------------------------ |
+| `/planning/mission_planning/mission_planner/srv/set_mrm_route`   | `autoware_adapi_v1_msgs::srv::SetRoutePoints` | Set route points for MRM |
+| `/planning/mission_planning/mission_planner/srv/clear_mrm_route` | `autoware_adapi_v1_msgs::srv::ClearRoute`     | Clear route for MRM      |
+
+## Parameters
+
+No parameters.
+
+## Assumptions / Known limits
+
+TBD.

system/emergency_goal_manager/include/emergency_goal_manager_core.hpp

@@ -0,0 +1,67 @@
+// 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 EMERGENCY_GOAL_MANAGER_CORE_HPP_
+#define EMERGENCY_GOAL_MANAGER_CORE_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <autoware_adapi_v1_msgs/srv/clear_route.hpp>
+#include <autoware_adapi_v1_msgs/srv/set_route_points.hpp>
+#include <geometry_msgs/msg/pose.hpp>
+#include <std_srvs/srv/trigger.hpp>
+#include <tier4_system_msgs/msg/emergency_goals_clear_command.hpp>
+#include <tier4_system_msgs/msg/emergency_goals_stamped.hpp>
+
+#include <queue>
+#include <string>
+#include <unordered_map>
+
+namespace emergency_goal_manager
+{
+class EmergencyGoalManager : public rclcpp::Node
+{
+public:
+  EmergencyGoalManager();
+
+private:
+  using SetRoutePoints = autoware_adapi_v1_msgs::srv::SetRoutePoints;
+  using ClearRoute = autoware_adapi_v1_msgs::srv::ClearRoute;
+
+  // Subscriber
+  rclcpp::Subscription<tier4_system_msgs::msg::EmergencyGoalsStamped>::SharedPtr
+    sub_emergency_goals_;
+  rclcpp::Subscription<tier4_system_msgs::msg::EmergencyGoalsClearCommand>::SharedPtr
+    sub_emergency_goals_clear_command_;
+
+  void onEmergencyGoals(const tier4_system_msgs::msg::EmergencyGoalsStamped::SharedPtr msg);
+  void onEmergencyGoalsClearCommand(
+    const tier4_system_msgs::msg::EmergencyGoalsClearCommand::SharedPtr msg);
+
+  // Client
+  rclcpp::CallbackGroup::SharedPtr client_set_mrm_route_points_callback_group_;
+  rclcpp::Client<SetRoutePoints>::SharedPtr client_set_mrm_route_points_;
+  rclcpp::CallbackGroup::SharedPtr client_clear_mrm_route_callback_group_;
+  rclcpp::Client<ClearRoute>::SharedPtr client_clear_mrm_route_;
+
+  // Variables
+  std::unordered_map<std::string, std::queue<geometry_msgs::msg::Pose>> emergency_goals_map_;
+
+  // Algorithm
+  void callSetMrmRoutePoints();
+  void callClearMrmRoute();
+};
+}  // namespace emergency_goal_manager
+
+#endif  // EMERGENCY_GOAL_MANAGER_CORE_HPP_

system/emergency_goal_manager/launch/emergency_goal_manager.launch.xml

@@ -0,0 +1,9 @@
+<launch>
+  <arg name="input_emergency_goals" default="/system/mrm/pull_over_manager/emergency_goals"/>
+  <arg name="input_goals_clear_command" default="/system/mrm/pull_over_manager/goals_clear_command"/>
+
+  <node pkg="emergency_goal_manager" exec="emergency_goal_manager" name="emergency_goal_manager" output="screen">
+    <remap from="~/input/emergency_goals" to="$(var input_emergency_goals)"/>
+    <remap from="~/input/emergency_goals_clear_command" to="$(var input_goals_clear_command)"/>
+  </node>
+</launch>

system/emergency_goal_manager/package.xml

@@ -0,0 +1,26 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>emergency_goal_manager</name>
+  <version>0.1.0</version>
+  <description>The emergency goal manager package</description>
+  <maintainer email="[email protected]">Makoto Kurihara</maintainer>
+  <maintainer email="[email protected]">Tomohito Ando</maintainer>
+  <maintainer email="[email protected]">Ryuta Kambe</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <buildtool_depend>autoware_cmake</buildtool_depend>
+
+  <depend>autoware_adapi_v1_msgs</depend>
+  <depend>rclcpp_components</depend>
+  <depend>std_srvs</depend>
+  <depend>tier4_system_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

system/emergency_goal_manager/src/emergency_goal_manager_core.cpp

@@ -0,0 +1,146 @@
+// 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.
+
+#include <emergency_goal_manager_core.hpp>
+
+namespace emergency_goal_manager
+{
+EmergencyGoalManager::EmergencyGoalManager() : Node("emergency_goal_manager")
+{
+  // Subscriber
+  sub_emergency_goals_ = create_subscription<tier4_system_msgs::msg::EmergencyGoalsStamped>(
+    "~/input/emergency_goals", rclcpp::QoS{1},
+    std::bind(&EmergencyGoalManager::onEmergencyGoals, this, std::placeholders::_1));
+  sub_emergency_goals_clear_command_ =
+    create_subscription<tier4_system_msgs::msg::EmergencyGoalsClearCommand>(
+      "~/input/emergency_goals_clear_command", rclcpp::QoS{1},
+      std::bind(&EmergencyGoalManager::onEmergencyGoalsClearCommand, this, std::placeholders::_1));
+
+  // Client
+  client_set_mrm_route_points_callback_group_ =
+    create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+  client_set_mrm_route_points_ = create_client<SetRoutePoints>(
+    "/planning/mission_planning/mission_planner/srv/set_mrm_route",
+    rmw_qos_profile_services_default, client_set_mrm_route_points_callback_group_);
+  client_clear_mrm_route_callback_group_ =
+    create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+  client_clear_mrm_route_ = create_client<ClearRoute>(
+    "/planning/mission_planning/mission_planner/srv/clear_mrm_route",
+    rmw_qos_profile_services_default, client_clear_mrm_route_callback_group_);
+
+  // Initialize
+  while (!client_set_mrm_route_points_->wait_for_service(std::chrono::seconds(1)) && rclcpp::ok()) {
+  }
+  while (!client_clear_mrm_route_->wait_for_service(std::chrono::seconds(1)) && rclcpp::ok()) {
+  }
+}
+
+void EmergencyGoalManager::onEmergencyGoals(
+  const tier4_system_msgs::msg::EmergencyGoalsStamped::SharedPtr msg)
+{
+  std::queue<geometry_msgs::msg::Pose> emergency_goals_queue;
+  for (const auto & goal : msg->goals) {
+    emergency_goals_queue.push(goal);
+  }
+  emergency_goals_map_.emplace(msg->sender, emergency_goals_queue);
+
+  callSetMrmRoutePoints();
+}
+
+void EmergencyGoalManager::onEmergencyGoalsClearCommand(
+  const tier4_system_msgs::msg::EmergencyGoalsClearCommand::SharedPtr msg)
+{
+  if (emergency_goals_map_.count(msg->sender) == 0) {
+    RCLCPP_WARN(get_logger(), "Emergency goals from %s is empty.", msg->sender.c_str());
+  }
+
+  emergency_goals_map_.erase(msg->sender);
+
+  if (emergency_goals_map_.empty()) {
+    callClearMrmRoute();
+  } else {
+    callSetMrmRoutePoints();
+  }
+}
+
+void EmergencyGoalManager::callSetMrmRoutePoints()
+{
+  auto request = std::make_shared<SetRoutePoints::Request>();
+  request->header.frame_id = "map";
+  request->header.stamp = this->now();
+  request->option.allow_goal_modification = true;
+
+  while (!emergency_goals_map_.empty()) {
+    // TODO(Makoto Kurihara): set goals with the highest priority
+    auto goals = emergency_goals_map_.begin();
+
+    auto sender = goals->first;
+    auto & goal_queue = goals->second;
+    if (goal_queue.empty()) {
+      emergency_goals_map_.erase(sender);
+      continue;
+    }
+
+    request->goal = goal_queue.front();
+    goal_queue.pop();
+
+    auto future = client_set_mrm_route_points_->async_send_request(request);
+    const auto duration = std::chrono::duration<double, std::ratio<1>>(10);
+    if (future.wait_for(duration) != std::future_status::ready) {
+      RCLCPP_WARN(get_logger(), "MRM Route service timeout.");
+      continue;
+    } else {
+      if (future.get()->status.success) {
+        RCLCPP_INFO(get_logger(), "MRM Route has been successfully sent.");
+        return;
+      } else {
+        RCLCPP_WARN(get_logger(), "MRM Route service has failed.");
+        std::this_thread::sleep_for(std::chrono::seconds(1));
+        continue;
+      }
+    }
+  }
+
+  callClearMrmRoute();
+}
+
+void EmergencyGoalManager::callClearMrmRoute()
+{
+  auto request = std::make_shared<ClearRoute::Request>();
+  const auto duration = std::chrono::duration<double, std::ratio<1>>(10);
+  const auto start_time = std::chrono::steady_clock::now();
+
+  while (rclcpp::ok()) {
+    if (std::chrono::steady_clock::now() - start_time > duration) {
+      RCLCPP_WARN(get_logger(), "Clear MRM Route operation timeout.");
+      return;
+    }
+
+    auto future = client_clear_mrm_route_->async_send_request(request);
+    if (future.wait_for(duration) != std::future_status::ready) {
+      RCLCPP_WARN(get_logger(), "Clear MRM Route service timeout.");
+      return;
+    } else {
+      if (future.get()->status.success) {
+        RCLCPP_INFO(get_logger(), "Clear MRM Route has been successfully sent.");
+        return;
+      } else {
+        std::this_thread::sleep_for(std::chrono::seconds(1));
+        RCLCPP_WARN(get_logger(), "Clear MRM Route has failed.");
+        continue;
+      }
+    }
+  }
+}
+}  // namespace emergency_goal_manager

system/emergency_goal_manager/src/emergency_goal_manager_node.cpp

@@ -0,0 +1,28 @@
+// 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.
+
+#include "emergency_goal_manager_core.hpp"
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+  rclcpp::executors::MultiThreadedExecutor executor;
+  auto node = std::make_shared<emergency_goal_manager::EmergencyGoalManager>();
+  executor.add_node(node);
+  executor.spin();
+  executor.remove_node(node);
+  rclcpp::shutdown();
+
+  return 0;
+}

system/mrm_pull_over_manager/CMakeLists.txt

@@ -0,0 +1,15 @@
+cmake_minimum_required(VERSION 3.14)
+project(mrm_pull_over_manager)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+ament_auto_add_executable(mrm_pull_over_manager
+  src/mrm_pull_over_manager/mrm_pull_over_manager_node.cpp
+  src/mrm_pull_over_manager/mrm_pull_over_manager_core.cpp
+)
+
+ament_auto_package(INSTALL_TO_SHARE
+  launch
+  config
+)