fix: move parameters from handlers to operators

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

Author: veqcc

system/emergency_handler/config/emergency_handler.param.yaml

@@ -4,17 +4,14 @@
   ros__parameters:
     update_rate: 10
     timeout_hazard_status: 0.5
-    use_parking_after_stopped: false
     use_comfortable_stop: false
 
-    # setting whether to turn hazard lamp on for each situation
-    turning_hazard_on:
-      emergency: true
-
     # emergency stop operator class
     emergency_stop_operator:
       target_acceleration: -2.5
       target_jerk: -1.5
+      turning_hazard_on: true
+      use_parking_after_stopped: false
 
     # comfortable stop operator class
     comfortable_stop_operator:

system/emergency_handler/launch/emergency_handler.launch.xml

@@ -3,15 +3,15 @@
   <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_gear" default="/system/emergency/gear_cmd"/>
-  <arg name="output_hazard" default="/system/emergency/hazard_lights_cmd"/>
   <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"/>
@@ -22,15 +22,15 @@
     <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/gear" to="$(var output_gear)"/>
-    <remap from="~/output/hazard" to="$(var output_hazard)"/>
     <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)"/>

system/emergency_handler/schema/emergency_handler.schema.json

@@ -16,27 +16,11 @@
           "description": "If the input `hazard_status` topic cannot be received for more than `timeout_hazard_status`, vehicle will make an emergency stop.",
           "default": 0.5
         },
-        "use_parking_after_stopped": {
-          "type": "boolean",
-          "description": "If this parameter is true, it will publish PARKING shift command.",
-          "default": "false"
-        },
         "use_comfortable_stop": {
           "type": "boolean",
           "description": "If this parameter is true, operate comfortable stop when latent faults occur.",
           "default": "false"
         },
-        "turning_hazard_on": {
-          "type": "object",
-          "properties": {
-            "emergency": {
-              "type": "boolean",
-              "description": "If this parameter is true, hazard lamps will be turned on during emergency state.",
-              "default": "true"
-            }
-          },
-          "required": ["emergency"]
-        },
         "emergency_stop_operator": {
           "type": "object",
           "properties": {
@@ -47,11 +31,26 @@
             },
             "target_jerk": {
               "type": "number",
-              "description": "Target jerk for emergency stop [m/s^3] ",
+              "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": ["target_acceleration", "target_jerk"]
+          "required": [
+            "target_acceleration",
+            "target_jerk",
+            "turning_hazard_on",
+            "use_parking_after_stopped"
+          ]
         },
         "comfortable_stop_operator": {
           "type": "object",
@@ -72,15 +71,17 @@
               "default": -0.3
             }
           },
-          "required": ["min_acceleration", "max_jerk", "min_jerk"]
+          "required": [
+            "min_acceleration",
+            "max_jerk",
+            "min_jerk"
+          ]
         }
       },
       "required": [
         "update_rate",
         "timeout_hazard_status",
-        "use_parking_after_stopped",
         "use_comfortable_stop",
-        "turning_hazard_on",
         "emergency_stop",
         "comfortable_stop"
       ],

system/emergency_handler/src/emergency_handler_core.cpp

@@ -26,9 +26,7 @@ EmergencyHandler::EmergencyHandler() : Node("emergency_handler")
   // Parameter
   param_.update_rate = declare_parameter<int>("update_rate");
   param_.timeout_hazard_status = declare_parameter<double>("timeout_hazard_status");
-  param_.use_parking_after_stopped = declare_parameter<bool>("use_parking_after_stopped");
   param_.use_comfortable_stop = declare_parameter<bool>("use_comfortable_stop");
-  param_.turning_hazard_on.emergency = declare_parameter<bool>("turning_hazard_on.emergency");
 
   using std::placeholders::_1;
 
@@ -39,15 +37,10 @@ EmergencyHandler::EmergencyHandler() : Node("emergency_handler")
       std::bind(&EmergencyHandler::onHazardStatusStamped, this, _1));
   sub_odom_ = create_subscription<nav_msgs::msg::Odometry>(
     "~/input/odometry", rclcpp::QoS{1}, std::bind(&EmergencyHandler::onOdometry, this, _1));
-  // subscribe control mode
   sub_control_mode_ = create_subscription<autoware_auto_vehicle_msgs::msg::ControlModeReport>(
     "~/input/control_mode", rclcpp::QoS{1}, std::bind(&EmergencyHandler::onControlMode, this, _1));
 
   // Publisher
