fix(vehicle_cmd_gate): put back subscriber rather than using polling subsriber (#7500)

put back polling subscribers to subscribers in neccesary cases

Signed-off-by: Go Sakayori <go.sakayori@tier4.jp>
Signed-off-by: Go Sakayori <gsakayori@gmail.com>

Author: go-sakayori

control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp

@@ -87,6 +87,37 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options)
   external_emergency_stop_heartbeat_sub_ = create_subscription<Heartbeat>(
     "input/external_emergency_stop_heartbeat", 1,
     std::bind(&VehicleCmdGate::onExternalEmergencyStopHeartbeat, this, _1));
+  gate_mode_sub_ = create_subscription<GateMode>(
+    "input/gate_mode", 1, std::bind(&VehicleCmdGate::onGateMode, this, _1));
+  engage_sub_ = create_subscription<EngageMsg>(
+    "input/engage", 1, std::bind(&VehicleCmdGate::onEngage, this, _1));
+  kinematics_sub_ = create_subscription<Odometry>(
+    "/localization/kinematic_state", 1,
+    [this](Odometry::SharedPtr msg) { current_kinematics_ = *msg; });
+  acc_sub_ = create_subscription<AccelWithCovarianceStamped>(
+    "input/acceleration", 1, [this](AccelWithCovarianceStamped::SharedPtr msg) {
+      current_acceleration_ = msg->accel.accel.linear.x;
+    });
+  steer_sub_ = create_subscription<SteeringReport>(
+    "input/steering", 1,
+    [this](SteeringReport::SharedPtr msg) { current_steer_ = msg->steering_tire_angle; });
+  operation_mode_sub_ = create_subscription<OperationModeState>(
+    "input/operation_mode", rclcpp::QoS(1).transient_local(),
+    [this](const OperationModeState::SharedPtr msg) { current_operation_mode_ = *msg; });
+  mrm_state_sub_ = create_subscription<MrmState>(
+    "input/mrm_state", 1, std::bind(&VehicleCmdGate::onMrmState, this, _1));
+
+  // Subscriber for auto
+  auto_control_cmd_sub_ = create_subscription<Control>(
+    "input/auto/control_cmd", 1, std::bind(&VehicleCmdGate::onAutoCtrlCmd, this, _1));
+
+  // Subscriber for external
+  remote_control_cmd_sub_ = create_subscription<Control>(
+    "input/external/control_cmd", 1, std::bind(&VehicleCmdGate::onRemoteCtrlCmd, this, _1));
+
+  // Subscriber for emergency
+  emergency_control_cmd_sub_ = create_subscription<Control>(
+    "input/emergency/control_cmd", 1, std::bind(&VehicleCmdGate::onEmergencyCtrlCmd, this, _1));
 
   // Parameter
   use_emergency_handling_ = declare_parameter<bool>("use_emergency_handling");
@@ -291,32 +322,29 @@ bool VehicleCmdGate::isDataReady()
 }
 
 // for auto
-void VehicleCmdGate::onAutoCtrlCmd()
+void VehicleCmdGate::onAutoCtrlCmd(Control::ConstSharedPtr msg)
 {
-  const auto msg = auto_control_cmd_sub_.takeData();
-  if (msg) auto_commands_.control = *msg;
+  auto_commands_.control = *msg;
 
   if (current_gate_mode_.data == GateMode::AUTO) {
     publishControlCommands(auto_commands_);
   }
 }
 
 // for remote
-void VehicleCmdGate::onRemoteCtrlCmd()
+void VehicleCmdGate::onRemoteCtrlCmd(Control::ConstSharedPtr msg)
 {
-  const auto msg = remote_control_cmd_sub_.takeData();
-  if (msg) remote_commands_.control = *msg;
+  remote_commands_.control = *msg;
 
   if (current_gate_mode_.data == GateMode::EXTERNAL) {
     publishControlCommands(remote_commands_);
   }
 }
 
 // for emergency
