From d48b060f9b1d791de2c063953c16708f19b66054 Mon Sep 17 00:00:00 2001 From: Daniel Sanchez Date: Wed, 5 Jun 2024 09:33:41 +0900 Subject: [PATCH] add gear history Signed-off-by: Daniel Sanchez --- .../emergency_handler_core.hpp | 1 + .../emergency_handler_core.cpp | 25 +++++++------------ .../include/mrm_handler/mrm_handler_core.hpp | 1 + .../src/mrm_handler/mrm_handler_core.cpp | 25 +++++++------------ 4 files changed, 20 insertions(+), 32 deletions(-) diff --git a/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp b/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp index 532a837a18422..72eb549ebf012 100644 --- a/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp +++ b/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp @@ -135,6 +135,7 @@ class EmergencyHandler : public rclcpp::Node void checkHazardStatusTimeout(); // Algorithm + uint8_t last_gear_command_{autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE}; void transitionTo(const int new_state); void updateMrmState(); void operateMrm(); diff --git a/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp b/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp index d711f8eb22170..da9d18deb3079 100644 --- a/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp +++ b/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp @@ -167,22 +167,15 @@ void EmergencyHandler::publishControlCommands() { GearCommand msg; msg.stamp = stamp; - - if (isStopped()) { - if (param_.use_parking_after_stopped) { - msg.command = GearCommand::PARK; - pub_gear_cmd_->publish(msg); - } - return; - } - - if (isDrivingBackwards()) { - msg.command = GearCommand::REVERSE; - pub_gear_cmd_->publish(msg); - return; - } - - msg.command = GearCommand::DRIVE; + 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; pub_gear_cmd_->publish(msg); return; } 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 d51ba3030d3d2..50788ce9c2034 100644 --- a/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp +++ b/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp @@ -149,6 +149,7 @@ class MrmHandler : public rclcpp::Node // Algorithm bool is_emergency_holding_ = false; + uint8_t last_gear_command_{autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE}; void transitionTo(const int new_state); void updateMrmState(); void operateMrm(); 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 3b4c450a52ae2..0d4e3c4542821 100644 --- a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp +++ b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp @@ -191,22 +191,15 @@ void MrmHandler::publishGearCmd() GearCommand msg; msg.stamp = this->now(); - - if (isStopped()) { - if (param_.use_parking_after_stopped) { - msg.command = GearCommand::PARK; - pub_gear_cmd_->publish(msg); - } - return; - } - - if (isDrivingBackwards()) { - msg.command = GearCommand::REVERSE; - pub_gear_cmd_->publish(msg); - return; - } - - msg.command = GearCommand::DRIVE; + 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; pub_gear_cmd_->publish(msg); return; }