refactor(mrm_emergency_stop_operator): improve readability and change to read topic by polling

system/mrm_emergency_stop_operator/include/mrm_emergency_stop_operator/mrm_emergency_stop_operator_core.hpp

@@ -20,6 +20,8 @@
 #include <memory>
 
 // Autoware
+#include <autoware/universe_utils/ros/polling_subscriber.hpp>
+
 #include <autoware_control_msgs/msg/control.hpp>
 #include <tier4_system_msgs/msg/mrm_behavior_status.hpp>
 #include <tier4_system_msgs/srv/operate_mrm.hpp>
@@ -54,10 +56,11 @@ class MrmEmergencyStopOperator : public rclcpp::Node
   rcl_interfaces::msg::SetParametersResult onParameter(
     const std::vector<rclcpp::Parameter> & parameters);
 
-  // Subscriber
-  rclcpp::Subscription<Control>::SharedPtr sub_control_cmd_;
+  // Subscriber without callback
+  autoware::universe_utils::InterProcessPollingSubscriber<Control> sub_control_cmd_{
+    this, "~/input/control/control_cmd"};
 
-  void onControlCommand(Control::ConstSharedPtr msg);
+  Control subscribeControlCommand();
 
   // Server
   rclcpp::Service<OperateMrm>::SharedPtr service_operation_;
@@ -80,10 +83,9 @@ class MrmEmergencyStopOperator : public rclcpp::Node
   // States
   MrmBehaviorStatus status_;
   Control prev_control_cmd_;
-  bool is_prev_control_cmd_subscribed_;
 
   // Algorithm
-  Control calcTargetAcceleration(const Control & prev_control_cmd) const;
+  Control compensateControlCommand(const Control & prev_control_cmd) const;
 };
 
 }  // namespace mrm_emergency_stop_operator

system/mrm_emergency_stop_operator/src/mrm_emergency_stop_operator/mrm_emergency_stop_operator_core.cpp

@@ -27,11 +27,6 @@
   params_.target_acceleration = declare_parameter<double>("target_acceleration", -2.5);
   params_.target_jerk = declare_parameter<double>("target_jerk", -1.5);
 
-  // Subscriber
-  sub_control_cmd_ = create_subscription<Control>(
-    "~/input/control/control_cmd", 1,
-    std::bind(&MrmEmergencyStopOperator::onControlCommand, this, std::placeholders::_1));
-
   // Server
   service_operation_ = create_service<OperateMrm>(
     "~/input/mrm/emergency_stop/operate", std::bind(
@@ -49,7 +44,6 @@
 
   // Initialize
   status_.state = MrmBehaviorStatus::AVAILABLE;
-  is_prev_control_cmd_subscribed_ = false;
 
   // Parameter Callback
   set_param_res_ = add_on_set_parameters_callback(
@@ -69,12 +63,19 @@
   return result;
 }
 
-void MrmEmergencyStopOperator::onControlCommand(Control::ConstSharedPtr msg)
+Control MrmEmergencyStopOperator::subscribeControlCommand()
 {
-  if (status_.state != MrmBehaviorStatus::OPERATING) {
-    prev_control_cmd_ = *msg;
-    is_prev_control_cmd_subscribed_ = true;
+  auto control_cmd = sub_control_cmd_.takeData();
+  if (control_cmd == nullptr) {
+    auto stop_cmd = Control();
+    stop_cmd.stamp = this->now();
+    stop_cmd.longitudinal.stamp = this->now();
+    stop_cmd.longitudinal.velocity = 0.0;
+    stop_cmd.longitudinal.acceleration = 0.0;
+    stop_cmd.longitudinal.jerk = 0.0;
+    return stop_cmd;
   }
+  return *control_cmd;
 }
 
 void MrmEmergencyStopOperator::operateEmergencyStop(
@@ -103,33 +104,18 @@
 
 void MrmEmergencyStopOperator::onTimer()
 {
-  if (status_.state == MrmBehaviorStatus::OPERATING) {
-    auto control_cmd = calcTargetAcceleration(prev_control_cmd_);
-    publishControlCommand(control_cmd);
-    prev_control_cmd_ = control_cmd;
-  } else {
-    publishControlCommand(prev_control_cmd_);
-  }
+  auto control_cmd = status_.state == MrmBehaviorStatus::OPERATING
+                       ? compensateControlCommand(prev_control_cmd_)
+                       : subscribeControlCommand();
+  publishControlCommand(control_cmd);
+  prev_control_cmd_ = control_cmd;
+
   publishStatus();
 }
 
-Control MrmEmergencyStopOperator::calcTargetAcceleration(const Control & prev_control_cmd) const
+Control MrmEmergencyStopOperator::compensateControlCommand(const Control & prev_control_cmd) const
 {
-  auto control_cmd = Control();
-
-  if (!is_prev_control_cmd_subscribed_) {
-    control_cmd.stamp = this->now();
-    control_cmd.longitudinal.stamp = this->now();
-    control_cmd.longitudinal.velocity = 0.0;
-    control_cmd.longitudinal.acceleration = static_cast<float>(params_.target_acceleration);
-    control_cmd.longitudinal.jerk = 0.0;
-    control_cmd.lateral.stamp = this->now();
-    control_cmd.lateral.steering_tire_angle = 0.0;
-    control_cmd.lateral.steering_tire_rotation_rate = 0.0;
-    return control_cmd;
-  }
-
-  control_cmd = prev_control_cmd;
+  auto control_cmd = prev_control_cmd;
   const auto dt = (this->now() - prev_control_cmd.stamp).seconds();
 
   control_cmd.stamp = this->now();
@@ -147,7 +133,6 @@
 
   return control_cmd;
 }
-
 }  // namespace mrm_emergency_stop_operator
 
 #include <rclcpp_components/register_node_macro.hpp>