diff --git a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp index 8f083990cd4ea..79b43ebe299ba 100644 --- a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp +++ b/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( "input/external_emergency_stop_heartbeat", 1, std::bind(&VehicleCmdGate::onExternalEmergencyStopHeartbeat, this, _1)); + gate_mode_sub_ = create_subscription( + "input/gate_mode", 1, std::bind(&VehicleCmdGate::onGateMode, this, _1)); + engage_sub_ = create_subscription( + "input/engage", 1, std::bind(&VehicleCmdGate::onEngage, this, _1)); + kinematics_sub_ = create_subscription( + "/localization/kinematic_state", 1, + [this](Odometry::SharedPtr msg) { current_kinematics_ = *msg; }); + acc_sub_ = create_subscription( + "input/acceleration", 1, [this](AccelWithCovarianceStamped::SharedPtr msg) { + current_acceleration_ = msg->accel.accel.linear.x; + }); + steer_sub_ = create_subscription( + "input/steering", 1, + [this](SteeringReport::SharedPtr msg) { current_steer_ = msg->steering_tire_angle; }); + operation_mode_sub_ = create_subscription( + "input/operation_mode", rclcpp::QoS(1).transient_local(), + [this](const OperationModeState::SharedPtr msg) { current_operation_mode_ = *msg; }); + mrm_state_sub_ = create_subscription( + "input/mrm_state", 1, std::bind(&VehicleCmdGate::onMrmState, this, _1)); + + // Subscriber for auto + auto_control_cmd_sub_ = create_subscription( + "input/auto/control_cmd", 1, std::bind(&VehicleCmdGate::onAutoCtrlCmd, this, _1)); + + // Subscriber for external + remote_control_cmd_sub_ = create_subscription( + "input/external/control_cmd", 1, std::bind(&VehicleCmdGate::onRemoteCtrlCmd, this, _1)); + + // Subscriber for emergency + emergency_control_cmd_sub_ = create_subscription( + "input/emergency/control_cmd", 1, std::bind(&VehicleCmdGate::onEmergencyCtrlCmd, this, _1)); // Parameter use_emergency_handling_ = declare_parameter("use_emergency_handling"); @@ -291,10 +322,9 @@ 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_); @@ -302,10 +332,9 @@ void VehicleCmdGate::onAutoCtrlCmd() } // 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_); @@ -313,10 +342,9 @@ void VehicleCmdGate::onRemoteCtrlCmd() } // 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,8 +377,6 @@ 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; @@ -378,8 +384,6 @@ void VehicleCmdGate::onTimer() 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(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; diff --git a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp index 5ca8db5497e59..f69ac24d2a6f8 100644 --- a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp +++ b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp @@ -119,22 +119,16 @@ class VehicleCmdGate : public rclcpp::Node const std::vector & parameters); // Subscription rclcpp::Subscription::SharedPtr external_emergency_stop_heartbeat_sub_; - tier4_autoware_utils::InterProcessPollingSubscriber gate_mode_sub_{ - this, "input/gate_mode"}; - tier4_autoware_utils::InterProcessPollingSubscriber operation_mode_sub_{ - this, "input/operation_mode", rclcpp::QoS(1).transient_local()}; - tier4_autoware_utils::InterProcessPollingSubscriber mrm_state_sub_{ - this, "input/mrm_state"}; - tier4_autoware_utils::InterProcessPollingSubscriber kinematics_sub_{ - this, "/localization/kinematic_state"}; // for filter - tier4_autoware_utils::InterProcessPollingSubscriber acc_sub_{ - this, "input/acceleration"}; // for filter - tier4_autoware_utils::InterProcessPollingSubscriber steer_sub_{ - this, "input/steering"}; // for filter - - void onGateMode(); + rclcpp::Subscription::SharedPtr gate_mode_sub_; + rclcpp::Subscription::SharedPtr operation_mode_sub_; + rclcpp::Subscription::SharedPtr mrm_state_sub_; + rclcpp::Subscription::SharedPtr kinematics_sub_; // for filter + rclcpp::Subscription::SharedPtr acc_sub_; // for filter + rclcpp::Subscription::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 auto_control_cmd_sub_{ - this, "input/auto/control_cmd"}; + rclcpp::Subscription::SharedPtr auto_control_cmd_sub_; tier4_autoware_utils::InterProcessPollingSubscriber auto_turn_indicator_cmd_sub_{this, "input/auto/turn_indicators_cmd"}; tier4_autoware_utils::InterProcessPollingSubscriber auto_hazard_light_cmd_sub_{this, "input/auto/hazard_lights_cmd"}; tier4_autoware_utils::InterProcessPollingSubscriber 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 remote_control_cmd_sub_{ - this, "input/external/control_cmd"}; + rclcpp::Subscription::SharedPtr remote_control_cmd_sub_; tier4_autoware_utils::InterProcessPollingSubscriber remote_turn_indicator_cmd_sub_{this, "input/external/turn_indicators_cmd"}; tier4_autoware_utils::InterProcessPollingSubscriber remote_hazard_light_cmd_sub_{this, "input/external/hazard_lights_cmd"}; tier4_autoware_utils::InterProcessPollingSubscriber 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 emergency_control_cmd_sub_{ - this, "input/emergency/control_cmd"}; + rclcpp::Subscription::SharedPtr emergency_control_cmd_sub_; tier4_autoware_utils::InterProcessPollingSubscriber emergency_hazard_light_cmd_sub_{this, "input/emergency/hazard_lights_cmd"}; tier4_autoware_utils::InterProcessPollingSubscriber 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 engage_sub_{this, "input/engage"}; + rclcpp::Subscription::SharedPtr engage_sub_; rclcpp::Service::SharedPtr srv_external_emergency_stop_; rclcpp::Service::SharedPtr srv_clear_external_emergency_stop_; - void onEngage(); + void onEngage(EngageMsg::ConstSharedPtr msg); bool onSetExternalEmergencyStopService( const std::shared_ptr req_header, const Trigger::Request::SharedPtr req, const Trigger::Response::SharedPtr res);