refactor(mrm_handler): delete control_cmd publish function (autowaref…

…oundation#6514) * refactor(mrm_handler): delete control_cmd publish function Signed-off-by: veqcc <[email protected]>
Interplai · Jun 3, 2024 · 4e7f0da · 4e7f0da
1 parent f417fc9
commit 4e7f0da
Show file tree

Hide file tree

Showing 2 changed files with 23 additions and 29 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
@@ -104,9 +104,8 @@ class MrmHandler : public rclcpp::Node
     pub_hazard_cmd_;
   rclcpp::Publisher<autoware_auto_vehicle_msgs::msg::GearCommand>::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<autoware_adapi_v1_msgs::msg::MrmState>::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
@@ -161,48 +161,39 @@ 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();
-
+  msg.stamp = this->now();
   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()
@@ -380,17 +371,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)