Skip to content

Commit

Permalink
feat(mrm_handler): input gear command (autowarefoundation#8080) (#1498)
Browse files Browse the repository at this point in the history
* feat(mrm_handler): input gear command



* style(pre-commit): autofix

* fix minor



---------

Signed-off-by: veqcc <[email protected]>
Co-authored-by: Ryuta Kambe <[email protected]>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
  • Loading branch information
3 people authored Aug 30, 2024
1 parent cf3d631 commit c9e78d6
Show file tree
Hide file tree
Showing 3 changed files with 16 additions and 20 deletions.
3 changes: 2 additions & 1 deletion system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -86,6 +86,8 @@ class MrmHandler : public rclcpp::Node
autoware::universe_utils::InterProcessPollingSubscriber<
autoware_adapi_v1_msgs::msg::OperationModeState>
sub_operation_mode_state_{this, "~/input/api/operation_mode/state"};
autoware::universe_utils::InterProcessPollingSubscriber<autoware_vehicle_msgs::msg::GearCommand>
sub_gear_cmd_{this, "~/input/gear"};

tier4_system_msgs::msg::OperationModeAvailability::ConstSharedPtr operation_mode_availability_;

Expand Down Expand Up @@ -146,7 +148,6 @@ class MrmHandler : public rclcpp::Node
void handleFailedRequest();
autoware_adapi_v1_msgs::msg::MrmState::_behavior_type getCurrentMrmBehavior();
bool isStopped();
bool isDrivingBackwards();
bool isEmergency() const;
bool isControlModeAutonomous();
bool isOperationModeAutonomous();
Expand Down
2 changes: 2 additions & 0 deletions system/mrm_handler/launch/mrm_handler.launch.xml
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,7 @@
<arg name="input_mrm_comfortable_stop_state" default="/system/mrm/comfortable_stop/status"/>
<arg name="input_mrm_emergency_stop_state" default="/system/mrm/emergency_stop/status"/>
<arg name="input_api_operation_mode_state" default="/api/operation_mode/state"/>
<arg name="input_gear" default="/control/command/gear_cmd"/>

<arg name="output_gear" default="/system/emergency/gear_cmd"/>
<arg name="output_hazard" default="/system/emergency/hazard_lights_cmd"/>
Expand All @@ -26,6 +27,7 @@
<remap from="~/input/mrm/comfortable_stop/status" to="$(var input_mrm_comfortable_stop_state)"/>
<remap from="~/input/mrm/emergency_stop/status" to="$(var input_mrm_emergency_stop_state)"/>
<remap from="~/input/api/operation_mode/state" to="$(var input_api_operation_mode_state)"/>
<remap from="~/input/gear" to="$(var input_gear)"/>

<remap from="~/output/gear" to="$(var output_gear)"/>
<remap from="~/output/hazard" to="$(var output_hazard)"/>
Expand Down
31 changes: 12 additions & 19 deletions system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -130,19 +130,20 @@ void MrmHandler::publishGearCmd()
{
using autoware_vehicle_msgs::msg::GearCommand;
GearCommand msg;

msg.stamp = this->now();
const auto command = [&]() {
// If stopped and use_parking is not true, send the last gear command
if (isStopped())
return (param_.use_parking_after_stopped) ? GearCommand::PARK : last_gear_command_;
return (isDrivingBackwards()) ? GearCommand::REVERSE : GearCommand::DRIVE;
}();

msg.command = command;
last_gear_command_ = msg.command;

if (isEmergency()) {
// gear command is created within mrm_handler
msg.command =
(param_.use_parking_after_stopped && isStopped()) ? GearCommand::PARK : last_gear_command_;
} else {
// use the same gear as the input gear
auto gear = sub_gear_cmd_.takeData();
msg.command = (gear == nullptr) ? last_gear_command_ : gear->command;
last_gear_command_ = msg.command;
}

pub_gear_cmd_->publish(msg);
return;
}

void MrmHandler::publishMrmState()
Expand Down Expand Up @@ -524,14 +525,6 @@ bool MrmHandler::isStopped()
return (std::abs(odom->twist.twist.linear.x) < th_stopped_velocity);
}

bool MrmHandler::isDrivingBackwards()
{
auto odom = sub_odom_.takeData();
if (odom == nullptr) return false;
constexpr auto th_moving_backwards = -0.001;
return odom->twist.twist.linear.x < th_moving_backwards;
}

bool MrmHandler::isEmergency() const
{
return !operation_mode_availability_->autonomous || is_emergency_holding_ ||
Expand Down

0 comments on commit c9e78d6

Please sign in to comment.