wip: update: create MRM operator libraries

Signed-off-by: veqcc <ryuta.kambe@tier4.jp>

Author: veqcc

launch/tier4_system_launch/launch/system.launch.xml

@@ -83,6 +83,12 @@
       <include file="$(find-pkg-share emergency_handler)/launch/emergency_handler.launch.xml">
         <arg name="config_file" value="$(var emergency_handler_param_path)"/>
       </include>
+      <include file="$(find-pkg-share mrm_comfortable_stop_operator)/launch/mrm_comfortable_stop_operator.launch.xml">
+        <arg name="config_file" value="$(var mrm_comfortable_stop_operator_param_path)"/>
+      </include>
+      <include file="$(find-pkg-share mrm_emergency_stop_operator)/launch/mrm_emergency_stop_operator.launch.xml">
+        <arg name="config_file" value="$(var mrm_emergency_stop_operator_param_path)"/>
+      </include>
     </group>
 
     <group>

system/emergency_handler/CMakeLists.txt

@@ -7,8 +7,6 @@ autoware_package()
 ament_auto_add_executable(emergency_handler
   src/emergency_handler_node.cpp
   src/emergency_handler_core.cpp
-  src/emergency_stop_operator.cpp
-  src/comfortable_stop_operator.cpp
 )
 
 ament_auto_package(INSTALL_TO_SHARE

system/emergency_handler/config/emergency_handler.param.yaml

@@ -5,17 +5,3 @@
     update_rate: 10
     timeout_hazard_status: 0.5
     use_comfortable_stop: false
-
-    # emergency stop operator class
-    emergency_stop_operator:
-      update_rate: 30
-      target_acceleration: -2.5
-      target_jerk: -1.5
-      turning_hazard_on: true
-      use_parking_after_stopped: false
-
-    # comfortable stop operator class
-    comfortable_stop_operator:
-      min_acceleration: -1.0
-      max_jerk: 0.3
-      min_jerk: -0.3

system/emergency_handler/launch/emergency_handler.launch.xml

@@ -1,39 +1,15 @@
 <launch>
-  <!-- emergency_handler -->
   <arg name="input_hazard_status" default="/system/emergency/hazard_status"/>
   <arg name="input_odometry" default="/localization/kinematic_state"/>
   <arg name="input_control_mode" default="/vehicle/status/control_mode"/>
   <arg name="output_mrm_state" default="/system/fail_safe/mrm_state"/>
-
   <arg name="config_file" default="$(find-pkg-share emergency_handler)/config/emergency_handler.param.yaml"/>
 
-  <!-- emergency stop operator class-->
-  <arg name="input_control_command" default="/control/command/control_cmd"/>
-  <arg name="output_emergency_command" default="/system/emergency/control_cmd"/>
-  <arg name="output_hazard" default="/system/emergency/hazard_lights_cmd"/>
-  <arg name="output_gear" default="/system/emergency/gear_cmd"/>
-
-  <!-- comfortable stop operator class -->
-  <arg name="output_velocity_limit" default="/planning/scenario_planning/max_velocity_candidates"/>
-  <arg name="output_velocity_limit_clear" default="/planning/scenario_planning/clear_velocity_limit"/>
-
-  <!-- emergency_handler -->
   <node pkg="emergency_handler" exec="emergency_handler" name="emergency_handler" output="screen">
     <remap from="~/input/hazard_status" to="$(var input_hazard_status)"/>
     <remap from="~/input/odometry" to="$(var input_odometry)"/>
     <remap from="~/input/control_mode" to="$(var input_control_mode)"/>
     <remap from="~/output/mrm/state" to="$(var output_mrm_state)"/>
-
     <param from="$(var config_file)"/>
-
-    <!-- emergency stop operator class -->
-    <remap from="~/input/control/control_cmd" to="$(var input_control_command)"/>
-    <remap from="~/output/mrm/emergency_stop/control_cmd" to="$(var output_emergency_command)"/>
-    <remap from="~/output/gear" to="$(var output_gear)"/>
-    <remap from="~/output/hazard" to="$(var output_hazard)"/>
-
-    <!-- comfortable stop operator class -->
-    <remap from="~/output/velocity_limit" to="$(var output_velocity_limit)"/>
-    <remap from="~/output/velocity_limit/clear" to="$(var output_velocity_limit_clear)"/>
   </node>
 </launch>

system/emergency_handler/package.xml

@@ -19,7 +19,8 @@
   <depend>nav_msgs</depend>
   <depend>rclcpp</depend>
   <depend>std_msgs</depend>
-  <depend>tier4_planning_msgs</depend>
+  <depend>mrm_comfortable_stop_operator</depend>
+  <depend>mrm_emergency_stop_operator</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>autoware_lint_common</test_depend>

system/emergency_handler/schema/emergency_handler.schema.json

@@ -20,72 +20,12 @@
           "type": "boolean",
           "description": "If this parameter is true, operate comfortable stop when latent faults occur.",
           "default": "false"
-        },
-        "emergency_stop_operator": {
-          "type": "object",
-          "update_rate": {
-            "type": "integer",
-            "description": "Timer callback period.",
-            "default": 30
-          },
-          "properties": {
-            "target_acceleration": {
-              "type": "number",
-              "description": "Target acceleration for emergency stop [m/s^2]",
-              "default": -2.5
-            },
-            "target_jerk": {
-              "type": "number",
-              "description": "Target jerk for emergency stop [m/s^3]",
-              "default": -1.5
-            },
-            "turning_hazard_on": {
-              "type": "boolean",
-              "description": "Whether to turn on hazard lamps when emergency_stop is being operated",
-              "default": "true"
-            },
-            "use_parking_after_stopped": {
-              "type": "boolean",
-              "description": "Whether to enable GearCommand::PARK after emergency_stop succeeded",
-              "default": "false"
-            }
-          },
-          "required": [
-            "update_rate",
-            "target_acceleration",
-            "target_jerk",
-            "turning_hazard_on",
-            "use_parking_after_stopped"
-          ]
-        },
-        "comfortable_stop_operator": {
-          "type": "object",
-          "properties": {
-            "min_acceleration": {
-              "type": "number",
-              "description": "Minimum acceleration for comfortable stop [m/s^2]",
-              "default": -1.0
-            },
-            "max_jerk": {
-              "type": "number",
-              "description": "Maximum jerk for comfortable stop [m/s^3]",
-              "default": 0.3
-            },
-            "min_jerk": {
-              "type": "number",
-              "description": "Minimum jerk for comfortable stop [m/s^3]",
-              "default": -0.3
-            }
-          },
-          "required": ["min_acceleration", "max_jerk", "min_jerk"]
         }
       },
       "required": [
         "update_rate",
         "timeout_hazard_status",
-        "use_comfortable_stop",
-        "emergency_stop",
-        "comfortable_stop"
+        "use_comfortable_stop"
       ],
       "additionalProperties": false
     }

