Skip to content

Commit aa56a0b

Browse files
TetsuKawaTetsuKawa
committed
feat: add commnad mode decider redundancy
Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com>
1 parent 662eca1 commit aa56a0b

File tree

4 files changed

+152
-9
lines changed

4 files changed

+152
-9
lines changed

‎system/autoware_command_mode_decider/CMakeLists.txt

+7
Original file line numberDiff line numberDiff line change
@@ -9,6 +9,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED
99
"src/command_mode_status.cpp"
1010
"src/command_mode_decider_base.cpp"
1111
"src/command_mode_decider.cpp"
12+
"src/command_mode_decider_redundancy.cpp"
1213
)
1314
target_include_directories(${PROJECT_NAME} PRIVATE "src")
1415

@@ -18,6 +19,12 @@ rclcpp_components_register_node(${PROJECT_NAME}
1819
EXECUTOR MultiThreadedExecutor
1920
)
2021

22+
rclcpp_components_register_node(${PROJECT_NAME}
23+
PLUGIN "autoware::command_mode_decider::CommandModeDeciderRedundancy"
24+
EXECUTABLE "command_mode_decider_redundancy_node"
25+
EXECUTOR MultiThreadedExecutor
26+
)
27+
2128
if(BUILD_TESTING)
2229
endif()
2330

‎system/autoware_command_mode_decider/launch/decider.launch.xml

+25-9
Original file line numberDiff line numberDiff line change
@@ -1,12 +1,28 @@
11
<launch>
2+
<arg name="is_redundant" default="false"/>
23
<arg name="config" default="$(find-pkg-share autoware_command_mode_decider)/config/default.param.yaml"/>
3-
<node pkg="autoware_command_mode_decider" exec="command_mode_decider_node">
4-
<param from="$(var config)"/>
5-
<remap from="~/command_mode/status" to="/system/command_mode/status"/>
6-
<remap from="~/command_mode/request" to="/system/command_mode/request"/>
7-
<remap from="~/operation_mode/state" to="/system/operation_mode/state"/>
8-
<remap from="~/operation_mode/change_operation_mode" to="/system/operation_mode/change_operation_mode"/>
9-
<remap from="~/mrm/state" to="/system/fail_safe/mrm_state"/>
10-
<remap from="~/mrm/request" to="/system/fail_safe/mrm_request"/>
11-
</node>
4+
5+
<group if="$(var is_redundant)">
6+
<node pkg="autoware_command_mode_decider" exec="command_mode_decider_redundancy_node">
7+
<param from="$(var config)"/>
8+
<remap from="~/command_mode/status" to="/system/command_mode/status"/>
9+
<remap from="~/command_mode/request" to="/system/command_mode/request"/>
10+
<remap from="~/operation_mode/state" to="/system/operation_mode/state"/>
11+
<remap from="~/operation_mode/change_operation_mode" to="/system/operation_mode/change_operation_mode"/>
12+
<remap from="~/mrm/state" to="/system/fail_safe/mrm_state"/>
13+
<remap from="~/mrm/request" to="/system/fail_safe/mrm_request"/>
14+
</node>
15+
</group>
16+
17+
<group unless="$(var is_redundant)">
18+
<node pkg="autoware_command_mode_decider" exec="command_mode_decider_node">
19+
<param from="$(var config)"/>
20+
<remap from="~/command_mode/status" to="/system/command_mode/status"/>
21+
<remap from="~/command_mode/request" to="/system/command_mode/request"/>
22+
<remap from="~/operation_mode/state" to="/system/operation_mode/state"/>
23+
<remap from="~/operation_mode/change_operation_mode" to="/system/operation_mode/change_operation_mode"/>
24+
<remap from="~/mrm/state" to="/system/fail_safe/mrm_state"/>
25+
<remap from="~/mrm/request" to="/system/fail_safe/mrm_request"/>
26+
</node>
27+
</group>
1228
</launch>

‎system/autoware_command_mode_decider/src/command_mode_decider_redundancy.cpp

