Skip to content

Commit 52dbe75

Browse files
TetsuKawapre-commit-ci[bot]mkuri
authored
feat(mrm_handler): add mrm_handler (autowarefoundation#6400)
* feat: add mrm_handler Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com> * style(pre-commit): autofix Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com> * modify: update README Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com> * feat: refactor isArrivedAtGoal() Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com> * modify: fix error massages gramatically. Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com> * feat: update the person in charge of the unimplemented parts Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com> * modify: fix typo in schema.json Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com> * modify: fix copyright Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com> * modify: fix mistakes in README Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com> * modify: correct a type mistake in README Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com> * style(pre-commit): autofix Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com> --------- Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Makoto Kurihara <mkuri8m@gmail.com>
1 parent 30cd241 commit 52dbe75

File tree

10 files changed

+1382
-0
lines changed

10 files changed

+1382
-0
lines changed

system/mrm_handler/CMakeLists.txt

+15
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,15 @@
1+
cmake_minimum_required(VERSION 3.14)
2+
project(mrm_handler)
3+
4+
find_package(autoware_cmake REQUIRED)
5+
autoware_package()
6+
7+
ament_auto_add_executable(mrm_handler
8+
src/mrm_handler/mrm_handler_node.cpp
9+
src/mrm_handler/mrm_handler_core.cpp
10+
)
11+
12+
ament_auto_package(INSTALL_TO_SHARE
13+
launch
14+
config
15+
)

system/mrm_handler/README.md

+44
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,44 @@
1+
# mrm_handler
2+
3+
## Purpose
4+
5+
MRM Handler is a node to select a proper MRM from a system failure state contained in OperationModeAvailability.
6+
7+
## Inner-workings / Algorithms
8+
9+
### State Transitions
10+
11+
![mrm-state](image/mrm-state.svg)
12+
13+
## Inputs / Outputs
14+
15+
### Input
16+
17+
| Name | Type | Description |
18+
| -------------------------------------- | ---------------------------------------------------- | --------------------------------------------------------------------------------------------------- |
19+
| `/localization/kinematic_state` | `nav_msgs::msg::Odometry` | Used to decide whether vehicle is stopped or not |
20+
| `/system/operation_mode/availability` | `tier4_system_msgs::msg::OperationModeAvailability` | Used to select proper MRM from system available mrm behavior contained in operationModeAvailability |
21+
| `/vehicle/status/control_mode` | `autoware_auto_vehicle_msgs::msg::ControlModeReport` | Used to check vehicle mode: autonomous or manual |
22+
| `/system/mrm/emergency_stop/status` | `tier4_system_msgs::msg::MrmBehaviorStatus` | Used to check if MRM emergency stop operation is available |
23+
| `/system/mrm/comfortable_stop/status` | `tier4_system_msgs::msg::MrmBehaviorStatus` | Used to check if MRM comfortable stop operation is available |
24+
| `/system/mrm/pull_over_manager/status` | `tier4_system_msgs::msg::MrmBehaviorStatus` | Used to check if MRM pull over operation is available |
25+
| `/api/operation_mode/state` | `autoware_adapi_v1_msgs::msg::OperationModeState` | Used to check whether the current operation mode is AUTO or STOP. |
26+
27+
### Output
28+
29+
| Name | Type | Description |
30+
| --------------------------------------- | ------------------------------------------------------ | ----------------------------------------------------- |
31+
| `/system/emergency/gear_cmd` | `autoware_auto_vehicle_msgs::msg::GearCommand` | Required to execute proper MRM (send gear cmd) |
32+
| `/system/emergency/hazard_lights_cmd` | `autoware_auto_vehicle_msgs::msg::HazardLightsCommand` | Required to execute proper MRM (send turn signal cmd) |
33+
| `/system/fail_safe/mrm_state` | `autoware_adapi_v1_msgs::msg::MrmState` | Inform MRM execution state and selected MRM behavior |
34+
| `/system/mrm/emergency_stop/operate` | `tier4_system_msgs::srv::OperateMrm` | Execution order for MRM emergency stop |
35+
| `/system/mrm/comfortable_stop/operate` | `tier4_system_msgs::srv::OperateMrm` | Execution order for MRM comfortable stop |
36+
| `/system/mrm/pull_over_manager/operate` | `tier4_system_msgs::srv::OperateMrm` | Execution order for MRM pull over |
37+
38+
## Parameters
39+
40+
{{ json_to_markdown("system/mrm_handler/schema/mrm_handler.schema.json") }}
41+
42+
## Assumptions / Known limits
43+
44+
TBD.
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,17 @@
1+
# Default configuration for mrm handler
2+
---
3+
/**:
4+
ros__parameters:
5+
update_rate: 10
6+
timeout_operation_mode_availability: 0.5
7+
use_emergency_holding: false
8+
timeout_emergency_recovery: 5.0
9+
timeout_takeover_request: 10.0
10+
use_takeover_request: false
11+
use_parking_after_stopped: false
12+
use_pull_over: false
13+
use_comfortable_stop: false
14+
15+
# setting whether to turn hazard lamp on for each situation
16+
turning_hazard_on:
17+
emergency: true

system/mrm_handler/image/mrm-state.svg

+362
Loading
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,160 @@
1+
// Copyright 2024 TIER IV, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef MRM_HANDLER__MRM_HANDLER_CORE_HPP_
16+
#define MRM_HANDLER__MRM_HANDLER_CORE_HPP_
17+
18+
// Core
19+
#include <memory>
20+
#include <optional>
21+
#include <string>
22+
#include <variant>
23+
24+
// Autoware
25+
#include <autoware_adapi_v1_msgs/msg/mrm_state.hpp>
26+
#include <autoware_adapi_v1_msgs/msg/operation_mode_state.hpp>
27+
#include <autoware_auto_vehicle_msgs/msg/control_mode_report.hpp>
28+
#include <autoware_auto_vehicle_msgs/msg/gear_command.hpp>
29+
#include <autoware_auto_vehicle_msgs/msg/hazard_lights_command.hpp>
30+
#include <tier4_system_msgs/msg/mrm_behavior_status.hpp>
31+
#include <tier4_system_msgs/msg/operation_mode_availability.hpp>
32+
#include <tier4_system_msgs/srv/operate_mrm.hpp>
33+
34+
// ROS 2 core
35+
#include <rclcpp/create_timer.hpp>
36+
#include <rclcpp/rclcpp.hpp>
37+
38+
#include <diagnostic_msgs/msg/diagnostic_array.hpp>
39+
#include <nav_msgs/msg/odometry.hpp>
40+
41+
struct HazardLampPolicy
42+
{
43+
bool emergency;
44+
};
45+
46+
struct Param
47+
{
48+
int update_rate;
49+
double timeout_operation_mode_availability;
50+
bool use_emergency_holding;
51+
double timeout_emergency_recovery;
52+
double timeout_takeover_request;
53+
bool use_takeover_request;
54+
bool use_parking_after_stopped;
55+
bool use_pull_over;
56+
bool use_comfortable_stop;
57+
HazardLampPolicy turning_hazard_on{};
58+
};
59+
60+
class MrmHandler : public rclcpp::Node
61+
{
62+
public:
63+
MrmHandler();
64+
65+
private:
66+
// Subscribers
67+
rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr sub_odom_;
68+
rclcpp::Subscription<autoware_auto_vehicle_msgs::msg::ControlModeReport>::SharedPtr
69+
sub_control_mode_;
70+
rclcpp::Subscription<tier4_system_msgs::msg::OperationModeAvailability>::SharedPtr
71+
sub_operation_mode_availability_;
72+
rclcpp::Subscription<tier4_system_msgs::msg::MrmBehaviorStatus>::SharedPtr
73+
sub_mrm_pull_over_status_;
74+
rclcpp::Subscription<tier4_system_msgs::msg::MrmBehaviorStatus>::SharedPtr
75+
sub_mrm_comfortable_stop_status_;
76+
rclcpp::Subscription<tier4_system_msgs::msg::MrmBehaviorStatus>::SharedPtr
77+
sub_mrm_emergency_stop_status_;
78+
rclcpp::Subscription<autoware_adapi_v1_msgs::msg::OperationModeState>::SharedPtr
79+
sub_operation_mode_state_;
80+
81+
nav_msgs::msg::Odometry::ConstSharedPtr odom_;
82+
autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr control_mode_;
83+
tier4_system_msgs::msg::OperationModeAvailability::ConstSharedPtr operation_mode_availability_;
84+
tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr mrm_pull_over_status_;
85+
tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr mrm_comfortable_stop_status_;
86+
tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr mrm_emergency_stop_status_;
87+
autoware_adapi_v1_msgs::msg::OperationModeState::ConstSharedPtr operation_mode_state_;
88+
89+
void onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg);
90+
void onControlMode(const autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg);
91+
void onOperationModeAvailability(
92+
const tier4_system_msgs::msg::OperationModeAvailability::ConstSharedPtr msg);
93+
void onMrmPullOverStatus(const tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr msg);
94+
void onMrmComfortableStopStatus(
95+
const tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr msg);
96+
void onMrmEmergencyStopStatus(
97+
const tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr msg);
98+
void onOperationModeState(
99+
const autoware_adapi_v1_msgs::msg::OperationModeState::ConstSharedPtr msg);
100+
101+
// Publisher
102+
103+
// rclcpp::Publisher<tier4_vehicle_msgs::msg::ShiftStamped>::SharedPtr pub_shift_;
104+
// rclcpp::Publisher<tier4_vehicle_msgs::msg::TurnSignal>::SharedPtr pub_turn_signal_;
105+
rclcpp::Publisher<autoware_auto_vehicle_msgs::msg::HazardLightsCommand>::SharedPtr
106+
pub_hazard_cmd_;
107+
rclcpp::Publisher<autoware_auto_vehicle_msgs::msg::GearCommand>::SharedPtr pub_gear_cmd_;
108+
109+
autoware_auto_vehicle_msgs::msg::HazardLightsCommand createHazardCmdMsg();
110+
autoware_auto_vehicle_msgs::msg::GearCommand createGearCmdMsg();
111+
void publishControlCommands();
112+
113+
rclcpp::Publisher<autoware_adapi_v1_msgs::msg::MrmState>::SharedPtr pub_mrm_state_;
114+
115+
autoware_adapi_v1_msgs::msg::MrmState mrm_state_;
116+
void publishMrmState();
117+
118+
// Clients
119+
rclcpp::CallbackGroup::SharedPtr client_mrm_pull_over_group_;
120+
rclcpp::Client<tier4_system_msgs::srv::OperateMrm>::SharedPtr client_mrm_pull_over_;
121+
rclcpp::CallbackGroup::SharedPtr client_mrm_comfortable_stop_group_;
122+
rclcpp::Client<tier4_system_msgs::srv::OperateMrm>::SharedPtr client_mrm_comfortable_stop_;
123+
rclcpp::CallbackGroup::SharedPtr client_mrm_emergency_stop_group_;
124+
rclcpp::Client<tier4_system_msgs::srv::OperateMrm>::SharedPtr client_mrm_emergency_stop_;
125+
126+
void callMrmBehavior(
127+
const autoware_adapi_v1_msgs::msg::MrmState::_behavior_type & mrm_behavior) const;
128+
void cancelMrmBehavior(
129+
const autoware_adapi_v1_msgs::msg::MrmState::_behavior_type & mrm_behavior) const;
130+
void logMrmCallingResult(
131+
const tier4_system_msgs::srv::OperateMrm::Response & result, const std::string & behavior,
132+
bool is_call) const;
133+
134+
// Timer
135+
rclcpp::TimerBase::SharedPtr timer_;
136+
137+
// Parameters
138+
Param param_;
139+
140+
bool isDataReady();
141+
void onTimer();
142+
143+
// Heartbeat
144+
rclcpp::Time stamp_operation_mode_availability_;
145+
std::optional<rclcpp::Time> stamp_autonomous_become_unavailable_ = std::nullopt;
146+
147+
// Algorithm
148+
rclcpp::Time takeover_requested_time_;
149+
bool is_takeover_request_ = false;
150+
bool is_emergency_holding_ = false;
151+
void transitionTo(const int new_state);
152+
void updateMrmState();
153+
void operateMrm();
154+
autoware_adapi_v1_msgs::msg::MrmState::_behavior_type getCurrentMrmBehavior();
155+
bool isStopped();
156+
bool isEmergency() const;
157+
bool isArrivedAtGoal();
158+
};
159+
160+
#endif // MRM_HANDLER__MRM_HANDLER_CORE_HPP_
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,39 @@
1+
<launch>
2+
<arg name="input_operation_mode_availability" default="/system/operation_mode/availability"/>
3+
<!-- To be replaced by ControlCommand -->
4+
<arg name="input_odometry" default="/localization/kinematic_state"/>
5+
<arg name="input_control_mode" default="/vehicle/status/control_mode"/>
6+
<arg name="input_mrm_pull_over_state" default="/system/mrm/pull_over_manager/status"/>
7+
<arg name="input_mrm_comfortable_stop_state" default="/system/mrm/comfortable_stop/status"/>
8+
<arg name="input_mrm_emergency_stop_state" default="/system/mrm/emergency_stop/status"/>
9+
<arg name="input_api_operation_mode_state" default="/api/operation_mode/state"/>
10+
11+
<arg name="output_gear" default="/system/emergency/gear_cmd"/>
12+
<arg name="output_hazard" default="/system/emergency/hazard_lights_cmd"/>
13+
<arg name="output_mrm_state" default="/system/fail_safe/mrm_state"/>
14+
<arg name="output_mrm_pull_over_operate" default="/system/mrm/pull_over_manager/operate"/>
15+
<arg name="output_mrm_comfortable_stop_operate" default="/system/mrm/comfortable_stop/operate"/>
16+
<arg name="output_mrm_emergency_stop_operate" default="/system/mrm/emergency_stop/operate"/>
17+
18+
<arg name="config_file" default="$(find-pkg-share mrm_handler)/config/mrm_handler.param.yaml"/>
19+
20+
<!-- mrm_handler -->
21+
<node pkg="mrm_handler" exec="mrm_handler" name="mrm_handler" output="screen">
22+
<remap from="~/input/operation_mode_availability" to="$(var input_operation_mode_availability)"/>
23+
<remap from="~/input/odometry" to="$(var input_odometry)"/>
24+
<remap from="~/input/control_mode" to="$(var input_control_mode)"/>
25+
<remap from="~/input/mrm/pull_over/status" to="$(var input_mrm_pull_over_state)"/>
26+
<remap from="~/input/mrm/comfortable_stop/status" to="$(var input_mrm_comfortable_stop_state)"/>
27+
<remap from="~/input/mrm/emergency_stop/status" to="$(var input_mrm_emergency_stop_state)"/>
28+
<remap from="~/input/api/operation_mode/state" to="$(var input_api_operation_mode_state)"/>
29+
30+
<remap from="~/output/gear" to="$(var output_gear)"/>
31+
<remap from="~/output/hazard" to="$(var output_hazard)"/>
32+
<remap from="~/output/mrm/state" to="$(var output_mrm_state)"/>
33+
<remap from="~/output/mrm/pull_over/operate" to="$(var output_mrm_pull_over_operate)"/>
34+
<remap from="~/output/mrm/comfortable_stop/operate" to="$(var output_mrm_comfortable_stop_operate)"/>
35+
<remap from="~/output/mrm/emergency_stop/operate" to="$(var output_mrm_emergency_stop_operate)"/>
36+
37+
<param from="$(var config_file)"/>
38+
</node>
39+
</launch>

system/mrm_handler/package.xml

+31
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,31 @@
1+
<?xml version="1.0"?>
2+
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
3+
<package format="3">
4+
<name>mrm_handler</name>
5+
<version>0.1.0</version>
6+
<description>The mrm_handler ROS 2 package</description>
7+
<maintainer email="makoto.kurihara@tier4.jp">Makoto Kurihara</maintainer>
8+
<maintainer email="ryuta.kambe@tier4.jp">Ryuta Kambe</maintainer>
9+
<maintainer email="tetsuhiro.kawaguchi@tier4.jp">Tetsuhiro Kawaguchi</maintainer>
10+
<license>Apache License 2.0</license>
11+
12+
<buildtool_depend>ament_cmake_auto</buildtool_depend>
13+
<buildtool_depend>autoware_cmake</buildtool_depend>
14+
15+
<depend>autoware_adapi_v1_msgs</depend>
16+
<depend>autoware_auto_control_msgs</depend>
17+
<depend>autoware_auto_system_msgs</depend>
18+
<depend>autoware_auto_vehicle_msgs</depend>
19+
<depend>nav_msgs</depend>
20+
<depend>rclcpp</depend>
21+
<depend>std_msgs</depend>
22+
<depend>std_srvs</depend>
23+
<depend>tier4_system_msgs</depend>
24+
25+
<test_depend>ament_lint_auto</test_depend>
26+
<test_depend>autoware_lint_common</test_depend>
27+
28+
<export>
29+
<build_type>ament_cmake</build_type>
30+
</export>
31+
</package>

0 commit comments

Comments
 (0)