Skip to content

Commit 70c10fe

Browse files
veqccveqcc
committed
update(emergency_handler) change MRM operators to classes inside Emergency Handler
Signed-off-by: veqcc <ryuta.kambe@tier4.jp>
1 parent e2aee50 commit 70c10fe

14 files changed

+461
-134
lines changed

‎launch/tier4_system_launch/launch/system.launch.xml

-12
Original file line numberDiff line numberDiff line change
@@ -90,18 +90,6 @@
9090
<arg name="config_file" value="$(var duplicated_node_checker_param_path)"/>
9191
</include>
9292
</group>
93-
94-
<!-- MRM Operator -->
95-
<group>
96-
<include file="$(find-pkg-share mrm_comfortable_stop_operator)/launch/mrm_comfortable_stop_operator.launch.py">
97-
<arg name="config_file" value="$(var mrm_comfortable_stop_operator_param_path)"/>
98-
</include>
99-
</group>
100-
<group>
101-
<include file="$(find-pkg-share mrm_emergency_stop_operator)/launch/mrm_emergency_stop_operator.launch.py">
102-
<arg name="config_file" value="$(var mrm_emergency_stop_operator_param_path)"/>
103-
</include>
104-
</group>
10593
</group>
10694

10795
<!-- Dummy Diag Publisher -->

‎system/emergency_handler/CMakeLists.txt

+2
Original file line numberDiff line numberDiff line change
@@ -7,6 +7,8 @@ autoware_package()
77
ament_auto_add_executable(emergency_handler
88
src/emergency_handler/emergency_handler_node.cpp
99
src/emergency_handler/emergency_handler_core.cpp
10+
src/operators/emergency_stop.cpp
11+
src/operators/comfortable_stop.cpp
1012
)
1113

