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.

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): fix processing time measurement #9260

Merged
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
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
26 changes: 7 additions & 19 deletions control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -377,147 +377,153 @@

void VehicleCmdGate::onTimer()
{
stop_watch.tic();
autoware::universe_utils::StopWatch<std::chrono::milliseconds> stop_watch;

// Subscriber for auto
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
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
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;

updater_.force_update();

if (!isDataReady()) {
RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 5000, "waiting topics...");
return;
}

// Check system emergency heartbeat
if (use_emergency_handling_) {
is_emergency_state_heartbeat_timeout_ = isHeartbeatTimeout(
emergency_state_heartbeat_received_time_, system_emergency_heartbeat_timeout_);

if (is_emergency_state_heartbeat_timeout_) {
RCLCPP_WARN_THROTTLE(
get_logger(), *get_clock(), 5000 /*ms*/, "system_emergency heartbeat is timeout.");
publishEmergencyStopControlCommands();
return;
}
}

// Check external emergency stop heartbeat
if (check_external_emergency_heartbeat_) {
is_external_emergency_stop_heartbeat_timeout_ = isHeartbeatTimeout(
external_emergency_stop_heartbeat_received_time_, external_emergency_stop_heartbeat_timeout_);

if (is_external_emergency_stop_heartbeat_timeout_) {
RCLCPP_WARN_THROTTLE(
get_logger(), *get_clock(), 5000 /*ms*/, "external_emergency_stop heartbeat is timeout.");
is_external_emergency_stop_ = true;
}
}

// Check external emergency stop
if (is_external_emergency_stop_) {
if (!is_external_emergency_stop_heartbeat_timeout_) {
RCLCPP_INFO_THROTTLE(
get_logger(), *get_clock(), 5000 /*ms*/,
"Please call `clear_external_emergency_stop` service to clear state.");
}

publishEmergencyStopControlCommands();
return;
}

// Select commands
TurnIndicatorsCommand turn_indicator;
HazardLightsCommand hazard_light;
GearCommand gear;
if (use_emergency_handling_ && is_system_emergency_) {
turn_indicator = emergency_commands_.turn_indicator;
hazard_light = emergency_commands_.hazard_light;
gear = emergency_commands_.gear;
} else {
if (current_gate_mode_.data == GateMode::AUTO) {
turn_indicator = auto_commands_.turn_indicator;
hazard_light = auto_commands_.hazard_light;
gear = auto_commands_.gear;

// Don't send turn signal when autoware is not engaged
if (!is_engaged_) {
turn_indicator.command = TurnIndicatorsCommand::NO_COMMAND;
hazard_light.command = HazardLightsCommand::NO_COMMAND;
turn_indicator.stamp = this->now();
hazard_light.stamp = this->now();
}
} else if (current_gate_mode_.data == GateMode::EXTERNAL) {
turn_indicator = remote_commands_.turn_indicator;
hazard_light = remote_commands_.hazard_light;
gear = remote_commands_.gear;
} else {
throw std::runtime_error("invalid mode");
}
}

// Publish Turn Indicators, Hazard Lights and Gear Command
if (prev_turn_indicator_ != nullptr) {
*prev_turn_indicator_ =
getContinuousTopic(prev_turn_indicator_, turn_indicator, "TurnIndicatorsCommand");
turn_indicator_cmd_pub_->publish(*prev_turn_indicator_);
} else {
if (msg_auto_command_turn_indicator || msg_remote_command_turn_indicator) {
prev_turn_indicator_ = std::make_shared<TurnIndicatorsCommand>(turn_indicator);
}
turn_indicator_cmd_pub_->publish(turn_indicator);
}

if (prev_hazard_light_ != nullptr) {
*prev_hazard_light_ =
getContinuousTopic(prev_hazard_light_, hazard_light, "HazardLightsCommand");
hazard_light_cmd_pub_->publish(*prev_hazard_light_);
} else {
if (
msg_auto_command_hazard_light || msg_remote_command_hazard_light ||
msg_emergency_command_hazard_light) {
prev_hazard_light_ = std::make_shared<HazardLightsCommand>(hazard_light);
}
hazard_light_cmd_pub_->publish(hazard_light);
}

