Skip to content

Commit 19b03ae

Browse files
committed
fix(vehicle_cmd_gate): fix the problems while transition from/to stopped state
Signed-off-by: Berkay Karaman <brkay54@gmail.com>
1 parent e063f2d commit 19b03ae

File tree

2 files changed

+19
-2
lines changed

2 files changed

+19
-2
lines changed

control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp

+17-2
Original file line numberDiff line numberDiff line change
@@ -430,13 +430,17 @@ void VehicleCmdGate::publishControlCommands(const Commands & commands)
430430

431431
// Check engage
432432
if (!is_engaged_) {
433-
filtered_commands.control = createStopControlCmd();
433+
filtered_commands.control.longitudinal = createLongitudinalStopControlCmd();
434434
}
435435

436436
// Check pause. Place this check after all other checks as it needs the final output.
437437
adapi_pause_->update(filtered_commands.control);
438438
if (adapi_pause_->is_paused()) {
439-
filtered_commands.control = createStopControlCmd();
439+
if (is_engaged_) {
440+
filtered_commands.control.longitudinal = createLongitudinalStopControlCmd();
441+
} else {
442+
filtered_commands.control = createStopControlCmd();
443+
}
440444
}
441445

442446
// Check if command filtering option is enable
@@ -599,6 +603,17 @@ AckermannControlCommand VehicleCmdGate::createStopControlCmd() const
599603
return cmd;
600604
}
601605

606+
LongitudinalCommand VehicleCmdGate::createLongitudinalStopControlCmd() const
607+
{
608+
LongitudinalCommand cmd;
609+
const auto t = this->now();
610+
cmd.stamp = t;
611+
cmd.speed = 0.0;
612+
cmd.acceleration = stop_hold_acceleration_;
613+
614+
return cmd;
615+
}
616+
602617
AckermannControlCommand VehicleCmdGate::createEmergencyStopControlCmd() const
603618
{
604619
AckermannControlCommand cmd;

control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp

+2
Original file line numberDiff line numberDiff line change
@@ -55,6 +55,7 @@ namespace vehicle_cmd_gate
5555
using autoware_adapi_v1_msgs::msg::MrmState;
5656
using autoware_adapi_v1_msgs::msg::OperationModeState;
5757
using autoware_auto_control_msgs::msg::AckermannControlCommand;
58+
using autoware_auto_control_msgs::msg::LongitudinalCommand;
5859
using autoware_auto_vehicle_msgs::msg::GearCommand;
5960
using autoware_auto_vehicle_msgs::msg::HazardLightsCommand;
6061
using autoware_auto_vehicle_msgs::msg::SteeringReport;
@@ -220,6 +221,7 @@ class VehicleCmdGate : public rclcpp::Node
220221
// Algorithm
221222
AckermannControlCommand prev_control_cmd_;
222223
AckermannControlCommand createStopControlCmd() const;
224+
LongitudinalCommand createLongitudinalStopControlCmd() const;
223225
AckermannControlCommand createEmergencyStopControlCmd() const;
224226

225227
std::shared_ptr<rclcpp::Time> prev_time_;

0 commit comments

Comments
 (0)