1214
ament_auto_package(INSTALL_TO_SHARE

‎system/emergency_handler/README.md

+22-11
Original file line numberDiff line numberDiff line change
@@ -2,10 +2,22 @@
22

33
## Purpose
44

5-
Emergency Handler is a node to select proper MRM from from system failure state contained in HazardStatus.
5+
Emergency Handler is a node to select proper MRM from system failure state contained in HazardStatus.
66

77
## Inner-workings / Algorithms
88

9+
Emergency Handler has MRM operators inside.
10+
Currently, there are two types of MRM:
11+
12+
- Emergency Stop Operator: publishes `/system/emergency/control_cmd` to make an emergency stop
13+
- Comfortable Stop Operator: publishes `/planning/scenario_planning/max_velocity_candidates` to make a comfortable stop
14+
15+
These operators have `operate()`, `cancel()` and `onTimer` public functions.
16+
17+
- `operate()`: executes the MRM operation
18+
- `cancel()`: cancels the MRM operation
19+
- `onTimer()`: executes each operator's timer callback
20+
921
### State Transitions
1022

1123
![fail-safe-state](image/fail-safe-state.drawio.svg)
@@ -17,21 +29,20 @@ Emergency Handler is a node to select proper MRM from from system failure state
1729
| Name | Type | Description |
1830
| ----------------------------------------- | ---------------------------------------------------------- | ----------------------------------------------------------------------------- |
1931
| `/system/emergency/hazard_status` | `autoware_auto_system_msgs::msg::HazardStatusStamped` | Used to select proper MRM from system failure state contained in HazardStatus |
20-
| `/control/vehicle_cmd` | `autoware_auto_control_msgs::msg::AckermannControlCommand` | Used as reference when generate Emergency Control Command |
2132
| `/localization/kinematic_state` | `nav_msgs::msg::Odometry` | Used to decide whether vehicle is stopped or not |
2233
| `/vehicle/status/control_mode` | `autoware_auto_vehicle_msgs::msg::ControlModeReport` | Used to check vehicle mode: autonomous or manual |
23-
| `/system/api/mrm/comfortable_stop/status` | `tier4_system_msgs::msg::MrmBehaviorStatus` | Used to check if MRM comfortable stop operation is available |
24-
| `/system/api/mrm/emergency_stop/status` | `tier4_system_msgs::msg::MrmBehaviorStatus` | Used to check if MRM emergency stop operation is available |
34+
| `/control/command/control_cmd` | `autoware_auto_control_msgs::msg::AckermannControlCommand` | Used to calculate emergency control commands in emergency_stop_operator |
2535

2636
### Output
2737

28-
| Name | Type | Description |
29-
| ------------------------------------------ | ------------------------------------------------------ | ----------------------------------------------------- |
30-
| `/system/emergency/shift_cmd` | `autoware_auto_vehicle_msgs::msg::GearCommand` | Required to execute proper MRM (send gear cmd) |
31-
| `/system/emergency/hazard_cmd` | `autoware_auto_vehicle_msgs::msg::HazardLightsCommand` | Required to execute proper MRM (send turn signal cmd) |
32-
| `/api/fail_safe/mrm_state` | `autoware_adapi_v1_msgs::msg::MrmState` | Inform MRM execution state and selected MRM behavior |
33-
| `/system/api/mrm/comfortable_stop/operate` | `tier4_system_msgs::srv::OperateMrm` | Execution order for MRM comfortable stop |
34-
| `/system/api/mrm/emergency_stop/operate` | `tier4_system_msgs::srv::OperateMrm` | Execution order for MRM emergency stop |
38+
| Name | Type | Description |
39+
| ----------------------------------------------------- | ---------------------------------------------------------- | ----------------------------------------------------- |
40+
| `/system/emergency/shift_cmd` | `autoware_auto_vehicle_msgs::msg::GearCommand` | Required to execute proper MRM (send gear cmd) |
41+
| `/system/emergency/hazard_cmd` | `autoware_auto_vehicle_msgs::msg::HazardLightsCommand` | Required to execute proper MRM (send turn signal cmd) |
42+
| `/api/fail_safe/mrm_state` | `autoware_adapi_v1_msgs::msg::MrmState` | Inform MRM execution state and selected MRM behavior |
43+
| `/system/emergency/control_cmd` | `autoware_auto_control_msgs::msg::AckermannControlCommand` | Required to operate emergency_stop |
44+
| `/planning/scenario_planning/max_velocity_candidates` | `tier4_planning_msgs::msg::VelocityLimit` | Required to operate comfortable_stop |
45+
| `/planning/scenario_planning/clear_velocity_limit` | `tier4_planning_msgs::msg::VelocityLimitClearCommand` | Required to cancel comfortable_stop |
3546

3647
## Parameters
3748

‎system/emergency_handler/config/emergency_handler.param.yaml

+11
Original file line numberDiff line numberDiff line change
@@ -10,3 +10,14 @@
1010
# setting whether to turn hazard lamp on for each situation
1111
turning_hazard_on:
1212
emergency: true
13+
14+
# emergency stop operator class
15+
emergency_stop:
16+
target_acceleration: -2.5
17+
target_jerk: -1.5
18+
19+
# comfortable stop operator class
20+
comfortable_stop:
21+
min_acceleration: -1.0
22+
max_jerk: 0.3
23+
min_jerk: -0.3

‎system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp

+14-21
Original file line numberDiff line numberDiff line change
@@ -36,6 +36,13 @@
3636
#include <diagnostic_msgs/msg/diagnostic_array.hpp>
3737
#include <nav_msgs/msg/odometry.hpp>
3838

39+
// Operators
40+
#include "operators/emergency_stop.hpp"
41+
#include "operators/comfortable_stop.hpp"
42+
43+
namespace emergency_handler
44+
{
45+
3946
struct HazardLampPolicy
4047
{
4148
bool emergency;
@@ -48,6 +55,7 @@ struct Param
4855
bool use_parking_after_stopped;
4956
bool use_comfortable_stop;
5057
HazardLampPolicy turning_hazard_on{};
58+
comfortable_stop_operator::Param comfortable_stop{};
5159
};
5260

5361
class EmergencyHandler : public rclcpp::Node
@@ -59,33 +67,18 @@ class EmergencyHandler : public rclcpp::Node
5967
// Subscribers
6068
rclcpp::Subscription<autoware_auto_system_msgs::msg::HazardStatusStamped>::SharedPtr
6169
sub_hazard_status_stamped_;
62-
rclcpp::Subscription<autoware_auto_control_msgs::msg::AckermannControlCommand>::SharedPtr
63-
sub_prev_control_command_;
6470
rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr sub_odom_;
6571
rclcpp::Subscription<autoware_auto_vehicle_msgs::msg::ControlModeReport>::SharedPtr
6672
sub_control_mode_;
67-
rclcpp::Subscription<tier4_system_msgs::msg::MrmBehaviorStatus>::SharedPtr
68-
sub_mrm_comfortable_stop_status_;
69-
rclcpp::Subscription<tier4_system_msgs::msg::MrmBehaviorStatus>::SharedPtr
70-
sub_mrm_emergency_stop_status_;
7173

7274
autoware_auto_system_msgs::msg::HazardStatusStamped::ConstSharedPtr hazard_status_stamped_;
73-
autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr prev_control_command_;
7475
nav_msgs::msg::Odometry::ConstSharedPtr odom_;
7576
autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr control_mode_;
76-
tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr mrm_comfortable_stop_status_;
77-
tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr mrm_emergency_stop_status_;
7877

7978
void onHazardStatusStamped(
8079
const autoware_auto_system_msgs::msg::HazardStatusStamped::ConstSharedPtr msg);
81-
void onPrevControlCommand(
82-
const autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr msg);
8380
void onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg);
8481
void onControlMode(const autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg);
85-
void onMrmComfortableStopStatus(
86-
const tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr msg);
87-
void onMrmEmergencyStopStatus(
88-
const tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr msg);
8982

9083
// Publisher
9184
rclcpp::Publisher<autoware_auto_control_msgs::msg::AckermannControlCommand>::SharedPtr
@@ -106,12 +99,6 @@ class EmergencyHandler : public rclcpp::Node
10699
autoware_adapi_v1_msgs::msg::MrmState mrm_state_;
107100
void publishMrmState();
108101

109-
// Clients
110-
rclcpp::CallbackGroup::SharedPtr client_mrm_comfortable_stop_group_;
111-
rclcpp::Client<tier4_system_msgs::srv::OperateMrm>::SharedPtr client_mrm_comfortable_stop_;
112-
rclcpp::CallbackGroup::SharedPtr client_mrm_emergency_stop_group_;
113-
rclcpp::Client<tier4_system_msgs::srv::OperateMrm>::SharedPtr client_mrm_emergency_stop_;
114-
115102
void callMrmBehavior(
116103
const autoware_adapi_v1_msgs::msg::MrmState::_behavior_type & mrm_behavior) const;
117104
void cancelMrmBehavior(
@@ -134,6 +121,10 @@ class EmergencyHandler : public rclcpp::Node
134121
bool is_hazard_status_timeout_;
135122
void checkHazardStatusTimeout();
136123

124+
// Operators
125+
std::unique_ptr<emergency_stop_operator::EmergencyStopOperator> emergency_stop_operator_;
126+
std::unique_ptr<comfortable_stop_operator::ComfortableStopOperator> comfortable_stop_operator_;
127+
137128
// Algorithm
138129
void transitionTo(const int new_state);
139130
void updateMrmState();
@@ -143,4 +134,6 @@ class EmergencyHandler : public rclcpp::Node
143134
bool isEmergency();
144135
};
145136

137+
} // namespace emergency_handler
138+
146139
#endif // EMERGENCY_HANDLER__EMERGENCY_HANDLER_CORE_HPP_

