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 7ad685217f13d..552ce7a42a01d 100644 --- a/control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml +++ b/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] 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 1bed4ee67f412..967f207004a58 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp +++ b/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("filter_activated_count_threshold"); filter_activated_velocity_threshold_ = declare_parameter("filter_activated_velocity_threshold"); + 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(); @@ -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 diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp index c36aab240ae79..20a9fc8b7cdeb 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; @@ -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::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 prev_time_; double getDt(); AckermannControlCommand getActualStatusAsCommand();