if (prev_gear_ != nullptr) {
*prev_gear_ = getContinuousTopic(prev_gear_, gear, "GearCommand");
gear_cmd_pub_->publish(*prev_gear_);
} else {
if (msg_auto_command_gear || msg_remote_command_gear || msg_emergency_command_gear) {
prev_gear_ = std::make_shared<GearCommand>(gear);
}
gear_cmd_pub_->publish(gear);
}

// ProcessingTime
tier4_debug_msgs::msg::Float64Stamped processing_time_msg;
processing_time_msg.stamp = get_clock()->now();
processing_time_msg.data = stop_watch.toc();
processing_time_pub_->publish(processing_time_msg);

Check warning on line 526 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: Complex Method

VehicleCmdGate::onTimer already has high cyclomatic complexity, and now it increases in Lines of Code from 114 to 118. 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.
}

void VehicleCmdGate::publishControlCommands(const Commands & commands)
Expand Down Expand Up @@ -577,18 +583,12 @@
vehicle_cmd_emergency.emergency = (use_emergency_handling_ && is_system_emergency_);
vehicle_cmd_emergency.stamp = filtered_control.stamp;

// ProcessingTime
tier4_debug_msgs::msg::Float64Stamped processing_time_msg;
processing_time_msg.stamp = get_clock()->now();
processing_time_msg.data = stop_watch.toc();

// Publish commands
vehicle_cmd_emergency_pub_->publish(vehicle_cmd_emergency);
control_cmd_pub_->publish(filtered_control);
published_time_publisher_->publish_if_subscribed(control_cmd_pub_, filtered_control.stamp);
adapi_pause_->publish();
moderate_stop_interface_->publish();
processing_time_pub_->publish(processing_time_msg);

// Save ControlCmd to steering angle when disengaged
prev_control_cmd_ = filtered_control;
Expand Down Expand Up @@ -626,18 +626,12 @@
vehicle_cmd_emergency.stamp = stamp;
vehicle_cmd_emergency.emergency = true;

// ProcessingTime
tier4_debug_msgs::msg::Float64Stamped processing_time_msg;
processing_time_msg.stamp = get_clock()->now();
processing_time_msg.data = stop_watch.toc();

// Publish topics
vehicle_cmd_emergency_pub_->publish(vehicle_cmd_emergency);
control_cmd_pub_->publish(control_cmd);
turn_indicator_cmd_pub_->publish(turn_indicator);
hazard_light_cmd_pub_->publish(hazard_light);
gear_cmd_pub_->publish(gear);
processing_time_pub_->publish(processing_time_msg);
}

void VehicleCmdGate::publishStatus()
Expand All @@ -654,18 +648,12 @@
external_emergency.stamp = stamp;
external_emergency.emergency = is_external_emergency_stop_;

// ProcessingTime
tier4_debug_msgs::msg::Float64Stamped processing_time_msg;
processing_time_msg.stamp = get_clock()->now();
processing_time_msg.data = stop_watch.toc();

gate_mode_pub_->publish(current_gate_mode_);
engage_pub_->publish(autoware_engage);
pub_external_emergency_->publish(external_emergency);
operation_mode_pub_->publish(current_operation_mode_);
adapi_pause_->publish();
moderate_stop_interface_->publish();
processing_time_pub_->publish(processing_time_msg);
}

Control VehicleCmdGate::filterControlCommand(const Control & in)
Expand Down
3 changes: 0 additions & 3 deletions control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -271,9 +271,6 @@ class VehicleCmdGate : public rclcpp::Node
std::unique_ptr<VehicleStopChecker> vehicle_stop_checker_;
double stop_check_duration_;

// processing time
autoware::universe_utils::StopWatch<std::chrono::milliseconds> stop_watch;

// debug
MarkerArray createMarkerArray(const IsFilterActivated & filter_activated);
void publishMarkers(const IsFilterActivated & filter_activated);
Expand Down
Loading