+80
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,80 @@
1+
// Copyright 2025 The Autoware Contributors
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+
#include "command_mode_decider_redundancy.hpp"
16+
17+
#include <string>
18+
19+
namespace autoware::command_mode_decider
20+
{
21+
22+
CommandModeDeciderRedundancy::CommandModeDeciderRedundancy(const rclcpp::NodeOptions & options)
23+
: CommandModeDeciderBase(options)
24+
{
25+
}
26+
27+
std::string CommandModeDeciderRedundancy::decide_command_mode()
28+
{
29+
const auto command_mode_status = get_command_mode_status();
30+
const auto request_mode_status = get_request_mode_status();
31+
32+
// Use the requested MRM if available.
33+
{
34+
const auto status = command_mode_status.get(request_mode_status.mrm);
35+
if (status.available) {
36+
return request_mode_status.mrm;
37+
}
38+
}
39+
40+
// Use the specified operation mode if available.
41+
{
42+
const auto status = command_mode_status.get(request_mode_status.operation_mode);
43+
if (status.available) {
44+
return request_mode_status.operation_mode;
45+
}
46+
}
47+
48+
// TODO(Takagi, Isamu): Use the available MRM according to the state transitions at the
49+
// following.
50+
// https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-interfaces/ad-api/features/fail-safe/#behavior
51+
const auto comfortable_stop = "comfortable_stop";
52+
const auto main_ecu_in_lane_stop_0_4g = "main_ecu_in_lane_stop_0_4g";
53+
const auto main_ecu_in_lane_stop_0_6g = "main_ecu_in_lane_stop_0_6g";
54+
const auto sub_ecu_in_lane_stop_0_4g = "sub_ecu_in_lane_stop_0_4g";
55+
56+
// TODO(Takagi, Isamu): check command_modes parameter
57+
if (command_mode_status.get(comfortable_stop).available /*&& use_comfortable_stop_*/) {
58+
return comfortable_stop;
59+
}
60+
if (command_mode_status.get(main_ecu_in_lane_stop_0_4g).available /*&& use_main_ecu_in_lane_stop_0_4g_*/) {
61+
return main_ecu_in_lane_stop_0_4g;
62+
}
63+
if (command_mode_status.get(main_ecu_in_lane_stop_0_6g).available /*&& use_main_ecu_in_lane_stop_0_6g_*/) {
64+
return main_ecu_in_lane_stop_0_6g;
65+
}
66+
if (command_mode_status.get(sub_ecu_in_lane_stop_0_4g).available /*&& use_sub_ecu_in_lane_stop_0_4g_*/) {
67+
return sub_ecu_in_lane_stop_0_4g;
68+
}
69+
// FIXME(TetsuKawa): How to handle the case where no MRM is available.
70+
71+
// Use an empty string to delegate to switcher node.
72+
RCLCPP_WARN_THROTTLE(
73+
get_logger(), *get_clock(), 5000, "no mrm available: delegate to switcher node");
74+
return std::string();
75+
}
76+
77+
} // namespace autoware::command_mode_decider
78+
79+
#include <rclcpp_components/register_node_macro.hpp>
80+
RCLCPP_COMPONENTS_REGISTER_NODE(autoware::command_mode_decider::CommandModeDeciderRedundancy)

‎system/autoware_command_mode_decider/src/command_mode_decider_redundancy.hpp

+40
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,40 @@
1+
// Copyright 2025 The Autoware Contributors
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 COMMAND_MODE_DECIDER_REDUNDANCY_HPP_
16+
#define COMMAND_MODE_DECIDER_REDUNDANCY_HPP_
17+
18+
#include "command_mode_decider_base.hpp"
19+
20+
#include <string>
21+
22+
namespace autoware::command_mode_decider
23+
{
24+
25+
class CommandModeDeciderRedundancy : public CommandModeDeciderBase
26+
{
27+
public:
28+
explicit CommandModeDeciderRedundancy(const rclcpp::NodeOptions & options);
29+
30+
protected:
31+
std::string decide_command_mode() override;
32+
33+
private:
34+
bool use_pull_over_;
35+
bool use_comfortable_stop_;
36+
};
37+
38+
} // namespace autoware::command_mode_decider
39+
40+
#endif // COMMAND_MODE_DECIDER_REDUNDANCY_HPP_

0 commit comments

Comments
 (0)