system/emergency_handler/src/emergency_handler_core.cpp

@@ -52,10 +52,11 @@ EmergencyHandler::EmergencyHandler() : Node("emergency_handler")
   mrm_state_.behavior = autoware_adapi_v1_msgs::msg::MrmState::NONE;
   is_hazard_status_timeout_ = false;
 
-  // Opertors
-  emergency_stop_operator_ = std::make_unique<emergency_stop_operator::EmergencyStopOperator>(this);
-  comfortable_stop_operator_ =
-    std::make_unique<comfortable_stop_operator::ComfortableStopOperator>(this);
+  // MRM opertors
+  mrm_emergency_stop_operator_ =
+    std::make_unique<mrm_emergency_stop_operator::MrmEmergencyStopOperator>();
+  mrm_comfortable_stop_operator_ =
+    std::make_unique<mrm_comfortable_stop_operator::MrmComfortableStopOperator>();
 
   // Timer
   const auto update_period_ns = rclcpp::Rate(param_.update_rate).period();
@@ -131,15 +132,15 @@ void EmergencyHandler::callMrmBehavior(
     return;
   }
   if (mrm_behavior == MrmState::COMFORTABLE_STOP) {
-    if (comfortable_stop_operator_->operate()) {
+    if (mrm_comfortable_stop_operator_->operate()) {
       RCLCPP_WARN(this->get_logger(), "Comfortable stop is operated");
     } else {
       RCLCPP_ERROR(this->get_logger(), "Comfortable stop is failed to operate");
     }
     return;
   }
   if (mrm_behavior == MrmState::EMERGENCY_STOP) {
-    if (emergency_stop_operator_->operate()) {
+    if (mrm_emergency_stop_operator_->operate()) {
       RCLCPP_WARN(this->get_logger(), "Emergency stop is operated");
     } else {
       RCLCPP_ERROR(this->get_logger(), "Emergency stop is failed to operate");
@@ -159,15 +160,15 @@ void EmergencyHandler::cancelMrmBehavior(
     return;
   }
   if (mrm_behavior == MrmState::COMFORTABLE_STOP) {
-    if (comfortable_stop_operator_->cancel()) {
+    if (mrm_comfortable_stop_operator_->cancel()) {
       RCLCPP_WARN(this->get_logger(), "Comfortable stop is canceled");
     } else {
       RCLCPP_ERROR(this->get_logger(), "Comfortable stop is failed to cancel");
     }
     return;
   }
   if (mrm_behavior == MrmState::EMERGENCY_STOP) {
-    if (emergency_stop_operator_->cancel()) {
+    if (mrm_emergency_stop_operator_->cancel()) {
       RCLCPP_WARN(this->get_logger(), "Emergency stop is canceled");
     } else {
       RCLCPP_ERROR(this->get_logger(), "Emergency stop is failed to cancel");

system/emergency_handler/src/emergency_handler_core.hpp

@@ -23,16 +23,13 @@
 #include <autoware_adapi_v1_msgs/msg/mrm_state.hpp>
 #include <autoware_auto_system_msgs/msg/hazard_status_stamped.hpp>
 #include <autoware_auto_vehicle_msgs/msg/control_mode_report.hpp>
-
-// ROS 2 core
-#include <rclcpp/rclcpp.hpp>
-
 #include <diagnostic_msgs/msg/diagnostic_array.hpp>
 #include <nav_msgs/msg/odometry.hpp>
+#include <mrm_emergency_stop_operator/mrm_emergency_stop_operator.hpp>
+#include <mrm_comfortable_stop_operator/mrm_comfortable_stop_operator.hpp>
 
-// Operators
-#include "comfortable_stop_operator.hpp"
-#include "emergency_stop_operator.hpp"
+// ROS 2 core
+#include <rclcpp/rclcpp.hpp>
 
 namespace emergency_handler
 {
@@ -41,7 +38,6 @@ struct Param
 {
   int update_rate;
   double timeout_hazard_status;
-  bool use_parking_after_stopped;
   bool use_comfortable_stop;
 };
 
@@ -92,8 +88,8 @@ class EmergencyHandler : public rclcpp::Node
   void checkHazardStatusTimeout();
 
   // Operators
-  std::unique_ptr<emergency_stop_operator::EmergencyStopOperator> emergency_stop_operator_;
-  std::unique_ptr<comfortable_stop_operator::ComfortableStopOperator> comfortable_stop_operator_;
+  std::unique_ptr<mrm_emergency_stop_operator::MrmEmergencyStopOperator> mrm_emergency_stop_operator_;
+  std::unique_ptr<mrm_comfortable_stop_operator::MrmComfortableStopOperator> mrm_comfortable_stop_operator_;
 
   // Algorithm
   void transitionTo(const int new_state);

system/mrm_comfortable_stop_operator/CMakeLists.txt

@@ -4,14 +4,10 @@ project(mrm_comfortable_stop_operator)
 find_package(autoware_cmake REQUIRED)
 autoware_package()
 
-ament_auto_add_library(mrm_comfortable_stop_operator_component SHARED
-  src/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator_core.cpp
+ament_auto_add_library(mrm_comfortable_stop_operator SHARED
+  src/mrm_comfortable_stop_operator.cpp
 )
 
-rclcpp_components_register_node(mrm_comfortable_stop_operator_component
-  PLUGIN "mrm_comfortable_stop_operator::MrmComfortableStopOperator"
-  EXECUTABLE mrm_comfortable_stop_operator)
-
 ament_auto_package(INSTALL_TO_SHARE
   launch
   config

system/mrm_comfortable_stop_operator/README.md

@@ -10,26 +10,15 @@ MRM comfortable stop operator is a node that generates comfortable stop commands
 
 ### Input
 
-| Name                                   | Type                                 | Description         |
-| -------------------------------------- | ------------------------------------ | ------------------- |
-| `~/input/mrm/comfortable_stop/operate` | `tier4_system_msgs::srv::OperateMrm` | MRM execution order |
-
 ### Output
 
 | Name                                   | Type                                                  | Description                  |
 | -------------------------------------- | ----------------------------------------------------- | ---------------------------- |
-| `~/output/mrm/comfortable_stop/status` | `tier4_system_msgs::msg::MrmBehaviorStatus`           | MRM execution status         |
 | `~/output/velocity_limit`              | `tier4_planning_msgs::msg::VelocityLimit`             | Velocity limit command       |
 | `~/output/velocity_limit/clear`        | `tier4_planning_msgs::msg::VelocityLimitClearCommand` | Velocity limit clear command |
 
 ## Parameters
 
-### Node Parameters
-
-| Name        | Type | Default value | Explanation                   |
-| ----------- | ---- | ------------- | ----------------------------- |
-| update_rate | int  | `10`          | Timer callback frequency [Hz] |
-
 ### Core Parameters
 
 | Name             | Type   | Default value | Explanation                                       |

system/mrm_comfortable_stop_operator/config/mrm_comfortable_stop_operator.param.yaml

@@ -1,6 +1,5 @@
 /**:
   ros__parameters:
-    update_rate: 10
     min_acceleration: -1.0
     max_jerk: 0.3
     min_jerk: -0.3

system/emergency_handler/src/comfortable_stop_operator.hpp system/mrm_comfortable_stop_operator/include/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator.hpp

@@ -1,4 +1,4 @@
-// Copyright 2024 TIER IV, Inc.
+// Copyright 2022 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.
@@ -12,47 +12,49 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef COMFORTABLE_STOP_OPERATOR_HPP_
-#define COMFORTABLE_STOP_OPERATOR_HPP_
+#ifndef MRM_COMFORTABLE_STOP_OPERATOR_HPP_
+#define MRM_COMFORTABLE_STOP_OPERATOR_HPP_
 
-#include <rclcpp/rclcpp.hpp>
+// Core
+#include <memory>
 
+// Autoware
 #include <tier4_planning_msgs/msg/velocity_limit.hpp>
 #include <tier4_planning_msgs/msg/velocity_limit_clear_command.hpp>
 
-namespace emergency_handler::comfortable_stop_operator
+// ROS 2 core
+#include <rclcpp/rclcpp.hpp>
+
+namespace mrm_comfortable_stop_operator
 {
 
-struct Param
+struct Parameters
 {
-  double min_acceleration;
-  double max_jerk;
-  double min_jerk;
+  double min_acceleration;  // [m/s^2]
+  double max_jerk;          // [m/s^3]
+  double min_jerk;          // [m/s^3]
 };
 
-class ComfortableStopOperator
+class MrmComfortableStopOperator : public rclcpp::Node
 {
 public:
-  explicit ComfortableStopOperator(rclcpp::Node * node);
+  explicit MrmComfortableStopOperator();
   bool operate();
   bool cancel();
 
 private:
   // Parameters
-  Param params_;
+  Parameters params_;
 
   // Publisher
   rclcpp::Publisher<tier4_planning_msgs::msg::VelocityLimit>::SharedPtr pub_velocity_limit_;
   rclcpp::Publisher<tier4_planning_msgs::msg::VelocityLimitClearCommand>::SharedPtr
     pub_velocity_limit_clear_command_;
 
-  // Algorithm
-  void publishVelocityLimit();
-  void publishVelocityLimitClearCommand();
-
-  rclcpp::Node * node_;
+  void publishVelocityLimit() const;
+  void publishVelocityLimitClearCommand() const;
 };
 
-}  // namespace emergency_handler::comfortable_stop_operator
+}  // namespace mrm_comfortable_stop_operator
 
-#endif  // COMFORTABLE_STOP_OPERATOR_HPP_
+#endif  // MRM_COMFORTABLE_STOP_OPERATOR_HPP_