Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

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

Merged
merged 3 commits into from
Jun 17, 2024
Merged

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

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
3855cfa
put back polling subscribers to subscribers in neccesary cases
go-sakayori Jun 14, 2024
1db6406
Merge branch 'awf-latest' into fix/vehicle_cmd_gate_subscriber
go-sakayori Jun 15, 2024
34161f8
Merge branch 'main' into fix/vehicle_cmd_gate_subscriber
go-sakayori Jun 17, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
88 changes: 43 additions & 45 deletions control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -87,6 +87,37 @@
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));

Check warning on line 120 in control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ Getting worse: Large Method 

VehicleCmdGate::VehicleCmdGate increases from 113 to 138 lines of code, threshold = 70. Large functions with many lines of code are generally harder to understand and lower the code health. Avoid adding more lines to this function.

// Parameter
use_emergency_handling_ = declare_parameter<bool>("use_emergency_handling");
Expand Down Expand Up @@ -291,32 +322,29 @@
}

// 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 @@ -324,62 +352,38 @@
}

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;

Check notice on line 386 in control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ Getting better: Complex Method 

VehicleCmdGate::onTimer decreases in cyclomatic complexity from 26 to 22, threshold = 9. This function has many conditional statements (e.g. if, for, while), leading to lower code health. Avoid adding more conditionals and code to it without refactoring.
onEngage();

updater_.force_update();

if (!isDataReady()) {
Expand Down Expand Up @@ -705,11 +709,8 @@
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::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 @@
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 @@
} 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
Open in desktop
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
Loading