‎system/emergency_handler/include/operators/comfortable_stop.hpp

+64
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,64 @@
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 COMFORTABLE_STOP_HPP_
16+
#define COMFORTABLE_STOP_HPP_
17+
18+
#include <tier4_planning_msgs/msg/velocity_limit.hpp>
19+
#include <tier4_planning_msgs/msg/velocity_limit_clear_command.hpp>
20+
21+
#include <rclcpp/rclcpp.hpp>
22+
23+
namespace emergency_handler
24+
{
25+
26+
namespace comfortable_stop_operator
27+
{
28+
29+
struct Param
30+
{
31+
double min_acceleration;
32+
double max_jerk;
33+
double min_jerk;
34+
};
35+
36+
class ComfortableStopOperator
37+
{
38+
public:
39+
explicit ComfortableStopOperator(rclcpp::Node * node);
40+
bool operate();
41+
bool cancel();
42+
void onTimer();
43+
44+
private:
45+
// Parameters
46+
Param params_;
47+
48+
// Publisher
49+
rclcpp::Publisher<tier4_planning_msgs::msg::VelocityLimit>::SharedPtr pub_velocity_limit_;
50+
rclcpp::Publisher<tier4_planning_msgs::msg::VelocityLimitClearCommand>::SharedPtr
51+
pub_velocity_limit_clear_command_;
52+
53+
// Algorithm
54+
void publishVelocityLimit();
55+
void publishVelocityLimitClearCommand();
56+
57+
rclcpp::Node * node_;
58+
};
59+
60+
} // namespace comfortable_stop_operator
61+
62+
} // namespace emergency_handler
63+
64+
#endif // COMFORTABLE_STOP_HPP_

‎system/emergency_handler/include/operators/emergency_stop.hpp

+68
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,68 @@
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 EMERGENCY_STOP_HPP_
16+
#define EMERGENCY_STOP_HPP_
17+
18+
#include <autoware_auto_control_msgs/msg/ackermann_control_command.hpp>
19+
20+
#include <rclcpp/rclcpp.hpp>
21+
22+
23+
namespace emergency_handler
24+
{
25+
26+
namespace emergency_stop_operator
27+
{
28+
29+
using autoware_auto_control_msgs::msg::AckermannControlCommand;
30+
31+
struct Param
32+
{
33+
double target_acceleration;
34+
double target_jerk;
35+
};
36+
37+
class EmergencyStopOperator
38+
{
39+
public:
40+
explicit EmergencyStopOperator(rclcpp::Node * node);
41+
bool operate();
42+
bool cancel();
43+
void onTimer();
44+
45+
private:
46+
// Parameters
47+
Param params_;
48+
49+
// Subscriber
50+
rclcpp::Subscription<AckermannControlCommand>::SharedPtr sub_control_cmd_;
51+
void onControlCommand(AckermannControlCommand::ConstSharedPtr msg);
52+
53+
// Publisher
54+
rclcpp::Publisher<AckermannControlCommand>::SharedPtr pub_control_cmd_;
55+
56+
// Alrogithm
57+
bool is_prev_control_cmd_subscribed_;
58+
AckermannControlCommand prev_control_cmd_;
59+
AckermannControlCommand calcNextControlCmd();
60+
61+
rclcpp::Node * node_;
62+
};
63+
64+
} // namespace emergency_stop_operator
65+
66+
} // namespace emergency_handler
67+
68+
#endif // EMERGENCY_STOP_HPP_