-void VehicleCmdGate::onEmergencyCtrlCmd()
+void VehicleCmdGate::onEmergencyCtrlCmd(Control::ConstSharedPtr msg)
 {
-  const auto msg = emergency_control_cmd_sub_.takeData();
-  if (msg) emergency_commands_.control = *msg;
+  emergency_commands_.control = *msg;
 
   if (use_emergency_handling_ && is_system_emergency_) {
     publishControlCommands(emergency_commands_);
@@ -325,25 +353,7 @@ void VehicleCmdGate::onEmergencyCtrlCmd()
 
 void VehicleCmdGate::onTimer()
 {
-  onGateMode();
-
-  const auto msg_kinematics = kinematics_sub_.takeData();
-  if (msg_kinematics) current_kinematics_ = *msg_kinematics;
-
-  const auto msg_acceleration = acc_sub_.takeData();
-  if (msg_acceleration) current_acceleration_ = msg_acceleration->accel.accel.linear.x;
-
-  const auto msg_steer = steer_sub_.takeData();
-  if (msg_steer) current_steer_ = msg_steer->steering_tire_angle;
-
-  const auto msg_operation_mode = operation_mode_sub_.takeData();
-  if (msg_operation_mode) current_operation_mode_ = *msg_operation_mode;
-
-  onMrmState();
-
   // Subscriber for auto
-  onAutoCtrlCmd();
-
   const auto msg_auto_command_turn_indicator = auto_turn_indicator_cmd_sub_.takeData();
   if (msg_auto_command_turn_indicator)
     auto_commands_.turn_indicator = *msg_auto_command_turn_indicator;
@@ -355,8 +365,6 @@ void VehicleCmdGate::onTimer()
   if (msg_auto_command_gear) auto_commands_.gear = *msg_auto_command_gear;
 
   // Subscribe for external
-  onRemoteCtrlCmd();
-
   const auto msg_remote_command_turn_indicator = remote_turn_indicator_cmd_sub_.takeData();
   if (msg_remote_command_turn_indicator)
     remote_commands_.turn_indicator = *msg_remote_command_turn_indicator;
@@ -369,17 +377,13 @@ void VehicleCmdGate::onTimer()
   if (msg_remote_command_gear) remote_commands_.gear = *msg_remote_command_gear;
 
   // Subscribe for emergency
-  onEmergencyCtrlCmd();
-
   const auto msg_emergency_command_hazard_light = emergency_hazard_light_cmd_sub_.takeData();
   if (msg_emergency_command_hazard_light)
     emergency_commands_.hazard_light = *msg_emergency_command_hazard_light;
 
   const auto msg_emergency_command_gear = emergency_gear_cmd_sub_.takeData();
   if (msg_emergency_command_gear) emergency_commands_.gear = *msg_emergency_command_gear;
 
-  onEngage();
-
   updater_.force_update();
 
   if (!isDataReady()) {
@@ -705,11 +709,8 @@ void VehicleCmdGate::onExternalEmergencyStopHeartbeat(
   external_emergency_stop_heartbeat_received_time_ = std::make_shared<rclcpp::Time>(this->now());
 }
 
-void VehicleCmdGate::onGateMode()
+void VehicleCmdGate::onGateMode(GateMode::ConstSharedPtr msg)
 {
-  const auto msg = gate_mode_sub_.takeData();
-  if (!msg) return;
-
   const auto prev_gate_mode = current_gate_mode_;
   current_gate_mode_ = *msg;
 
@@ -720,10 +721,9 @@ void VehicleCmdGate::onGateMode()
   }
 }
 
-void VehicleCmdGate::onEngage()
+void VehicleCmdGate::onEngage(EngageMsg::ConstSharedPtr msg)
 {
-  const auto msg = engage_sub_.takeData();
-  if (msg) is_engaged_ = msg->engage;
+  is_engaged_ = msg->engage;
 }
 
 void VehicleCmdGate::onEngageService(
@@ -733,11 +733,8 @@ void VehicleCmdGate::onEngageService(
   response->status = tier4_api_utils::response_success();
 }
 
-void VehicleCmdGate::onMrmState()
+void VehicleCmdGate::onMrmState(MrmState::ConstSharedPtr msg)
 {
-  const auto msg = mrm_state_sub_.takeData();
-  if (!msg) return;
-
   is_system_emergency_ =
     (msg->state == MrmState::MRM_OPERATING || msg->state == MrmState::MRM_SUCCEEDED ||
      msg->state == MrmState::MRM_FAILED) &&
@@ -830,7 +827,8 @@ void VehicleCmdGate::checkExternalEmergencyStop(diagnostic_updater::DiagnosticSt
   } else if (is_external_emergency_stop_) {
     status.level = DiagnosticStatus::ERROR;
     status.message =
-      "external_emergency_stop is required. Please call `clear_external_emergency_stop` service to "
+      "external_emergency_stop is required. Please call `clear_external_emergency_stop` service "
+      "to "
       "clear state.";
   } else {
     status.level = DiagnosticStatus::OK;

control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp

@@ -119,22 +119,16 @@ class VehicleCmdGate : public rclcpp::Node
     const std::vector<rclcpp::Parameter> & parameters);
   // Subscription
   rclcpp::Subscription<Heartbeat>::SharedPtr external_emergency_stop_heartbeat_sub_;
-  tier4_autoware_utils::InterProcessPollingSubscriber<GateMode> gate_mode_sub_{
-    this, "input/gate_mode"};
-  tier4_autoware_utils::InterProcessPollingSubscriber<OperationModeState> operation_mode_sub_{
-    this, "input/operation_mode", rclcpp::QoS(1).transient_local()};
-  tier4_autoware_utils::InterProcessPollingSubscriber<MrmState> mrm_state_sub_{
-    this, "input/mrm_state"};
-  tier4_autoware_utils::InterProcessPollingSubscriber<Odometry> kinematics_sub_{
-    this, "/localization/kinematic_state"};  // for filter
-  tier4_autoware_utils::InterProcessPollingSubscriber<AccelWithCovarianceStamped> acc_sub_{
-    this, "input/acceleration"};  // for filter
-  tier4_autoware_utils::InterProcessPollingSubscriber<SteeringReport> steer_sub_{
-    this, "input/steering"};  // for filter
-
-  void onGateMode();
+  rclcpp::Subscription<GateMode>::SharedPtr gate_mode_sub_;
+  rclcpp::Subscription<OperationModeState>::SharedPtr operation_mode_sub_;
+  rclcpp::Subscription<MrmState>::SharedPtr mrm_state_sub_;
+  rclcpp::Subscription<Odometry>::SharedPtr kinematics_sub_;             // for filter
+  rclcpp::Subscription<AccelWithCovarianceStamped>::SharedPtr acc_sub_;  // for filter
+  rclcpp::Subscription<SteeringReport>::SharedPtr steer_sub_;            // for filter
+
+  void onGateMode(GateMode::ConstSharedPtr msg);
   void onExternalEmergencyStopHeartbeat(Heartbeat::ConstSharedPtr msg);
-  void onMrmState();
+  void onMrmState(MrmState::ConstSharedPtr msg);
 
   bool is_engaged_;
   bool is_system_emergency_ = false;
@@ -160,37 +154,34 @@ class VehicleCmdGate : public rclcpp::Node
 
   // Subscriber for auto
   Commands auto_commands_;
-  tier4_autoware_utils::InterProcessPollingSubscriber<Control> auto_control_cmd_sub_{
-    this, "input/auto/control_cmd"};
+  rclcpp::Subscription<Control>::SharedPtr auto_control_cmd_sub_;
   tier4_autoware_utils::InterProcessPollingSubscriber<TurnIndicatorsCommand>
     auto_turn_indicator_cmd_sub_{this, "input/auto/turn_indicators_cmd"};
   tier4_autoware_utils::InterProcessPollingSubscriber<HazardLightsCommand>
     auto_hazard_light_cmd_sub_{this, "input/auto/hazard_lights_cmd"};
   tier4_autoware_utils::InterProcessPollingSubscriber<GearCommand> auto_gear_cmd_sub_{
     this, "input/auto/gear_cmd"};
-  void onAutoCtrlCmd();
+  void onAutoCtrlCmd(Control::ConstSharedPtr msg);
 
   // Subscription for external
   Commands remote_commands_;
-  tier4_autoware_utils::InterProcessPollingSubscriber<Control> remote_control_cmd_sub_{
-    this, "input/external/control_cmd"};
+  rclcpp::Subscription<Control>::SharedPtr remote_control_cmd_sub_;
   tier4_autoware_utils::InterProcessPollingSubscriber<TurnIndicatorsCommand>
     remote_turn_indicator_cmd_sub_{this, "input/external/turn_indicators_cmd"};
   tier4_autoware_utils::InterProcessPollingSubscriber<HazardLightsCommand>
     remote_hazard_light_cmd_sub_{this, "input/external/hazard_lights_cmd"};
   tier4_autoware_utils::InterProcessPollingSubscriber<GearCommand> remote_gear_cmd_sub_{
     this, "input/external/gear_cmd"};
-  void onRemoteCtrlCmd();
+  void onRemoteCtrlCmd(Control::ConstSharedPtr msg);
 
   // Subscription for emergency
   Commands emergency_commands_;
-  tier4_autoware_utils::InterProcessPollingSubscriber<Control> emergency_control_cmd_sub_{
-    this, "input/emergency/control_cmd"};
+  rclcpp::Subscription<Control>::SharedPtr emergency_control_cmd_sub_;
   tier4_autoware_utils::InterProcessPollingSubscriber<HazardLightsCommand>
     emergency_hazard_light_cmd_sub_{this, "input/emergency/hazard_lights_cmd"};
   tier4_autoware_utils::InterProcessPollingSubscriber<GearCommand> emergency_gear_cmd_sub_{
     this, "input/emergency/gear_cmd"};
-  void onEmergencyCtrlCmd();
+  void onEmergencyCtrlCmd(Control::ConstSharedPtr msg);
 
   // Parameter
   bool use_emergency_handling_;
@@ -216,10 +207,10 @@ class VehicleCmdGate : public rclcpp::Node
     const SetEmergency::Response::SharedPtr response);
 
   // TODO(Takagi, Isamu): deprecated
-  tier4_autoware_utils::InterProcessPollingSubscriber<EngageMsg> engage_sub_{this, "input/engage"};
+  rclcpp::Subscription<EngageMsg>::SharedPtr engage_sub_;
   rclcpp::Service<Trigger>::SharedPtr srv_external_emergency_stop_;
   rclcpp::Service<Trigger>::SharedPtr srv_clear_external_emergency_stop_;
-  void onEngage();
+  void onEngage(EngageMsg::ConstSharedPtr msg);
   bool onSetExternalEmergencyStopService(
     const std::shared_ptr<rmw_request_id_t> req_header, const Trigger::Request::SharedPtr req,
     const Trigger::Response::SharedPtr res);