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

…ped state

Signed-off-by: Berkay Karaman <[email protected]>

control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml

@@ -14,6 +14,8 @@
     moderate_stop_service_acceleration: -1.5
     stopped_state_entry_duration_time: 0.1
     stop_check_duration: 1.0
+    enable_keep_steering_until_convergence: true
+    steering_convergence_threshold: 0.01 # [rad]
     nominal:
       vel_lim: 25.0
       reference_speed_points: [20.0, 30.0]

control/vehicle_cmd_gate/src/adapi_pause_interface.cpp

@@ -34,6 +34,11 @@ bool AdapiPauseInterface::is_paused()
   return is_paused_;
 }
 
+bool AdapiPauseInterface::is_start_requested()
+{
+  return is_start_requested_;
+}
+
 void AdapiPauseInterface::publish()
 {
   if (prev_is_paused_ != is_paused_) {

control/vehicle_cmd_gate/src/adapi_pause_interface.hpp

@@ -36,6 +36,7 @@ class AdapiPauseInterface
 public:
   explicit AdapiPauseInterface(rclcpp::Node * node);
   bool is_paused();
+  bool is_start_requested();
   void publish();
   void update(const AckermannControlCommand & control);
 

control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp

@@ -167,6 +167,9 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options)
   filter_activated_count_threshold_ = declare_parameter<int>("filter_activated_count_threshold");
   filter_activated_velocity_threshold_ =
     declare_parameter<double>("filter_activated_velocity_threshold");
+  enable_keep_steering_until_convergence_ =
+    declare_parameter<bool>("enable_keep_steering_until_convergence");
+  steering_convergence_threshold_ = declare_parameter<double>("steering_convergence_threshold");
 
   // Vehicle Parameter
   const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo();
@@ -430,13 +433,25 @@ void VehicleCmdGate::publishControlCommands(const Commands & commands)
 
   // Check engage
   if (!is_engaged_) {
-    filtered_commands.control = createStopControlCmd();
+    // TODO(brkay54): Connect the engage state to moderate_stop_interface
+    filtered_commands.control.longitudinal = createStopTransitionControlCmd();
   }
 
   // Check pause. Place this check after all other checks as it needs the final output.
+  constexpr double stopped_vel_th = 1e-2;
   adapi_pause_->update(filtered_commands.control);
   if (adapi_pause_->is_paused()) {
-    filtered_commands.control = createStopControlCmd();
+    if (adapi_pause_->is_start_requested()) {
+      filtered_commands.control.longitudinal = createStopTransitionControlCmd();
+    } else {
+      filtered_commands.control = createStopControlCmd();
+    }
+  } else if (enable_keep_steering_until_convergence_) {
+    if (fabs(current_kinematics_.twist.twist.linear.x) < stopped_vel_th) {
+      if (!isSteeringConverged(filtered_commands.control.lateral)) {
+        filtered_commands.control.longitudinal = createStopTransitionControlCmd();
+      }
+    }
   }
 
   // Check if command filtering option is enable
@@ -599,6 +614,17 @@ AckermannControlCommand VehicleCmdGate::createStopControlCmd() const
   return cmd;
 }
 
+LongitudinalCommand VehicleCmdGate::createStopTransitionControlCmd() const
+{
+  LongitudinalCommand longitudinal_cmd;
+  const auto t = this->now();
+  longitudinal_cmd.stamp = t;
+  longitudinal_cmd.speed = 0.0;
+  longitudinal_cmd.acceleration = stop_hold_acceleration_;
+
+  return longitudinal_cmd;
+}
+
 AckermannControlCommand VehicleCmdGate::createEmergencyStopControlCmd() const
 {
   AckermannControlCommand cmd;
@@ -816,6 +842,12 @@ void VehicleCmdGate::publishMarkers(const IsFilterActivated & filter_activated)
   filter_activated_flag_pub_->publish(filter_activated_flag);
   filter_activated_marker_raw_pub_->publish(createMarkerArray(filter_activated));
 }
+bool VehicleCmdGate::isSteeringConverged(AckermannLateralCommand & lateral_cmd)
+{
+  return std::fabs(lateral_cmd.steering_tire_angle - current_steer_) <
+         steering_convergence_threshold_;
+}
+
 }  // namespace vehicle_cmd_gate
 
 #include <rclcpp_components/register_node_macro.hpp>

control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp

@@ -55,6 +55,8 @@ 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::AckermannLateralCommand;
+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;
@@ -179,6 +181,8 @@ class VehicleCmdGate : public rclcpp::Node
   bool enable_cmd_limit_filter_;
   int filter_activated_count_threshold_;
   double filter_activated_velocity_threshold_;
+  bool enable_keep_steering_until_convergence_;
+  double steering_convergence_threshold_;
 
   // Service
   rclcpp::Service<EngageSrv>::SharedPtr srv_engage_;
@@ -220,8 +224,9 @@ class VehicleCmdGate : public rclcpp::Node
   // Algorithm
   AckermannControlCommand prev_control_cmd_;
   AckermannControlCommand createStopControlCmd() const;
+  LongitudinalCommand createStopTransitionControlCmd() const;
   AckermannControlCommand createEmergencyStopControlCmd() const;
-
+  bool isSteeringConverged(AckermannLateralCommand & lateral_cmd);
   std::shared_ptr<rclcpp::Time> prev_time_;
   double getDt();
   AckermannControlCommand getActualStatusAsCommand();