Commit 19b03ae 1 parent e063f2d commit 19b03ae Copy full SHA for 19b03ae
File tree 2 files changed +19
-2
lines changed
control/vehicle_cmd_gate/src
2 files changed +19
-2
lines changed Original file line number Diff line number Diff line change @@ -430,13 +430,17 @@ void VehicleCmdGate::publishControlCommands(const Commands & commands)
430
430
431
431
// Check engage
432
432
if (!is_engaged_) {
433
- filtered_commands.control = createStopControlCmd ();
433
+ filtered_commands.control . longitudinal = createLongitudinalStopControlCmd ();
434
434
}
435
435
436
436
// Check pause. Place this check after all other checks as it needs the final output.
437
437
adapi_pause_->update (filtered_commands.control );
438
438
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
+ }
440
444
}
441
445
442
446
// Check if command filtering option is enable
@@ -599,6 +603,17 @@ AckermannControlCommand VehicleCmdGate::createStopControlCmd() const
599
603
return cmd;
600
604
}
601
605
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
+
602
617
AckermannControlCommand VehicleCmdGate::createEmergencyStopControlCmd () const
603
618
{
604
619
AckermannControlCommand cmd;
Original file line number Diff line number Diff line change @@ -55,6 +55,7 @@ namespace vehicle_cmd_gate
55
55
using autoware_adapi_v1_msgs::msg::MrmState;
56
56
using autoware_adapi_v1_msgs::msg::OperationModeState;
57
57
using autoware_auto_control_msgs::msg::AckermannControlCommand;
58
+ using autoware_auto_control_msgs::msg::LongitudinalCommand;
58
59
using autoware_auto_vehicle_msgs::msg::GearCommand;
59
60
using autoware_auto_vehicle_msgs::msg::HazardLightsCommand;
60
61
using autoware_auto_vehicle_msgs::msg::SteeringReport;
@@ -220,6 +221,7 @@ class VehicleCmdGate : public rclcpp::Node
220
221
// Algorithm
221
222
AckermannControlCommand prev_control_cmd_;
222
223
AckermannControlCommand createStopControlCmd () const ;
224
+ LongitudinalCommand createLongitudinalStopControlCmd () const ;
223
225
AckermannControlCommand createEmergencyStopControlCmd () const ;
224
226
225
227
std::shared_ptr<rclcpp::Time> prev_time_;
You can’t perform that action at this time.
0 commit comments