@@ -130,19 +130,20 @@ void MrmHandler::publishGearCmd()
130
130
{
131
131
using autoware_vehicle_msgs::msg::GearCommand;
132
132
GearCommand msg;
133
-
134
133
msg.stamp = this ->now ();
135
- const auto command = [&]() {
136
- // If stopped and use_parking is not true, send the last gear command
137
- if (isStopped ())
138
- return (param_.use_parking_after_stopped ) ? GearCommand::PARK : last_gear_command_;
139
- return (isDrivingBackwards ()) ? GearCommand::REVERSE : GearCommand::DRIVE;
140
- }();
141
-
142
- msg.command = command;
143
- last_gear_command_ = msg.command ;
134
+
135
+ if (isEmergency ()) {
136
+ // gear command is created within mrm_handler
137
+ msg.command =
138
+ (param_.use_parking_after_stopped && isStopped ()) ? GearCommand::PARK : last_gear_command_;
139
+ } else {
140
+ // use the same gear as the input gear
141
+ auto gear = sub_gear_cmd_.takeData ();
142
+ msg.command = (gear == nullptr ) ? last_gear_command_ : gear->command ;
143
+ last_gear_command_ = msg.command ;
144
+ }
145
+
144
146
pub_gear_cmd_->publish (msg);
145
- return ;
146
147
}
147
148
148
149
void MrmHandler::publishMrmState ()
@@ -524,14 +525,6 @@ bool MrmHandler::isStopped()
524
525
return (std::abs (odom->twist .twist .linear .x ) < th_stopped_velocity);
525
526
}
526
527
527
- bool MrmHandler::isDrivingBackwards ()
528
- {
529
- auto odom = sub_odom_.takeData ();
530
- if (odom == nullptr ) return false ;
531
- constexpr auto th_moving_backwards = -0.001 ;
532
- return odom->twist .twist .linear .x < th_moving_backwards;
533
- }
534
-
535
528
bool MrmHandler::isEmergency () const
536
529
{
537
530
return !operation_mode_availability_->autonomous || is_emergency_holding_ ||
0 commit comments