Merge branch 'main' into fix_mrm_handler

Author: veqcc

control/mpc_lateral_controller/src/mpc_lateral_controller.cpp

@@ -243,7 +243,11 @@ trajectory_follower::LateralOutput MpcLateralController::run(
   Trajectory predicted_traj;
   Float32MultiArrayStamped debug_values;
 
-  if (!m_is_ctrl_cmd_prev_initialized) {
+  const bool is_under_control = input_data.current_operation_mode.is_autoware_control_enabled &&
+                                input_data.current_operation_mode.mode ==
+                                  autoware_adapi_v1_msgs::msg::OperationModeState::AUTONOMOUS;
+
+  if (!m_is_ctrl_cmd_prev_initialized || !is_under_control) {
     m_ctrl_cmd_prev = getInitialControlCommand();
     m_is_ctrl_cmd_prev_initialized = true;
   }

system/emergency_handler/config/emergency_handler.param.yaml

@@ -4,8 +4,6 @@
   ros__parameters:
     update_rate: 10
     timeout_hazard_status: 0.5
-    timeout_takeover_request: 10.0
-    use_takeover_request: false
     use_parking_after_stopped: false
     use_comfortable_stop: false
 

system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp

@@ -45,8 +45,6 @@ struct Param
 {
   int update_rate;
   double timeout_hazard_status;
-  double timeout_takeover_request;
-  bool use_takeover_request;
   bool use_parking_after_stopped;
   bool use_comfortable_stop;
   HazardLampPolicy turning_hazard_on{};
@@ -135,8 +133,6 @@ class EmergencyHandler : public rclcpp::Node
   rclcpp::Time stamp_hazard_status_;
 
   // Algorithm
-  rclcpp::Time takeover_requested_time_;
-  bool is_takeover_request_ = false;
   void transitionTo(const int new_state);
   void updateMrmState();
   void operateMrm();

system/emergency_handler/schema/emergency_handler.schema.json

@@ -16,16 +16,6 @@
           "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
         },
-        "timeout_takeover_request": {
-          "type": "number",
-          "description": "Transition to MRR_OPERATING if the time from the last takeover request exceeds `timeout_takeover_request`.",
-          "default": 10.0
-        },
-        "use_takeover_request": {
-          "type": "boolean",
-          "description": "If this parameter is true, the handler will record the time and make take over request to the driver when emergency state occurs.",
-          "default": "false"
-        },
         "use_parking_after_stopped": {
           "type": "boolean",
           "description": "If this parameter is true, it will publish PARKING shift command.",
@@ -51,8 +41,6 @@
       "required": [
         "update_rate",
         "timeout_hazard_status",
-        "timeout_takeover_request",
-        "use_takeover_request",
         "use_parking_after_stopped",
         "use_comfortable_stop",
         "turning_hazard_on"

system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp

@@ -23,8 +23,6 @@ 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_.timeout_takeover_request = declare_parameter<double>("timeout_takeover_request");
-  param_.use_takeover_request = declare_parameter<bool>("use_takeover_request");
   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");
@@ -375,41 +373,23 @@ void EmergencyHandler::updateMrmState()
 
   // Get mode
   const bool is_auto_mode = control_mode_->mode == ControlModeReport::AUTONOMOUS;
-  const bool is_takeover_done = control_mode_->mode == ControlModeReport::MANUAL;
 
   // State Machine
   if (mrm_state_.state == MrmState::NORMAL) {
     // NORMAL
     if (is_auto_mode && is_emergency) {
-      if (param_.use_takeover_request) {
-        takeover_requested_time_ = this->get_clock()->now();
-        is_takeover_request_ = true;
-        return;
-      } else {
-        transitionTo(MrmState::MRM_OPERATING);
-        return;
-      }
+      transitionTo(MrmState::MRM_OPERATING);
+      return;
     }
   } else {
     // Emergency
-    // Send recovery events if "not emergency" or "takeover done"
+    // Send recovery events if "not emergency"
     if (!is_emergency) {
       transitionTo(MrmState::NORMAL);
       return;
     }
-    // TODO(Kenji Miyake): Check if human can safely override, for example using DSM
-    if (is_takeover_done) {
-      transitionTo(MrmState::NORMAL);
-      return;
-    }
 
-    if (is_takeover_request_) {
-      const auto time_from_takeover_request = this->get_clock()->now() - takeover_requested_time_;
-      if (time_from_takeover_request.seconds() > param_.timeout_takeover_request) {
-        transitionTo(MrmState::MRM_OPERATING);
-        return;
-      }
-    } else if (mrm_state_.state == MrmState::MRM_OPERATING) {
+    if (mrm_state_.state == MrmState::MRM_OPERATING) {
       // TODO(Kenji Miyake): Check MRC is accomplished
       if (isStopped()) {
         transitionTo(MrmState::MRM_SUCCEEDED);

system/mrm_handler/config/mrm_handler.param.yaml

@@ -6,8 +6,6 @@
     timeout_operation_mode_availability: 0.5
     use_emergency_holding: false
     timeout_emergency_recovery: 5.0
-    timeout_takeover_request: 10.0
-    use_takeover_request: false
     use_parking_after_stopped: false
     use_pull_over: false
     use_comfortable_stop: false

system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp

@@ -49,8 +49,6 @@ struct Param
   double timeout_operation_mode_availability;
   bool use_emergency_holding;
   double timeout_emergency_recovery;
-  double timeout_takeover_request;
-  bool use_takeover_request;
   bool use_parking_after_stopped;
   bool use_pull_over;
   bool use_comfortable_stop;
@@ -106,9 +104,8 @@ class MrmHandler : public rclcpp::Node
     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();
+  void publishHazardCmd();
+  void publishGearCmd();
 
   rclcpp::Publisher<autoware_adapi_v1_msgs::msg::MrmState>::SharedPtr pub_mrm_state_;
 
@@ -147,8 +144,6 @@ class MrmHandler : public rclcpp::Node
   void checkOperationModeAvailabilityTimeout();
 
   // Algorithm
-  rclcpp::Time takeover_requested_time_;
-  bool is_takeover_request_ = false;
   bool is_emergency_holding_ = false;
   void transitionTo(const int new_state);
   void updateMrmState();

system/mrm_handler/schema/mrm_handler.schema.json

@@ -26,16 +26,6 @@
           "description": "If the duration of the EMERGENCY state is longer than `timeout_emergency_recovery`, it does not recover to the NORMAL state.",
           "default": 5.0
         },
-        "timeout_takeover_request": {
-          "type": "number",
-          "description": "Transition to MRR_OPERATING if the time from the last takeover request exceeds `timeout_takeover_request`.",
-          "default": 10.0
-        },
-        "use_takeover_request": {
-          "type": "boolean",
-          "description": "If this parameter is true, the handler will record the time and make take over request to the driver when emergency state occurs.",
-          "default": "false"
-        },
         "use_parking_after_stopped": {
           "type": "boolean",
           "description": "If this parameter is true, it will publish PARKING shift command.",
@@ -68,8 +58,6 @@
         "timeout_operation_mode_availability",
         "use_emergency_holding",
         "timeout_emergency_recovery",
-        "timeout_takeover_request",
-        "use_takeover_request",
         "use_parking_after_stopped",
         "use_pull_over",
         "use_comfortable_stop",

system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp

@@ -26,8 +26,6 @@ MrmHandler::MrmHandler() : Node("mrm_handler")
     declare_parameter<double>("timeout_operation_mode_availability", 0.5);
   param_.use_emergency_holding = declare_parameter<bool>("use_emergency_holding", false);
   param_.timeout_emergency_recovery = declare_parameter<double>("timeout_emergency_recovery", 5.0);
-  param_.timeout_takeover_request = declare_parameter<double>("timeout_takeover_request", 10.0);
-  param_.use_takeover_request = declare_parameter<bool>("use_takeover_request", false);
   param_.use_parking_after_stopped = declare_parameter<bool>("use_parking_after_stopped", false);
   param_.use_pull_over = declare_parameter<bool>("use_pull_over", false);
   param_.use_comfortable_stop = declare_parameter<bool>("use_comfortable_stop", false);
@@ -164,48 +162,39 @@ void MrmHandler::onOperationModeState(
   operation_mode_state_ = msg;
 }
 
-autoware_auto_vehicle_msgs::msg::HazardLightsCommand MrmHandler::createHazardCmdMsg()
+void MrmHandler::publishHazardCmd()
 {
   using autoware_auto_vehicle_msgs::msg::HazardLightsCommand;
   HazardLightsCommand msg;
 
-  // Check emergency
-  const bool is_emergency = isEmergency();
-
+  msg.stamp = this->now();
   if (is_emergency_holding_) {
     // turn hazard on during emergency holding
     msg.command = HazardLightsCommand::ENABLE;
-  } else if (is_emergency && param_.turning_hazard_on.emergency) {
+  } else if (isEmergency() && 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;
+
+  pub_hazard_cmd_->publish(msg);
 }
 
-void MrmHandler::publishControlCommands()
+void MrmHandler::publishGearCmd()
 {
   using autoware_auto_vehicle_msgs::msg::GearCommand;
+  GearCommand msg;
 
-  // 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);
+  msg.stamp = this->now();
+  if (param_.use_parking_after_stopped && isStopped()) {
+    msg.command = GearCommand::PARK;
+  } else {
+    msg.command = GearCommand::DRIVE;
   }
+
+  pub_gear_cmd_->publish(msg);
 }
 
 void MrmHandler::publishMrmState()
@@ -396,10 +385,13 @@ void MrmHandler::onTimer()
   // Update Emergency State
   updateMrmState();
 
-  // Publish control commands
-  publishControlCommands();
+  // Operate MRM
   operateMrm();
+
+  // Publish
   publishMrmState();
+  publishHazardCmd();
+  publishGearCmd();
 }
 
 void MrmHandler::transitionTo(const int new_state)
@@ -441,41 +433,23 @@ void MrmHandler::updateMrmState()
 
   // Get mode
   const bool is_auto_mode = control_mode_->mode == ControlModeReport::AUTONOMOUS;
-  const bool is_takeover_done = control_mode_->mode == ControlModeReport::MANUAL;
 
   // State Machine
   if (mrm_state_.state == MrmState::NORMAL) {
     // NORMAL
     if (is_auto_mode && is_emergency) {
-      if (param_.use_takeover_request) {
-        takeover_requested_time_ = this->get_clock()->now();
-        is_takeover_request_ = true;
-        return;
-      } else {
-        transitionTo(MrmState::MRM_OPERATING);
-        return;
-      }
+      transitionTo(MrmState::MRM_OPERATING);
+      return;
     }
   } else {
     // Emergency
-    // Send recovery events if "not emergency" or "takeover done"
+    // Send recovery events if "not emergency"
     if (!is_emergency) {
       transitionTo(MrmState::NORMAL);
       return;
     }
-    // TODO(TetsuKawa): Check if human can safely override, for example using DSM
-    if (is_takeover_done) {
-      transitionTo(MrmState::NORMAL);
-      return;
-    }
 
-    if (is_takeover_request_) {
-      const auto time_from_takeover_request = this->get_clock()->now() - takeover_requested_time_;
-      if (time_from_takeover_request.seconds() > param_.timeout_takeover_request) {
-        transitionTo(MrmState::MRM_OPERATING);
-        return;
-      }
-    } else if (mrm_state_.state == MrmState::MRM_OPERATING) {
+    if (mrm_state_.state == MrmState::MRM_OPERATING) {
       // TODO(TetsuKawa): Check MRC is accomplished
       if (mrm_state_.behavior == MrmState::PULL_OVER) {
         if (isStopped() && isArrivedAtGoal()) {