-  pub_hazard_cmd_ = create_publisher<autoware_auto_vehicle_msgs::msg::HazardLightsCommand>(
-    "~/output/hazard", rclcpp::QoS{1});
-  pub_gear_cmd_ =
-    create_publisher<autoware_auto_vehicle_msgs::msg::GearCommand>("~/output/gear", rclcpp::QoS{1});
   pub_mrm_state_ =
     create_publisher<autoware_adapi_v1_msgs::msg::MrmState>("~/output/mrm/state", rclcpp::QoS{1});
 
@@ -88,51 +81,6 @@ void EmergencyHandler::onControlMode(
   control_mode_ = msg;
 }
 
-autoware_auto_vehicle_msgs::msg::HazardLightsCommand EmergencyHandler::createHazardCmdMsg()
-{
-  using autoware_auto_vehicle_msgs::msg::HazardLightsCommand;
-  HazardLightsCommand msg;
-
-  // Check emergency
-  const bool is_emergency = isEmergency();
-
-  if (hazard_status_stamped_->status.emergency_holding) {
-    // turn hazard on during emergency holding
-    msg.command = HazardLightsCommand::ENABLE;
-  } else if (is_emergency && param_.turning_hazard_on.emergency) {
-    // turn hazard on if vehicle is in emergency state and
-    // turning hazard on if emergency flag is true
-    msg.command = HazardLightsCommand::ENABLE;
-
-  } else {
-    msg.command = HazardLightsCommand::NO_COMMAND;
-  }
-  return msg;
-}
-
-void EmergencyHandler::publishControlCommands()
-{
-  using autoware_auto_vehicle_msgs::msg::GearCommand;
-
-  // Create timestamp
-  const auto stamp = this->now();
-
-  // Publish hazard command
-  pub_hazard_cmd_->publish(createHazardCmdMsg());
-
-  // Publish gear
-  {
-    GearCommand msg;
-    msg.stamp = stamp;
-    if (param_.use_parking_after_stopped && isStopped()) {
-      msg.command = GearCommand::PARK;
-    } else {
-      msg.command = GearCommand::DRIVE;
-    }
-    pub_gear_cmd_->publish(msg);
-  }
-}
-
 void EmergencyHandler::publishMrmState()
 {
   mrm_state_.stamp = this->now();
@@ -269,8 +217,6 @@ void EmergencyHandler::onTimer()
   // Update Emergency State
   updateMrmState();
 
-  // Publish control commands
-  publishControlCommands();
   operateMrm();
   publishMrmState();
 }

system/emergency_handler/src/emergency_handler_core.hpp

@@ -23,8 +23,6 @@
 #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>
-#include <autoware_auto_vehicle_msgs/msg/gear_command.hpp>
-#include <autoware_auto_vehicle_msgs/msg/hazard_lights_command.hpp>
 
 // ROS 2 core
 #include <rclcpp/rclcpp.hpp>
