Skip to content

Commit

Permalink
fix(vehicle_cmd_gate): put back subscriber rather than using polling …
Browse files Browse the repository at this point in the history 
…subsriber (#7500)

put back polling subscribers to subscribers in neccesary cases

Signed-off-by: Go Sakayori <[email protected]>
Signed-off-by: Go Sakayori <[email protected]>
  • Loading branch information
@go-sakayori @KhalilSelyan
go-sakayori authored and KhalilSelyan committed Jul 22, 2024
1 parent 0110e8f commit 7adc3d0
Show file tree
Hide file tree
Showing 2 changed files with 60 additions and 71 deletions.
88 changes: 43 additions & 45 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,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");
Expand Down Expand Up @@ -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_);
Expand All @@ -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;
Expand All @@ -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;
Expand All @@ -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()) {
Expand Down Expand Up @@ -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;

Expand All @@ -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(
Expand All @@ -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) &&
Expand Down Expand Up @@ -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;
Expand Down
43 changes: 17 additions & 26 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 @@ -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;
Expand All @@ -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_;
Expand All @@ -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);
Expand Down

0 comments on commit 7adc3d0

Please sign in to comment.