check for ego speed when determining the gear command

Signed-off-by: Daniel Sanchez <[email protected]>
autowarefoundation · Jun 4, 2024 · 96b6321 · 96b6321
1 parent 19ea101
commit 96b6321
Show file tree

Hide file tree

Showing 4 changed files with 45 additions and 16 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
@@ -140,6 +140,7 @@ class EmergencyHandler : public rclcpp::Node
   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
@@ -167,12 +167,24 @@ void EmergencyHandler::publishControlCommands()
   {
     GearCommand msg;
     msg.stamp = stamp;
-    if (param_.use_parking_after_stopped && isStopped()) {
-      msg.command = GearCommand::PARK;
-    } else {
-      msg.command = GearCommand::DRIVE;
+
+    if (isStopped()) {
+      if (param_.use_parking_after_stopped) {
+        msg.command = GearCommand::PARK;
+        pub_gear_cmd_->publish(msg);
+      }
+      return;
+    }
+
+    if (isDrivingBackwards()) {
+      msg.command = GearCommand::REVERSE;
+      pub_gear_cmd_->publish(msg);
+      return;
     }
+
+    msg.command = GearCommand::DRIVE;
     pub_gear_cmd_->publish(msg);
+    return;
   }
 }
 
@@ -454,11 +466,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
@@ -155,6 +155,7 @@ class MrmHandler : public rclcpp::Node
   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,24 @@ 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;
+
+  if (isStopped()) {
+    if (param_.use_parking_after_stopped) {
+      msg.command = GearCommand::PARK;
+      pub_gear_cmd_->publish(msg);
+    }
+    return;
+  }
+
+  if (isDrivingBackwards()) {
+    msg.command = GearCommand::REVERSE;
+    pub_gear_cmd_->publish(msg);
+    return;
   }
 
+  msg.command = GearCommand::DRIVE;
   pub_gear_cmd_->publish(msg);
+  return;
 }
 
 void MrmHandler::publishMrmState()
@@ -578,11 +589,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