Skip to content
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.

Commit f173fad

Browse files
soblinmkuriTomohitoAndo
authoredMar 11, 2024
feat(hazard_lights_selector): add hazard_lights_selector (autowarefoundation#1157)
* feat(hazard_lights_selector): turn on hazard lights from mrm comfortable stop operator (autowarefoundation#323) * chore(hazard_lights_selector): add the hazard_lights_selector package Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com> * chore(hazard_lights_selector): implement a header file Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com> * chore(hazard_lights_selector): implement the hazard_lights_selector package Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com> * chore(hazard_lights_selector): remove a current hazard lights command state Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com> * refacotr(hazard_lights_selector): remove extra lines Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com> * feat(mrm_comfortable_stop_operator): add hazard lights publisher Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com> * chore(hazard_lights_selector): add a maintainer Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com> * style: pre-commit fixes Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> --------- Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com> Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> Co-authored-by: Tomohito Ando <tomohito.ando@tier4.jp> * migrate to latest Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> --------- Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com> Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp> Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> Co-authored-by: Makoto Kurihara <mkuri8m@gmail.com> Co-authored-by: Tomohito Ando <tomohito.ando@tier4.jp>
1 parent 9133ea1 commit f173fad

File tree

13 files changed

+321
-5
lines changed

13 files changed

+321
-5
lines changed
 

‎launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml

+1-1
Original file line numberDiff line numberDiff line change
@@ -194,7 +194,7 @@
194194
<remap from="~/input/scenario" to="/planning/scenario_planning/scenario"/>
195195
<remap from="~/output/path" to="path_with_lane_id"/>
196196
<remap from="~/output/turn_indicators_cmd" to="/planning/turn_indicators_cmd"/>
197-
<remap from="~/output/hazard_lights_cmd" to="/planning/hazard_lights_cmd"/>
197+
<remap from="~/output/hazard_lights_cmd" to="/planning/behavior_path_planner/hazard_lights_cmd"/>
198198
<remap from="~/output/modified_goal" to="/planning/scenario_planning/modified_goal"/>
199199
<remap from="~/output/stop_reasons" to="/planning/scenario_planning/status/stop_reasons"/>
200200
<!-- params -->

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

+8-4
Original file line numberDiff line numberDiff line change
@@ -6,10 +6,7 @@
66
<arg name="duplicated_node_checker_param_path"/>
77
<arg name="mrm_comfortable_stop_operator_param_path"/>
88
<arg name="mrm_emergency_stop_operator_param_path"/>
9-
<arg name="system_error_monitor_param_path"/>
10-
<arg name="system_error_monitor_planning_simulator_param_path"/>
11-
<arg name="diagnostic_aggregator_system_param_path"/>
12-
<arg name="diagnostic_aggregator_vehicle_param_path"/>
9+
<arg name="hazard_lights_selector_param_path"/>
1310
<arg name="dummy_diag_publisher_param_path"/>
1411
<arg name="system_monitor_cpu_monitor_param_path"/>
1512
<arg name="system_monitor_gpu_monitor_param_path"/>
@@ -104,6 +101,13 @@
104101
</group>
105102
</group>
106103

104+
<!-- hazard selector -->
105+
<group>
106+
<include file="$(find-pkg-share hazard_lights_selector)/launch/hazard_lights_selector.launch.xml">
107+
<arg name="param_path" value="$(var hazard_lights_selector_param_path)"/>
108+
</include>
109+
</group>
110+
107111
<!-- Dummy Diag Publisher -->
108112
<group if="$(var launch_dummy_diag_publisher)">
109113
<include file="$(find-pkg-share dummy_diag_publisher)/launch/dummy_diag_publisher.launch.xml">

‎system/hazard_lights_selector/CMakeLists.txt

+19
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,19 @@
1+
cmake_minimum_required(VERSION 3.14)
2+
project(hazard_lights_selector)
3+
4+
find_package(autoware_cmake REQUIRED)
5+
autoware_package()
6+
7+
ament_auto_add_library(hazard_lights_selector_node SHARED
8+
src/hazard_lights_selector_node.cpp
9+
)
10+
11+
rclcpp_components_register_node(hazard_lights_selector_node
12+
PLUGIN "hazard_lights_selector::HazardLightsSelectorNode"
13+
EXECUTABLE hazard_lights_selector
14+
)
15+
16+
ament_auto_package(INSTALL_TO_SHARE
17+
launch
18+
config
19+
)

‎system/hazard_lights_selector/README.md

+80
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,80 @@
1+
# Hazard lights selector
2+
3+
## Purpose
4+
5+
## Inner-workings / Algorithms
6+
7+
<!-- Write how this package works. Flowcharts and figures are great. Add sub-sections as you like.
8+
9+
Example:
10+
### Flowcharts
11+
12+
...(PlantUML or something)
13+
14+
### State Transitions
15+
16+
...(PlantUML or something)
17+
18+
### How to filter target obstacles
19+
20+
...
21+
22+
### How to optimize trajectory
23+
24+
...
25+
-->
26+
27+
## Inputs
28+
29+
## Outputs
30+
31+
## Parameters
32+
33+
## Assumptions / Known limits
34+
35+
<!-- Write assumptions and limitations of your implementation.
36+
37+
Example:
38+
This algorithm assumes obstacles are not moving, so if they rapidly move after the vehicle started to avoid them, it might collide with them.
39+
Also, this algorithm doesn't care about blind spots. In general, since too close obstacles aren't visible due to the sensing performance limit, please take enough margin to obstacles.
40+
-->
41+
42+
## (Optional) Error detection and handling
43+
44+
<!-- Write how to detect errors and how to recover from them.
45+
46+
Example:
47+
This package can handle up to 20 obstacles. If more obstacles found, this node will give up and raise diagnostic errors.
48+
-->
49+
50+
## (Optional) Performance characterization
51+
52+
<!-- Write performance information like complexity. If it wouldn't be the bottleneck, not necessary.
53+
54+
Example:
55+
### Complexity
56+
57+
This algorithm is O(N).
58+
59+
### Processing time
60+
61+
...
62+
-->
63+
64+
## (Optional) References/External links
65+
66+
<!-- Write links you referred to when you implemented.
67+
68+
Example:
69+
[1] {link_to_a_thesis}
70+
[2] {link_to_an_issue}
71+
-->
72+
73+
## (Optional) Future extensions / Unimplemented parts
74+
75+
<!-- Write future extensions of this package.
76+
77+
Example:
78+
Currently, this package can't handle the chattering obstacles well. We plan to add some probabilistic filters in the perception layer to improve it.
79+
Also, there are some parameters that should be global(e.g. vehicle size, max steering, etc.). These will be refactored and defined as global parameters so that we can share the same parameters between different nodes.
80+
-->

‎system/hazard_lights_selector/config/hazard_lights_selector.param.yaml

+3
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,3 @@
1+
/**:
2+
ros__parameters:
3+
update_rate: 10 # hazard lights command publish rate [Hz]

‎system/hazard_lights_selector/include/hazard_lights_selector/hazard_lights_selector_node.hpp

+63
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,63 @@
1+
// Copyright 2021 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 HAZARD_LIGHTS_SELECTOR__HAZARD_LIGHTS_SELECTOR_NODE_HPP_
16+
#define HAZARD_LIGHTS_SELECTOR__HAZARD_LIGHTS_SELECTOR_NODE_HPP_
17+
18+
#include <rclcpp/rclcpp.hpp>
19+
20+
#include <autoware_auto_vehicle_msgs/msg/hazard_lights_command.hpp>
21+
22+
namespace hazard_lights_selector
23+
{
24+
using autoware_auto_vehicle_msgs::msg::HazardLightsCommand;
25+
26+
class HazardLightsSelectorNode : public rclcpp::Node
27+
{
28+
struct Parameters
29+
{
30+
int update_rate; // [Hz]
31+
};
32+
33+
public:
34+
explicit HazardLightsSelectorNode(const rclcpp::NodeOptions & node_options);
35+
36+
private:
37+
// Parameter
38+
Parameters params_;
39+
40+
// Subscriber
41+
rclcpp::Subscription<HazardLightsCommand>::SharedPtr sub_hazard_lights_cmd_from_path_planner_;
42+
rclcpp::Subscription<HazardLightsCommand>::SharedPtr sub_hazard_lights_cmd_from_mrm_operator_;
43+
44+
void onHazardLightsCommandFromPathPlanner(HazardLightsCommand::ConstSharedPtr msg);
45+
void onHazardLightsCommandFromMrmOperator(HazardLightsCommand::ConstSharedPtr msg);
46+
47+
// Publisher
48+
rclcpp::Publisher<HazardLightsCommand>::SharedPtr pub_hazard_lights_cmd_;
49+
50+
void publishHazardLightsCommand() const;
51+
52+
// Timer
53+
rclcpp::TimerBase::SharedPtr timer_;
54+
55+
void onTimer();
56+
57+
// State
58+
HazardLightsCommand::ConstSharedPtr hazard_lights_command_from_path_planner_;
59+
HazardLightsCommand::ConstSharedPtr hazard_lights_command_from_mrm_operator_;
60+
};
61+
} // namespace hazard_lights_selector
62+
63+
#endif // HAZARD_LIGHTS_SELECTOR__HAZARD_LIGHTS_SELECTOR_NODE_HPP_

‎system/hazard_lights_selector/launch/hazard_lights_selector.launch.xml

+16
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,16 @@
1+
<launch>
2+
<!-- common param -->
3+
<arg name="param_path" default="$(find-pkg-share hazard_lights_selector)/config/hazard_lights_selector.param.yaml"/>
4+
5+
<!-- input/output -->
6+
<arg name="input_hazard_lights_cmd_from_path_planner" default="/planning/behavior_path_planner/hazard_lights_cmd"/>
7+
<arg name="input_hazard_lights_cmd_from_mrm_operator" default="/system/mrm_comfortable_stop_operator/hazard_lights_cmd"/>
8+
<arg name="output_hazard_lights_cmd" default="/planning/hazard_lights_cmd"/>
9+
10+
<node pkg="hazard_lights_selector" exec="hazard_lights_selector" name="hazard_lights_selector" output="screen">
11+
<param from="$(var param_path)"/>
12+
<remap from="~/input/behavior_path_planner/hazard_lights_cmd" to="$(var input_hazard_lights_cmd_from_path_planner)"/>
13+
<remap from="~/input/behavior_mrm_operator/hazard_lights_cmd" to="$(var input_hazard_lights_cmd_from_mrm_operator)"/>
14+
<remap from="~/output/hazard_lights_cmd" to="$(var output_hazard_lights_cmd)"/>
15+
</node>
16+
</launch>

‎system/hazard_lights_selector/package.xml

+30
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,30 @@
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>hazard_lights_selector</name>
5+
<version>0.1.0</version>
6+
<description>The hazard_lights_selector ROS2 package</description>
7+
<maintainer email="makoto.kurihara@tier4.jp">Makoto Kurihara</maintainer>
8+
<maintainer email="tomohito.ando@tier4.jp">Tomohito Ando</maintainer>
9+
<license>Apache License 2.0</license>
10+
11+
<author email="makoto.kurihara@tier4.jp">Makoto Kurihara</author>
12+
13+
<buildtool_depend>ament_cmake_auto</buildtool_depend>
14+
15+
<build_depend>autoware_cmake</build_depend>
16+
17+
<depend>autoware_auto_vehicle_msgs</depend>
18+
<depend>rclcpp</depend>
19+
<depend>rclcpp_components</depend>
20+
21+
<exec_depend>ros2cli</exec_depend>
22+
<exec_depend>topic_tools</exec_depend>
23+
24+
<test_depend>ament_lint_auto</test_depend>
25+
<test_depend>autoware_lint_common</test_depend>
26+
27+
<export>
28+
<build_type>ament_cmake</build_type>
29+
</export>
30+
</package>

‎system/hazard_lights_selector/src/hazard_lights_selector_node.cpp

+80
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,80 @@
1+
// Copyright 2021 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+
#include <hazard_lights_selector/hazard_lights_selector_node.hpp>
16+
17+
namespace hazard_lights_selector
18+
{
19+
HazardLightsSelectorNode::HazardLightsSelectorNode(const rclcpp::NodeOptions & node_options)
20+
: Node("hazard_lights_selector", node_options)
21+
{
22+
using std::placeholders::_1;
23+
24+
// Parameter
25+
params_.update_rate = static_cast<int>(declare_parameter<int>("update_rate", 10));
26+
27+
// Subscriber
28+
sub_hazard_lights_cmd_from_path_planner_ = this->create_subscription<HazardLightsCommand>(
29+
"~/input/behavior_path_planner/hazard_lights_cmd", 1,
30+
std::bind(&HazardLightsSelectorNode::onHazardLightsCommandFromPathPlanner, this, _1));
31+
sub_hazard_lights_cmd_from_mrm_operator_ = this->create_subscription<HazardLightsCommand>(
32+
"~/input/behavior_mrm_operator/hazard_lights_cmd", 1,
33+
std::bind(&HazardLightsSelectorNode::onHazardLightsCommandFromMrmOperator, this, _1));
34+
35+
// Publisher
36+
pub_hazard_lights_cmd_ =
37+
this->create_publisher<HazardLightsCommand>("~/output/hazard_lights_cmd", 1);
38+
39+
// Timer
40+
const auto update_period_ns = rclcpp::Rate(params_.update_rate).period();
41+
timer_ = rclcpp::create_timer(
42+
this, get_clock(), update_period_ns, std::bind(&HazardLightsSelectorNode::onTimer, this));
43+
}
44+
45+
void HazardLightsSelectorNode::onHazardLightsCommandFromPathPlanner(
46+
HazardLightsCommand::ConstSharedPtr msg)
47+
{
48+
hazard_lights_command_from_path_planner_ = msg;
49+
}
50+
51+
void HazardLightsSelectorNode::onHazardLightsCommandFromMrmOperator(
52+
HazardLightsCommand::ConstSharedPtr msg)
53+
{
54+
hazard_lights_command_from_mrm_operator_ = msg;
55+
}
56+
57+
void HazardLightsSelectorNode::onTimer()
58+
{
59+
auto hazard_lights_cmd = HazardLightsCommand();
60+
hazard_lights_cmd.stamp = this->now();
61+
hazard_lights_cmd.command = HazardLightsCommand::DISABLE;
62+
63+
if (hazard_lights_command_from_path_planner_ != nullptr) {
64+
if (hazard_lights_command_from_path_planner_->command == HazardLightsCommand::ENABLE) {
65+
hazard_lights_cmd.command = HazardLightsCommand::ENABLE;
66+
}
67+
}
68+
if (hazard_lights_command_from_mrm_operator_ != nullptr) {
69+
if (hazard_lights_command_from_mrm_operator_->command == HazardLightsCommand::ENABLE) {
70+
hazard_lights_cmd.command = HazardLightsCommand::ENABLE;
71+
}
72+
}
73+
74+
pub_hazard_lights_cmd_->publish(hazard_lights_cmd);
75+
}
76+
77+
} // namespace hazard_lights_selector
78+
79+
#include "rclcpp_components/register_node_macro.hpp"
80+
RCLCPP_COMPONENTS_REGISTER_NODE(hazard_lights_selector::HazardLightsSelectorNode)

‎system/mrm_comfortable_stop_operator/include/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator_core.hpp

+4
Original file line numberDiff line numberDiff line change
@@ -19,6 +19,7 @@
1919
#include <memory>
2020

2121
// Autoware
22+
#include <autoware_auto_vehicle_msgs/msg/hazard_lights_command.hpp>
2223
#include <tier4_planning_msgs/msg/velocity_limit.hpp>
2324
#include <tier4_planning_msgs/msg/velocity_limit_clear_command.hpp>
2425
#include <tier4_planning_msgs/msg/velocity_limit_constraints.hpp>
@@ -30,6 +31,7 @@
3031

3132
namespace mrm_comfortable_stop_operator
3233
{
34+
using autoware_auto_vehicle_msgs::msg::HazardLightsCommand;
3335

3436
struct Parameters
3537
{
@@ -60,10 +62,12 @@ class MrmComfortableStopOperator : public rclcpp::Node
6062
rclcpp::Publisher<tier4_planning_msgs::msg::VelocityLimit>::SharedPtr pub_velocity_limit_;
6163
rclcpp::Publisher<tier4_planning_msgs::msg::VelocityLimitClearCommand>::SharedPtr
6264
pub_velocity_limit_clear_command_;
65+
rclcpp::Publisher<HazardLightsCommand>::SharedPtr pub_hazard_lights_cmd_;
6366

6467
void publishStatus() const;
6568
void publishVelocityLimit() const;
6669
void publishVelocityLimitClearCommand() const;
70+
void publishHazardLightsCommand() const;
6771

6872
// Timer
6973
rclcpp::TimerBase::SharedPtr timer_;

‎system/mrm_comfortable_stop_operator/launch/mrm_comfortable_stop_operator.launch.py

+1
Original file line numberDiff line numberDiff line change
@@ -39,6 +39,7 @@ def launch_setup(context, *args, **kwargs):
3939
("~/output/mrm/comfortable_stop/status", "/system/mrm/comfortable_stop/status"),
4040
("~/output/velocity_limit", "/planning/scenario_planning/max_velocity_candidates"),
4141
("~/output/velocity_limit/clear", "/planning/scenario_planning/clear_velocity_limit"),
42+
("~/output/hazard_lights_cmd", "~/hazard_lights_cmd"),
4243
],
4344
)
4445

‎system/mrm_comfortable_stop_operator/package.xml

+1
Original file line numberDiff line numberDiff line change
@@ -12,6 +12,7 @@
1212
<buildtool_depend>autoware_cmake</buildtool_depend>
1313

1414
<depend>autoware_adapi_v1_msgs</depend>
15+
<depend>autoware_auto_vehicle_msgs</depend>
1516
<depend>rclcpp</depend>
1617
<depend>rclcpp_components</depend>
1718
<depend>std_msgs</depend>

0 commit comments

Comments
 (0)
Please sign in to comment.