Skip to content

Commit

Permalink
feat(vehicle_cmd_gate): use polling subscriber (#7418)
Browse files Browse the repository at this point in the history 
* 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>
  • Loading branch information
@go-sakayori
go-sakayori authored Jun 10, 2024
1 parent 0e6528b commit c89cd08
Show file tree
Hide file tree
Showing 2 changed files with 118 additions and 99 deletions.
149 changes: 75 additions & 74 deletions control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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");
Expand Down Expand Up @@ -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_);
Expand All @@ -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()) {
Expand Down Expand Up @@ -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(
Expand All @@ -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(
Expand All @@ -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) &&
Expand Down
68 changes: 43 additions & 25 deletions control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand Down Expand Up @@ -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;
Expand All @@ -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_;
Expand All @@ -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);
Expand Down

0 comments on commit c89cd08

Please sign in to comment.