diff --git a/control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml b/control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml index 191e894622af7..1fdac3f7a8193 100644 --- a/control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml +++ b/control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml @@ -12,6 +12,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] diff --git a/control/vehicle_cmd_gate/src/adapi_pause_interface.cpp b/control/vehicle_cmd_gate/src/adapi_pause_interface.cpp index afc86f0b4fb20..a607395149c3f 100644 --- a/control/vehicle_cmd_gate/src/adapi_pause_interface.cpp +++ b/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_) { diff --git a/control/vehicle_cmd_gate/src/adapi_pause_interface.hpp b/control/vehicle_cmd_gate/src/adapi_pause_interface.hpp index c4294ee5f643d..47309e834e173 100644 --- a/control/vehicle_cmd_gate/src/adapi_pause_interface.hpp +++ b/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); diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp index e2ee0ec65eec0..f9ef0654aad24 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp @@ -160,6 +160,9 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options) declare_parameter("moderate_stop_service_acceleration"); stop_check_duration_ = declare_parameter("stop_check_duration"); enable_cmd_limit_filter_ = declare_parameter("enable_cmd_limit_filter"); + enable_keep_steering_until_convergence_ = + declare_parameter("enable_keep_steering_until_convergence"); + steering_convergence_threshold_ = declare_parameter("steering_convergence_threshold"); // Vehicle Parameter const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); @@ -423,13 +426,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 @@ -594,6 +609,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; @@ -789,6 +815,12 @@ MarkerArray VehicleCmdGate::createMarkerArray(const IsFilterActivated & filter_a return msg; } +bool VehicleCmdGate::isSteeringConverged(AckermannLateralCommand & lateral_cmd) +{ + return std::fabs(lateral_cmd.steering_tire_angle - current_steer_) < + steering_convergence_threshold_; +} + } // namespace vehicle_cmd_gate #include diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp index 8f0a6aa14d08b..83b6c69601240 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp +++ b/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; @@ -172,6 +174,8 @@ class VehicleCmdGate : public rclcpp::Node double emergency_acceleration_; double moderate_stop_service_acceleration_; bool enable_cmd_limit_filter_; + bool enable_keep_steering_until_convergence_; + double steering_convergence_threshold_; // Service rclcpp::Service::SharedPtr srv_engage_; @@ -213,8 +217,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 prev_time_; double getDt(); AckermannControlCommand getActualStatusAsCommand();