feat(vehicle_cmd_gate): use polling subscriber (autowarefoundation#7418)

* change to polling subscriber

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

* fix

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

---------

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

Author: go-sakayori

control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp

@@ -87,69 +87,6 @@ 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));
-
-  auto_turn_indicator_cmd_sub_ = create_subscription<TurnIndicatorsCommand>(
-    "input/auto/turn_indicators_cmd", 1,
-    [this](TurnIndicatorsCommand::ConstSharedPtr msg) { auto_commands_.turn_indicator = *msg; });
-
-  auto_hazard_light_cmd_sub_ = create_subscription<HazardLightsCommand>(
-    "input/auto/hazard_lights_cmd", 1,
-    [this](HazardLightsCommand::ConstSharedPtr msg) { auto_commands_.hazard_light = *msg; });
-
-  auto_gear_cmd_sub_ = create_subscription<GearCommand>(
-    "input/auto/gear_cmd", 1,
-    [this](GearCommand::ConstSharedPtr msg) { auto_commands_.gear = *msg; });
-
-  // Subscriber for external
-  remote_control_cmd_sub_ = create_subscription<Control>(
-    "input/external/control_cmd", 1, std::bind(&VehicleCmdGate::onRemoteCtrlCmd, this, _1));
-
-  remote_turn_indicator_cmd_sub_ = create_subscription<TurnIndicatorsCommand>(
-    "input/external/turn_indicators_cmd", 1,
-    [this](TurnIndicatorsCommand::ConstSharedPtr msg) { remote_commands_.turn_indicator = *msg; });
-
-  remote_hazard_light_cmd_sub_ = create_subscription<HazardLightsCommand>(
-    "input/external/hazard_lights_cmd", 1,
-    [this](HazardLightsCommand::ConstSharedPtr msg) { remote_commands_.hazard_light = *msg; });
-
-  remote_gear_cmd_sub_ = create_subscription<GearCommand>(
-    "input/external/gear_cmd", 1,
-    [this](GearCommand::ConstSharedPtr msg) { remote_commands_.gear = *msg; });
-
-  // Subscriber for emergency
-  emergency_control_cmd_sub_ = create_subscription<Control>(
-    "input/emergency/control_cmd", 1, std::bind(&VehicleCmdGate::onEmergencyCtrlCmd, this, _1));
-
-  emergency_hazard_light_cmd_sub_ = create_subscription<HazardLightsCommand>(
-    "input/emergency/hazard_lights_cmd", 1,
-    [this](HazardLightsCommand::ConstSharedPtr msg) { emergency_commands_.hazard_light = *msg; });
-
-  emergency_gear_cmd_sub_ = create_subscription<GearCommand>(
-    "input/emergency/gear_cmd", 1,
-    [this](GearCommand::ConstSharedPtr msg) { emergency_commands_.gear = *msg; });
 
   // Parameter
   use_emergency_handling_ = declare_parameter<bool>("use_emergency_handling");
@@ -354,29 +291,32 @@ bool VehicleCmdGate::isDataReady()
 }
 
 // for auto
-void VehicleCmdGate::onAutoCtrlCmd(Control::ConstSharedPtr msg)
+void VehicleCmdGate::onAutoCtrlCmd()
 {
-  auto_commands_.control = *msg;
+  const auto msg = auto_control_cmd_sub_.takeData();
+  if (msg) auto_commands_.control = *msg;
 
   if (current_gate_mode_.data == GateMode::AUTO) {
     publishControlCommands(auto_commands_);
   }
 }
 
 // for remote
-void VehicleCmdGate::onRemoteCtrlCmd(Control::ConstSharedPtr msg)
+void VehicleCmdGate::onRemoteCtrlCmd()
 {
-  remote_commands_.control = *msg;
+  const auto msg = remote_control_cmd_sub_.takeData();
+  if (msg) remote_commands_.control = *msg;
 
   if (current_gate_mode_.data == GateMode::EXTERNAL) {
     publishControlCommands(remote_commands_);
   }
 }
 
 // for emergency
