fix(emergency_handler,mrm_handler): check for ego speed when determin…

…ing the gear command (#7264) * check for ego speed when determining the gear command Signed-off-by: Daniel Sanchez <[email protected]> * add gear history Signed-off-by: Daniel Sanchez <[email protected]> * update msg types Signed-off-by: veqcc <[email protected]> --------- Signed-off-by: Daniel Sanchez <[email protected]> Signed-off-by: veqcc <[email protected]> Co-authored-by: veqcc <[email protected]>
autowarefoundation · Jun 6, 2024 · 6fb5221 · 6fb5221
1 parent d0be029
commit 6fb5221
Show file tree

Hide file tree

Showing 4 changed files with 36 additions and 19 deletions.
diff --git a/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp b/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp
@@ -130,11 +130,13 @@ class EmergencyHandler : public rclcpp::Node
   void checkHazardStatusTimeout();
 
   // Algorithm
+  uint8_t last_gear_command_{autoware_vehicle_msgs::msg::GearCommand::DRIVE};
   void transitionTo(const int new_state);
   void updateMrmState();
   void operateMrm();
   autoware_adapi_v1_msgs::msg::MrmState::_behavior_type getCurrentMrmBehavior();
   bool isStopped();
+  bool isDrivingBackwards();
   bool isEmergency();
 };
 

diff --git a/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp b/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp
@@ -164,12 +164,17 @@ void EmergencyHandler::publishControlCommands()
   {
     GearCommand msg;
     msg.stamp = stamp;
-    if (param_.use_parking_after_stopped && isStopped()) {
-      msg.command = GearCommand::PARK;
-    } else {
-      msg.command = GearCommand::DRIVE;
-    }
+    const auto command = [&]() {
+      // If stopped and use_parking is not true, send the last gear command
+      if (isStopped())
+        return (param_.use_parking_after_stopped) ? GearCommand::PARK : last_gear_command_;
+      return (isDrivingBackwards()) ? GearCommand::REVERSE : GearCommand::DRIVE;
+    }();
+
+    msg.command = command;
+    last_gear_command_ = msg.command;
     pub_gear_cmd_->publish(msg);
+    return;
   }
 }
 
@@ -451,11 +456,13 @@ bool EmergencyHandler::isEmergency()
 bool EmergencyHandler::isStopped()
 {
   constexpr auto th_stopped_velocity = 0.001;
-  if (odom_->twist.twist.linear.x < th_stopped_velocity) {
-    return true;
-  }
+  return (std::abs(odom_->twist.twist.linear.x) < th_stopped_velocity);
+}
 
-  return false;
+bool EmergencyHandler::isDrivingBackwards()
+{
+  constexpr auto th_moving_backwards = -0.001;
+  return odom_->twist.twist.linear.x < th_moving_backwards;
 }
 
 #include <rclcpp_components/register_node_macro.hpp>

diff --git a/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp b/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp
@@ -147,12 +147,14 @@ class MrmHandler : public rclcpp::Node
 
   // Algorithm
   bool is_emergency_holding_ = false;
+  uint8_t last_gear_command_{autoware_vehicle_msgs::msg::GearCommand::DRIVE};
   void transitionTo(const int new_state);
   void updateMrmState();
   void operateMrm();
   void handleFailedRequest();
   autoware_adapi_v1_msgs::msg::MrmState::_behavior_type getCurrentMrmBehavior();
   bool isStopped();
+  bool isDrivingBackwards();
   bool isEmergency() const;
   bool isArrivedAtGoal();
 };

diff --git a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp
@@ -191,13 +191,17 @@ void MrmHandler::publishGearCmd()
   GearCommand msg;
 
   msg.stamp = this->now();
-  if (param_.use_parking_after_stopped && isStopped()) {
-    msg.command = GearCommand::PARK;
-  } else {
-    msg.command = GearCommand::DRIVE;
-  }
-
+  const auto command = [&]() {
+    // If stopped and use_parking is not true, send the last gear command
+    if (isStopped())
+      return (param_.use_parking_after_stopped) ? GearCommand::PARK : last_gear_command_;
+    return (isDrivingBackwards()) ? GearCommand::REVERSE : GearCommand::DRIVE;
+  }();
+
+  msg.command = command;
+  last_gear_command_ = msg.command;
   pub_gear_cmd_->publish(msg);
+  return;
 }
 
 void MrmHandler::publishMrmState()
@@ -578,11 +582,13 @@ autoware_adapi_v1_msgs::msg::MrmState::_behavior_type MrmHandler::getCurrentMrmB
 bool MrmHandler::isStopped()
 {
   constexpr auto th_stopped_velocity = 0.001;
-  if (odom_->twist.twist.linear.x < th_stopped_velocity) {
-    return true;
-  }
+  return std::abs((odom_->twist.twist.linear.x < th_stopped_velocity) < th_stopped_velocity);
+}
 
-  return false;
+bool MrmHandler::isDrivingBackwards()
+{
+  constexpr auto th_moving_backwards = -0.001;
+  return odom_->twist.twist.linear.x < th_moving_backwards;
 }
 
 bool MrmHandler::isEmergency() const