diff --git a/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp b/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp index 7d7deb8c4a504..f73d0df4153ce 100644 --- a/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp +++ b/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp @@ -86,6 +86,8 @@ class MrmHandler : public rclcpp::Node autoware::universe_utils::InterProcessPollingSubscriber< autoware_adapi_v1_msgs::msg::OperationModeState> sub_operation_mode_state_{this, "~/input/api/operation_mode/state"}; + autoware::universe_utils::InterProcessPollingSubscriber + sub_gear_cmd_{this, "~/input/gear"}; tier4_system_msgs::msg::OperationModeAvailability::ConstSharedPtr operation_mode_availability_; @@ -146,7 +148,6 @@ class MrmHandler : public rclcpp::Node void handleFailedRequest(); autoware_adapi_v1_msgs::msg::MrmState::_behavior_type getCurrentMrmBehavior(); bool isStopped(); - bool isDrivingBackwards(); bool isEmergency() const; bool isControlModeAutonomous(); bool isOperationModeAutonomous(); diff --git a/system/mrm_handler/launch/mrm_handler.launch.xml b/system/mrm_handler/launch/mrm_handler.launch.xml index 7e761157956df..c99b22e10ad77 100644 --- a/system/mrm_handler/launch/mrm_handler.launch.xml +++ b/system/mrm_handler/launch/mrm_handler.launch.xml @@ -7,6 +7,7 @@ + @@ -26,6 +27,7 @@ + diff --git a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp index 47cbb0ae7d016..bac932a7d7e1e 100644 --- a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp +++ b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp @@ -130,19 +130,20 @@ void MrmHandler::publishGearCmd() { using autoware_vehicle_msgs::msg::GearCommand; GearCommand msg; - msg.stamp = this->now(); - const auto command = [&]() { - // If stopped and use_parking is not true, send the last gear command - if (isStopped()) - return (param_.use_parking_after_stopped) ? GearCommand::PARK : last_gear_command_; - return (isDrivingBackwards()) ? GearCommand::REVERSE : GearCommand::DRIVE; - }(); - - msg.command = command; - last_gear_command_ = msg.command; + + if (isEmergency()) { + // gear command is created within mrm_handler + msg.command = + (param_.use_parking_after_stopped && isStopped()) ? GearCommand::PARK : last_gear_command_; + } else { + // use the same gear as the input gear + auto gear = sub_gear_cmd_.takeData(); + msg.command = (gear == nullptr) ? last_gear_command_ : gear->command; + last_gear_command_ = msg.command; + } + pub_gear_cmd_->publish(msg); - return; } void MrmHandler::publishMrmState() @@ -524,14 +525,6 @@ bool MrmHandler::isStopped() return (std::abs(odom->twist.twist.linear.x) < th_stopped_velocity); } -bool MrmHandler::isDrivingBackwards() -{ - auto odom = sub_odom_.takeData(); - if (odom == nullptr) return false; - constexpr auto th_moving_backwards = -0.001; - return odom->twist.twist.linear.x < th_moving_backwards; -} - bool MrmHandler::isEmergency() const { return !operation_mode_availability_->autonomous || is_emergency_holding_ ||