‎system/emergency_handler/launch/emergency_handler.launch.xml

+17-13
Original file line numberDiff line numberDiff line change
@@ -1,35 +1,39 @@
11
<launch>
2+
<!-- emergency_handler -->
23
<arg name="input_hazard_status" default="/system/emergency/hazard_status"/>
3-
<!-- To be replaced by ControlCommand -->
4-
<arg name="input_prev_control_command" default="/control/command/control_cmd"/>
54
<arg name="input_odometry" default="/localization/kinematic_state"/>
65
<arg name="input_control_mode" default="/vehicle/status/control_mode"/>
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-
106
<arg name="output_gear" default="/system/emergency/gear_cmd"/>
117
<arg name="output_hazard" default="/system/emergency/hazard_lights_cmd"/>
128
<arg name="output_mrm_state" default="/system/fail_safe/mrm_state"/>
13-
<arg name="output_mrm_comfortable_stop_operate" default="/system/mrm/comfortable_stop/operate"/>
14-
<arg name="output_mrm_emergency_stop_operate" default="/system/mrm/emergency_stop/operate"/>
159

1610
<arg name="config_file" default="$(find-pkg-share emergency_handler)/config/emergency_handler.param.yaml"/>
1711

12+
<!-- emergency stop operator class-->
13+
<arg name="input_control_command" default="/control/command/control_cmd"/>
14+
<arg name="output_emergency_command" default="/system/emergency/control_cmd"/>
15+
16+
<!-- comfortable stop operator class -->
17+
<arg name="output_velocity_limit" default="/planning/scenario_planning/max_velocity_candidates"/>
18+
<arg name="output_velocity_limit_clear" default="/planning/scenario_planning/clear_velocity_limit"/>
19+
1820
<!-- emergency_handler -->
1921
<node pkg="emergency_handler" exec="emergency_handler" name="emergency_handler" output="screen">
2022
<remap from="~/input/hazard_status" to="$(var input_hazard_status)"/>
21-
<remap from="~/input/prev_control_command" to="$(var input_prev_control_command)"/>
2223
<remap from="~/input/odometry" to="$(var input_odometry)"/>
2324
<remap from="~/input/control_mode" to="$(var input_control_mode)"/>
24-
<remap from="~/input/mrm/comfortable_stop/status" to="$(var input_mrm_comfortable_stop_state)"/>
25-
<remap from="~/input/mrm/emergency_stop/status" to="$(var input_mrm_emergency_stop_state)"/>
26-
2725
<remap from="~/output/gear" to="$(var output_gear)"/>
2826
<remap from="~/output/hazard" to="$(var output_hazard)"/>
2927
<remap from="~/output/mrm/state" to="$(var output_mrm_state)"/>
30-
<remap from="~/output/mrm/comfortable_stop/operate" to="$(var output_mrm_comfortable_stop_operate)"/>
31-
<remap from="~/output/mrm/emergency_stop/operate" to="$(var output_mrm_emergency_stop_operate)"/>
3228

3329
<param from="$(var config_file)"/>
30+
31+
<!-- emergency stop operator class -->
32+
<remap from="~/input/control/control_cmd" to="$(var input_control_command)"/>
33+
<remap from="~/output/mrm/emergency_stop/control_cmd" to="$(var output_emergency_command)"/>
34+
35+
<!-- comfortable stop operator class -->
36+
<remap from="~/output/velocity_limit" to="$(var output_velocity_limit)"/>
37+
<remap from="~/output/velocity_limit/clear" to="$(var output_velocity_limit_clear)"/>
3438
</node>
3539
</launch>

‎system/emergency_handler/package.xml

+1
Original file line numberDiff line numberDiff line change
@@ -20,6 +20,7 @@
2020
<depend>rclcpp</depend>
2121
<depend>std_msgs</depend>
2222
<depend>std_srvs</depend>
23+
<depend>tier4_planning_msgs</depend>
2324
<depend>tier4_system_msgs</depend>
2425

2526
<test_depend>ament_lint_auto</test_depend>

0 commit comments

Comments
 (0)