@@ -39,18 +37,12 @@
 namespace emergency_handler
 {
 
-struct HazardLampPolicy
-{
-  bool emergency;
-};
-
 struct Param
 {
   int update_rate;
   double timeout_hazard_status;
   bool use_parking_after_stopped;
   bool use_comfortable_stop;
-  HazardLampPolicy turning_hazard_on{};
 };
 
 class EmergencyHandler : public rclcpp::Node
@@ -76,16 +68,7 @@ class EmergencyHandler : public rclcpp::Node
   void onControlMode(const autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg);
 
   // Publisher
-  rclcpp::Publisher<autoware_auto_vehicle_msgs::msg::HazardLightsCommand>::SharedPtr
-    pub_hazard_cmd_;
-  rclcpp::Publisher<autoware_auto_vehicle_msgs::msg::GearCommand>::SharedPtr pub_gear_cmd_;
-
-  autoware_auto_vehicle_msgs::msg::HazardLightsCommand createHazardCmdMsg();
-  autoware_auto_vehicle_msgs::msg::GearCommand createGearCmdMsg();
-  void publishControlCommands();
-
   rclcpp::Publisher<autoware_adapi_v1_msgs::msg::MrmState>::SharedPtr pub_mrm_state_;
-
   autoware_adapi_v1_msgs::msg::MrmState mrm_state_;
   void publishMrmState();
 

system/emergency_handler/src/emergency_stop_operator.cpp

@@ -23,15 +23,21 @@ EmergencyStopOperator::EmergencyStopOperator(rclcpp::Node * node) : node_(node)
   params_.target_acceleration =
     node_->declare_parameter<double>("emergency_stop_operator.target_acceleration");
   params_.target_jerk = node_->declare_parameter<double>("emergency_stop_operator.target_jerk");
+  params_.turning_hazard_on = node_->declare_parameter<bool>("emergency_stop_operator.turning_hazard_on");
+  params_.use_parking_after_stopped = node_->declare_parameter<bool>("emergency_stop_operator.use_parking_after_stopped");
 
   // Subscriber
   sub_control_cmd_ = node_->create_subscription<AckermannControlCommand>(
     "~/input/control/control_cmd", 1,
     std::bind(&EmergencyStopOperator::onControlCommand, this, std::placeholders::_1));
+  sub_odom_ = node_->create_subscription<nav_msgs::msg::Odometry>(
+    "~/input/odometry", rclcpp::QoS{1}, std::bind(&EmergencyStopOperator::onOdometry, this, std::placeholders::_1));
 
   // Publisher
   pub_control_cmd_ =
-    node_->create_publisher<AckermannControlCommand>("~/output/mrm/emergency_stop/control_cmd", 1);
+    node_->create_publisher<AckermannControlCommand>("~/output/mrm/emergency_stop/control_cmd", rclcpp::QoS{1});
+  pub_hazard_light_cmd_ = node_->create_publisher<HazardLightsCommand>("~/output/hazard", rclcpp::QoS{1});
+  pub_gear_cmd_ = node_->create_publisher<GearCommand>("~/output/gear", rclcpp::QoS{1});
 
   // Initialize
   is_prev_control_cmd_subscribed_ = false;
@@ -43,14 +49,24 @@ void EmergencyStopOperator::onControlCommand(AckermannControlCommand::ConstShare
   is_prev_control_cmd_subscribed_ = true;
 }
 
+void EmergencyStopOperator::onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg)
+{
+  constexpr auto th_stopped_velocity = 0.001;
+  is_stopped_ = msg->twist.twist.linear.x < th_stopped_velocity;
+}
+
 void EmergencyStopOperator::onTimer()
 {
-  pub_control_cmd_->publish(calcNextControlCmd());
+  publishControlCmd();
+  publishHazardLightCmd();
+  publishGearCmd();
 }
 
 bool EmergencyStopOperator::operate()
 {
-  pub_control_cmd_->publish(calcNextControlCmd());
+  publishControlCmd();
+  publishHazardLightCmd();
+  publishGearCmd();
 
   // Currently, EmergencyStopOperator does not return false
   return true;
@@ -64,7 +80,7 @@ bool EmergencyStopOperator::cancel()
   return true;
 }
 
-AckermannControlCommand EmergencyStopOperator::calcNextControlCmd()
+void EmergencyStopOperator::publishControlCmd()
 {
   AckermannControlCommand control_cmd;
   auto now = node_->now();
@@ -105,7 +121,31 @@ AckermannControlCommand EmergencyStopOperator::calcNextControlCmd()
     // lateral: keep previous lateral command
   }
 
-  return control_cmd;
+  pub_control_cmd_->publish(control_cmd);
+}
+
+void EmergencyStopOperator::publishHazardLightCmd()
+{
+  HazardLightsCommand hazard_light_cmd;
+  hazard_light_cmd.stamp = node_->now();
+  if (params_.turning_hazard_on) {
+    hazard_light_cmd.command = HazardLightsCommand::ENABLE;
+  } else {
+    hazard_light_cmd.command = HazardLightsCommand::NO_COMMAND;
+  }
+  pub_hazard_light_cmd_->publish(hazard_light_cmd);
+}
+
+void EmergencyStopOperator::publishGearCmd()
+{
+  GearCommand gear_cmd;
+  gear_cmd.stamp = node_->now();
+  if (params_.use_parking_after_stopped && is_stopped_) {
+    gear_cmd.command = GearCommand::PARK;
+  } else {
+    gear_cmd.command = GearCommand::DRIVE;
+  }
+  pub_gear_cmd_->publish(gear_cmd);
 }
 
 }  // namespace emergency_handler::emergency_stop_operator