-void VehicleCmdGate::onEmergencyCtrlCmd(Control::ConstSharedPtr msg)
+void VehicleCmdGate::onEmergencyCtrlCmd()
 {
-  emergency_commands_.control = *msg;
+  const auto msg = emergency_control_cmd_sub_.takeData();
+  if (msg) emergency_commands_.control = *msg;
 
   if (use_emergency_handling_ && is_system_emergency_) {
     publishControlCommands(emergency_commands_);
@@ -385,6 +325,61 @@ void VehicleCmdGate::onEmergencyCtrlCmd(Control::ConstSharedPtr msg)
 
 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;
+
+  const auto msg_auto_command_hazard_light = auto_hazard_light_cmd_sub_.takeData();
+  if (msg_auto_command_hazard_light) auto_commands_.hazard_light = *msg_auto_command_hazard_light;
+
+  const auto msg_auto_command_gear = auto_gear_cmd_sub_.takeData();
+  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;
+
+  const auto msg_remote_command_hazard_light = remote_hazard_light_cmd_sub_.takeData();
+  if (msg_remote_command_hazard_light)
+    remote_commands_.hazard_light = *msg_remote_command_hazard_light;
+
+  const auto msg_remote_command_gear = remote_gear_cmd_sub_.takeData();
+  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()) {
@@ -710,10 +705,12 @@ void VehicleCmdGate::onExternalEmergencyStopHeartbeat(
   external_emergency_stop_heartbeat_received_time_ = std::make_shared<rclcpp::Time>(this->now());
 }
 
-void VehicleCmdGate::onGateMode(GateMode::ConstSharedPtr msg)
+void VehicleCmdGate::onGateMode()
 {
+  const auto msg = gate_mode_sub_.takeData();
+  if (!msg) return;
+
   const auto prev_gate_mode = current_gate_mode_;
-  current_gate_mode_ = *msg;
 
   if (current_gate_mode_.data != prev_gate_mode.data) {
     RCLCPP_INFO(
@@ -722,9 +719,10 @@ void VehicleCmdGate::onGateMode(GateMode::ConstSharedPtr msg)
   }
 }
 
-void VehicleCmdGate::onEngage(EngageMsg::ConstSharedPtr msg)
+void VehicleCmdGate::onEngage()
 {
-  is_engaged_ = msg->engage;
+  const auto msg = engage_sub_.takeData();
+  if (msg) is_engaged_ = msg->engage;
 }
 
 void VehicleCmdGate::onEngageService(
@@ -734,8 +732,11 @@ void VehicleCmdGate::onEngageService(
   response->status = tier4_api_utils::response_success();
 }
 
-void VehicleCmdGate::onMrmState(MrmState::ConstSharedPtr msg)
+void VehicleCmdGate::onMrmState()
 {
+  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) &&

control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp

@@ -25,6 +25,7 @@
 #include <diagnostic_updater/diagnostic_updater.hpp>
 #include <motion_utils/vehicle/vehicle_state_checker.hpp>
 #include <rclcpp/rclcpp.hpp>
+#include <tier4_autoware_utils/ros/polling_subscriber.hpp>
 #include <tier4_autoware_utils/ros/published_time_publisher.hpp>
 
 #include <autoware_adapi_v1_msgs/msg/mrm_state.hpp>
@@ -118,16 +119,22 @@ class VehicleCmdGate : public rclcpp::Node
     const std::vector<rclcpp::Parameter> & parameters);
   // Subscription
   rclcpp::Subscription<Heartbeat>::SharedPtr external_emergency_stop_heartbeat_sub_;
-  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);
+  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();
   void onExternalEmergencyStopHeartbeat(Heartbeat::ConstSharedPtr msg);
-  void onMrmState(MrmState::ConstSharedPtr msg);
+  void onMrmState();
 
   bool is_engaged_;
   bool is_system_emergency_ = false;
@@ -153,26 +160,37 @@ class VehicleCmdGate : public rclcpp::Node
 
   // Subscriber for auto
   Commands auto_commands_;
-  rclcpp::Subscription<Control>::SharedPtr auto_control_cmd_sub_;
-  rclcpp::Subscription<TurnIndicatorsCommand>::SharedPtr auto_turn_indicator_cmd_sub_;
-  rclcpp::Subscription<HazardLightsCommand>::SharedPtr auto_hazard_light_cmd_sub_;
-  rclcpp::Subscription<GearCommand>::SharedPtr auto_gear_cmd_sub_;
-  void onAutoCtrlCmd(Control::ConstSharedPtr msg);
+  tier4_autoware_utils::InterProcessPollingSubscriber<Control> auto_control_cmd_sub_{
+    this, "input/auto/control_cmd"};
+  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();
 
   // Subscription for external
   Commands remote_commands_;
-  rclcpp::Subscription<Control>::SharedPtr remote_control_cmd_sub_;
-  rclcpp::Subscription<TurnIndicatorsCommand>::SharedPtr remote_turn_indicator_cmd_sub_;
-  rclcpp::Subscription<HazardLightsCommand>::SharedPtr remote_hazard_light_cmd_sub_;
-  rclcpp::Subscription<GearCommand>::SharedPtr remote_gear_cmd_sub_;
-  void onRemoteCtrlCmd(Control::ConstSharedPtr msg);
+  tier4_autoware_utils::InterProcessPollingSubscriber<Control> remote_control_cmd_sub_{
+    this, "input/external/control_cmd"};
+  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();
 
   // Subscription for emergency
   Commands emergency_commands_;
-  rclcpp::Subscription<Control>::SharedPtr emergency_control_cmd_sub_;
-  rclcpp::Subscription<HazardLightsCommand>::SharedPtr emergency_hazard_light_cmd_sub_;
-  rclcpp::Subscription<GearCommand>::SharedPtr emergency_gear_cmd_sub_;
-  void onEmergencyCtrlCmd(Control::ConstSharedPtr msg);
+  tier4_autoware_utils::InterProcessPollingSubscriber<Control> emergency_control_cmd_sub_{
+    this, "input/emergency/control_cmd"};
+  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();
 
   // Parameter
   bool use_emergency_handling_;
@@ -198,10 +216,10 @@ class VehicleCmdGate : public rclcpp::Node
     const SetEmergency::Response::SharedPtr response);
 
   // TODO(Takagi, Isamu): deprecated
-  rclcpp::Subscription<EngageMsg>::SharedPtr engage_sub_;
+  tier4_autoware_utils::InterProcessPollingSubscriber<EngageMsg> engage_sub_{this, "input/engage"};
   rclcpp::Service<Trigger>::SharedPtr srv_external_emergency_stop_;
   rclcpp::Service<Trigger>::SharedPtr srv_clear_external_emergency_stop_;
-  void onEngage(EngageMsg::ConstSharedPtr msg);
+  void onEngage();
   bool onSetExternalEmergencyStopService(
     const std::shared_ptr<rmw_request_id_t> req_header, const Trigger::Request::SharedPtr req,
     const Trigger::Response::SharedPtr res);