fix(vehicle_cmd_gate): fix the problems while transition from/to stop…

…ped state (autowarefoundation#5183) (#1328) Co-authored-by: Berkay Karaman <[email protected]>
tier4 · Jun 7, 2024 · fed4253 · fed4253
1 parent 6664ec5
commit fed4253
Show file tree

Hide file tree

Showing 2 changed files with 19 additions and 2 deletions.
diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp
@@ -430,13 +430,17 @@ void VehicleCmdGate::publishControlCommands(const Commands & commands)
 
   // Check engage
   if (!is_engaged_) {
-    filtered_commands.control = createStopControlCmd();
+    filtered_commands.control.longitudinal = createLongitudinalStopControlCmd();
   }
 
   // Check pause. Place this check after all other checks as it needs the final output.
   adapi_pause_->update(filtered_commands.control);
   if (adapi_pause_->is_paused()) {
-    filtered_commands.control = createStopControlCmd();
+    if (is_engaged_) {
+      filtered_commands.control.longitudinal = createLongitudinalStopControlCmd();
+    } else {
+      filtered_commands.control = createStopControlCmd();
+    }
   }
 
   // Check if command filtering option is enable
@@ -599,6 +603,17 @@ AckermannControlCommand VehicleCmdGate::createStopControlCmd() const
   return cmd;
 }
 
+LongitudinalCommand VehicleCmdGate::createLongitudinalStopControlCmd() const
+{
+  LongitudinalCommand cmd;
+  const auto t = this->now();
+  cmd.stamp = t;
+  cmd.speed = 0.0;
+  cmd.acceleration = stop_hold_acceleration_;
+
+  return cmd;
+}
+
 AckermannControlCommand VehicleCmdGate::createEmergencyStopControlCmd() const
 {
   AckermannControlCommand cmd;

diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp
@@ -55,6 +55,7 @@ namespace vehicle_cmd_gate
 using autoware_adapi_v1_msgs::msg::MrmState;
 using autoware_adapi_v1_msgs::msg::OperationModeState;
 using autoware_auto_control_msgs::msg::AckermannControlCommand;
+using autoware_auto_control_msgs::msg::LongitudinalCommand;
 using autoware_auto_vehicle_msgs::msg::GearCommand;
 using autoware_auto_vehicle_msgs::msg::HazardLightsCommand;
 using autoware_auto_vehicle_msgs::msg::SteeringReport;
@@ -220,6 +221,7 @@ class VehicleCmdGate : public rclcpp::Node
   // Algorithm
   AckermannControlCommand prev_control_cmd_;
   AckermannControlCommand createStopControlCmd() const;
+  LongitudinalCommand createLongitudinalStopControlCmd() const;
   AckermannControlCommand createEmergencyStopControlCmd() const;
 
   std::shared_ptr<rclcpp::Time> prev_time_;