Skip to content

Commit c9e78d6

Browse files
yuki-takagi-66veqccpre-commit-ci[bot]
authored
feat(mrm_handler): input gear command (autowarefoundation#8080) (#1498)
* feat(mrm_handler): input gear command * style(pre-commit): autofix * fix minor --------- Signed-off-by: veqcc <ryuta.kambe@tier4.jp> Co-authored-by: Ryuta Kambe <ryuta.kambe@tier4.jp> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
1 parent cf3d631 commit c9e78d6

File tree

3 files changed

+16
-20
lines changed

3 files changed

+16
-20
lines changed

system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp

+2-1
Original file line numberDiff line numberDiff line change
@@ -86,6 +86,8 @@ class MrmHandler : public rclcpp::Node
8686
autoware::universe_utils::InterProcessPollingSubscriber<
8787
autoware_adapi_v1_msgs::msg::OperationModeState>
8888
sub_operation_mode_state_{this, "~/input/api/operation_mode/state"};
89+
autoware::universe_utils::InterProcessPollingSubscriber<autoware_vehicle_msgs::msg::GearCommand>
90+
sub_gear_cmd_{this, "~/input/gear"};
8991

9092
tier4_system_msgs::msg::OperationModeAvailability::ConstSharedPtr operation_mode_availability_;
9193

@@ -146,7 +148,6 @@ class MrmHandler : public rclcpp::Node
146148
void handleFailedRequest();
147149
autoware_adapi_v1_msgs::msg::MrmState::_behavior_type getCurrentMrmBehavior();
148150
bool isStopped();
149-
bool isDrivingBackwards();
150151
bool isEmergency() const;
151152
bool isControlModeAutonomous();
152153
bool isOperationModeAutonomous();

system/mrm_handler/launch/mrm_handler.launch.xml

+2
Original file line numberDiff line numberDiff line change
@@ -7,6 +7,7 @@
77
<arg name="input_mrm_comfortable_stop_state" default="/system/mrm/comfortable_stop/status"/>
88
<arg name="input_mrm_emergency_stop_state" default="/system/mrm/emergency_stop/status"/>
99
<arg name="input_api_operation_mode_state" default="/api/operation_mode/state"/>
10+
<arg name="input_gear" default="/control/command/gear_cmd"/>
1011

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

3032
<remap from="~/output/gear" to="$(var output_gear)"/>
3133
<remap from="~/output/hazard" to="$(var output_hazard)"/>

system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp

+12-19
Original file line numberDiff line numberDiff line change
@@ -130,19 +130,20 @@ void MrmHandler::publishGearCmd()
130130
{
131131
using autoware_vehicle_msgs::msg::GearCommand;
132132
GearCommand msg;
133-
134133
msg.stamp = this->now();
135-
const auto command = [&]() {
136-
// If stopped and use_parking is not true, send the last gear command
137-
if (isStopped())
138-
return (param_.use_parking_after_stopped) ? GearCommand::PARK : last_gear_command_;
139-
return (isDrivingBackwards()) ? GearCommand::REVERSE : GearCommand::DRIVE;
140-
}();
141-
142-
msg.command = command;
143-
last_gear_command_ = msg.command;
134+
135+
if (isEmergency()) {
136+
// gear command is created within mrm_handler
137+
msg.command =
138+
(param_.use_parking_after_stopped && isStopped()) ? GearCommand::PARK : last_gear_command_;
139+
} else {
140+
// use the same gear as the input gear
141+
auto gear = sub_gear_cmd_.takeData();
142+
msg.command = (gear == nullptr) ? last_gear_command_ : gear->command;
143+
last_gear_command_ = msg.command;
144+
}
145+
144146
pub_gear_cmd_->publish(msg);
145-
return;
146147
}
147148

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

527-
bool MrmHandler::isDrivingBackwards()
528-
{
529-
auto odom = sub_odom_.takeData();
530-
if (odom == nullptr) return false;
531-
constexpr auto th_moving_backwards = -0.001;
532-
return odom->twist.twist.linear.x < th_moving_backwards;
533-
}
534-
535528
bool MrmHandler::isEmergency() const
536529
{
537530
return !operation_mode_availability_->autonomous || is_emergency_holding_ ||

0 commit comments

Comments
 (0)