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

refactor(mrm_handler): delete control_cmd publish function #6514

Merged
merged 5 commits into from
Mar 6, 2024
Merged

refactor(mrm_handler): delete control_cmd publish function #6514

Show file tree
Hide file tree
Changes from 2 commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
50d368c
refactor(mrm_handler): delete control_cmd publish function
veqcc Feb 29, 2024
9d1f6ee
style(pre-commit): autofix
pre-commit-ci[bot] Feb 29, 2024
cf11633
add timestamp to hazard_light_command topic
veqcc Mar 1, 2024
b74ac87
Merge branch 'main' into refactor_mrm_handler
veqcc Mar 5, 2024
5cfa39b
Merge branch 'main' into refactor_mrm_handler
veqcc Mar 6, 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
5 changes: 2 additions & 3 deletions system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -106,9 +106,8 @@ class MrmHandler : public rclcpp::Node
pub_hazard_cmd_;
rclcpp::Publisher<autoware_auto_vehicle_msgs::msg::GearCommand>::SharedPtr pub_gear_cmd_;

autoware_auto_vehicle_msgs::msg::HazardLightsCommand createHazardCmdMsg();
autoware_auto_vehicle_msgs::msg::GearCommand createGearCmdMsg();
void publishControlCommands();
void publishHazardCmd();
void publishGearCmd();

rclcpp::Publisher<autoware_adapi_v1_msgs::msg::MrmState>::SharedPtr pub_mrm_state_;

Expand Down
46 changes: 20 additions & 26 deletions system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -163,48 +163,38 @@ void MrmHandler::onOperationModeState(
operation_mode_state_ = msg;
}

autoware_auto_vehicle_msgs::msg::HazardLightsCommand MrmHandler::createHazardCmdMsg()
void MrmHandler::publishHazardCmd()
{
using autoware_auto_vehicle_msgs::msg::HazardLightsCommand;
HazardLightsCommand msg;
veqcc marked this conversation as resolved.
Show resolved Hide resolved

// Check emergency
const bool is_emergency = isEmergency();

if (is_emergency_holding_) {
// turn hazard on during emergency holding
msg.command = HazardLightsCommand::ENABLE;
} else if (is_emergency && param_.turning_hazard_on.emergency) {
} else if (isEmergency() && param_.turning_hazard_on.emergency) {
// turn hazard on if vehicle is in emergency state and
// turning hazard on if emergency flag is true
msg.command = HazardLightsCommand::ENABLE;
} else {
msg.command = HazardLightsCommand::NO_COMMAND;
}
return msg;

pub_hazard_cmd_->publish(msg);
}

void MrmHandler::publishControlCommands()
void MrmHandler::publishGearCmd()
{
using autoware_auto_vehicle_msgs::msg::GearCommand;
GearCommand msg;

// Create timestamp
const auto stamp = this->now();

// Publish hazard command
pub_hazard_cmd_->publish(createHazardCmdMsg());

// Publish gear
{
GearCommand msg;
msg.stamp = stamp;
if (param_.use_parking_after_stopped && isStopped()) {
msg.command = GearCommand::PARK;
} else {
msg.command = GearCommand::DRIVE;
}
pub_gear_cmd_->publish(msg);
msg.stamp = this->now();
if (param_.use_parking_after_stopped && isStopped()) {
msg.command = GearCommand::PARK;
} else {
msg.command = GearCommand::DRIVE;
}

pub_gear_cmd_->publish(msg);
}

void MrmHandler::publishMrmState()
Expand Down Expand Up @@ -382,17 +372,21 @@ void MrmHandler::onTimer()
this->get_logger(), *this->get_clock(), std::chrono::milliseconds(1000).count(),
"heartbeat operation_mode_availability is timeout");
mrm_state_.state = autoware_adapi_v1_msgs::msg::MrmState::MRM_OPERATING;
publishControlCommands();
publishHazardCmd();
publishGearCmd();
return;
}

// Update Emergency State
updateMrmState();

// Publish control commands
publishControlCommands();
// Operate MRM
operateMrm();

// Publish
publishMrmState();
publishHazardCmd();
publishGearCmd();
}

void MrmHandler::transitionTo(const int new_state)
Expand Down
Loading