Skip to content

Commit

Permalink
fix(vehicle_cmd_gate): fix the problems while transition from/to stop…
Browse files Browse the repository at this point in the history
…ped state

Signed-off-by: Berkay Karaman <[email protected]>
  • Loading branch information
brkay54 committed Oct 1, 2023
1 parent e337576 commit ae9d1d6
Show file tree
Hide file tree
Showing 5 changed files with 48 additions and 3 deletions.
2 changes: 2 additions & 0 deletions control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -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]
Expand Down
5 changes: 5 additions & 0 deletions control/vehicle_cmd_gate/src/adapi_pause_interface.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -34,6 +34,11 @@ bool AdapiPauseInterface::is_paused()
return is_paused_;
}

bool AdapiPauseInterface::is_start_requested()

Check warning on line 37 in control/vehicle_cmd_gate/src/adapi_pause_interface.cpp

View check run for this annotation

Codecov / codecov/patch

control/vehicle_cmd_gate/src/adapi_pause_interface.cpp#L37

Added line #L37 was not covered by tests
{
return is_start_requested_;

Check warning on line 39 in control/vehicle_cmd_gate/src/adapi_pause_interface.cpp

View check run for this annotation

Codecov / codecov/patch

control/vehicle_cmd_gate/src/adapi_pause_interface.cpp#L39

Added line #L39 was not covered by tests
}

void AdapiPauseInterface::publish()
{
if (prev_is_paused_ != is_paused_) {
Expand Down
1 change: 1 addition & 0 deletions control/vehicle_cmd_gate/src/adapi_pause_interface.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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);

Expand Down
36 changes: 34 additions & 2 deletions control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -160,6 +160,9 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options)
declare_parameter<double>("moderate_stop_service_acceleration");
stop_check_duration_ = declare_parameter<double>("stop_check_duration");
enable_cmd_limit_filter_ = declare_parameter<bool>("enable_cmd_limit_filter");
enable_keep_steering_until_convergence_ =
declare_parameter<bool>("enable_keep_steering_until_convergence");
steering_convergence_threshold_ = declare_parameter<double>("steering_convergence_threshold");

Check warning on line 165 in control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ Getting worse: Large Method

VehicleCmdGate::VehicleCmdGate increases from 152 to 155 lines of code, threshold = 70. Large functions with many lines of code are generally harder to understand and lower the code health. Avoid adding more lines to this function.

// Vehicle Parameter
const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo();
Expand Down Expand Up @@ -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 warning on line 430 in control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp

View check run for this annotation

Codecov / codecov/patch

control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp#L430

Added line #L430 was not covered by tests
}

// 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();

Check warning on line 438 in control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp

View check run for this annotation

Codecov / codecov/patch

control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp#L438

Added line #L438 was not covered by tests
} else {
filtered_commands.control = createStopControlCmd();

Check warning on line 440 in control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp

View check run for this annotation

Codecov / codecov/patch

control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp#L440

Added line #L440 was not covered by tests
}
} 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 warning on line 447 in control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ Getting worse: Complex Method

VehicleCmdGate::publishControlCommands increases in cyclomatic complexity from 12 to 16, threshold = 9. This function has many conditional statements (e.g. if, for, while), leading to lower code health. Avoid adding more conditionals and code to it without refactoring.

Check warning on line 447 in control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ New issue: Bumpy Road Ahead

VehicleCmdGate::publishControlCommands has 2 blocks with nested conditional logic. Any nesting of 2 or deeper is considered. Threshold is one single, nested block per function. The Bumpy Road code smell is a function that contains multiple chunks of nested conditional logic. The deeper the nesting and the more bumps, the lower the code health.
}

// Check if command filtering option is enable
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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 <rclcpp_components/register_node_macro.hpp>
Expand Down
7 changes: 6 additions & 1 deletion control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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<EngageSrv>::SharedPtr srv_engage_;
Expand Down Expand Up @@ -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<rclcpp::Time> prev_time_;
double getDt();
AckermannControlCommand getActualStatusAsCommand();
Expand Down

0 comments on commit ae9d1d6

Please sign in to comment.