From 50d368cbc8793b2e5e11ebb45a19d6df0a479b2a Mon Sep 17 00:00:00 2001 From: veqcc Date: Thu, 29 Feb 2024 14:13:50 +0900 Subject: [PATCH 1/3] refactor(mrm_handler): delete control_cmd publish function Signed-off-by: veqcc --- .../include/mrm_handler/mrm_handler_core.hpp | 5 +- .../src/mrm_handler/mrm_handler_core.cpp | 48 ++++++++----------- 2 files changed, 22 insertions(+), 31 deletions(-) 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 8a6ec81cbafff..e070a274bd98c 100644 --- a/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp +++ b/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp @@ -106,9 +106,8 @@ class MrmHandler : public rclcpp::Node pub_hazard_cmd_; rclcpp::Publisher::SharedPtr pub_gear_cmd_; - autoware_auto_vehicle_msgs::msg::HazardLightsCommand createHazardCmdMsg(); - autoware_auto_vehicle_msgs::msg::GearCommand createGearCmdMsg(); - void publishControlCommands(); + void publishHazardCmd(); + void publishGearCmd(); rclcpp::Publisher::SharedPtr pub_mrm_state_; 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 976f5b3164abd..363da2aa1b154 100644 --- a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp +++ b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp @@ -163,48 +163,36 @@ void MrmHandler::onOperationModeState( operation_mode_state_ = msg; } -autoware_auto_vehicle_msgs::msg::HazardLightsCommand MrmHandler::createHazardCmdMsg() -{ +void MrmHandler::publishHazardCmd() { using autoware_auto_vehicle_msgs::msg::HazardLightsCommand; HazardLightsCommand msg; - // Check emergency - const bool is_emergency = isEmergency(); - if (is_emergency_holding_) { // turn hazard on during emergency holding msg.command = HazardLightsCommand::ENABLE; - } else if (is_emergency && param_.turning_hazard_on.emergency) { + } else if (isEmergency() && param_.turning_hazard_on.emergency) { // turn hazard on if vehicle is in emergency state and // turning hazard on if emergency flag is true msg.command = HazardLightsCommand::ENABLE; } else { msg.command = HazardLightsCommand::NO_COMMAND; } - return msg; + + pub_hazard_cmd_->publish(msg); } -void MrmHandler::publishControlCommands() -{ +void MrmHandler::publishGearCmd() { using autoware_auto_vehicle_msgs::msg::GearCommand; + GearCommand msg; - // Create timestamp - const auto stamp = this->now(); - - // Publish hazard command - pub_hazard_cmd_->publish(createHazardCmdMsg()); - - // Publish gear - { - GearCommand msg; - msg.stamp = stamp; - if (param_.use_parking_after_stopped && isStopped()) { - msg.command = GearCommand::PARK; - } else { - msg.command = GearCommand::DRIVE; - } - pub_gear_cmd_->publish(msg); + msg.stamp = this->now(); + if (param_.use_parking_after_stopped && isStopped()) { + msg.command = GearCommand::PARK; + } else { + msg.command = GearCommand::DRIVE; } + + pub_gear_cmd_->publish(msg); } void MrmHandler::publishMrmState() @@ -382,17 +370,21 @@ void MrmHandler::onTimer() this->get_logger(), *this->get_clock(), std::chrono::milliseconds(1000).count(), "heartbeat operation_mode_availability is timeout"); mrm_state_.state = autoware_adapi_v1_msgs::msg::MrmState::MRM_OPERATING; - publishControlCommands(); + publishHazardCmd(); + publishGearCmd(); return; } // Update Emergency State updateMrmState(); - // Publish control commands - publishControlCommands(); + // Operate MRM operateMrm(); + + // Publish publishMrmState(); + publishHazardCmd(); + publishGearCmd(); } void MrmHandler::transitionTo(const int new_state) From 9d1f6ee41d08a9bc57d5f00c3fec4199a0874f37 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 29 Feb 2024 05:19:06 +0000 Subject: [PATCH 2/3] style(pre-commit): autofix --- system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) 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 363da2aa1b154..78aeb354ecab3 100644 --- a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp +++ b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp @@ -163,7 +163,8 @@ void MrmHandler::onOperationModeState( operation_mode_state_ = msg; } -void MrmHandler::publishHazardCmd() { +void MrmHandler::publishHazardCmd() +{ using autoware_auto_vehicle_msgs::msg::HazardLightsCommand; HazardLightsCommand msg; @@ -181,7 +182,8 @@ void MrmHandler::publishHazardCmd() { pub_hazard_cmd_->publish(msg); } -void MrmHandler::publishGearCmd() { +void MrmHandler::publishGearCmd() +{ using autoware_auto_vehicle_msgs::msg::GearCommand; GearCommand msg; From cf11633bff1e65617961de657cfc1110b0835be5 Mon Sep 17 00:00:00 2001 From: veqcc Date: Fri, 1 Mar 2024 17:52:40 +0900 Subject: [PATCH 3/3] add timestamp to hazard_light_command topic Signed-off-by: veqcc --- system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp | 1 + 1 file changed, 1 insertion(+) 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 78aeb354ecab3..8aff0421d4f1f 100644 --- a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp +++ b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp @@ -168,6 +168,7 @@ void MrmHandler::publishHazardCmd() using autoware_auto_vehicle_msgs::msg::HazardLightsCommand; HazardLightsCommand msg; + msg.stamp = this->now(); if (is_emergency_holding_) { // turn hazard on during emergency holding msg.command = HazardLightsCommand::ENABLE;