feat(hazard_lights_selector): add hazard_lights_selector (#1157)

* feat(hazard_lights_selector): turn on hazard lights from mrm comfortable stop operator (#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>

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

@@ -194,7 +194,7 @@
       <remap from="~/input/scenario" to="/planning/scenario_planning/scenario"/>
       <remap from="~/output/path" to="path_with_lane_id"/>
       <remap from="~/output/turn_indicators_cmd" to="/planning/turn_indicators_cmd"/>
-      <remap from="~/output/hazard_lights_cmd" to="/planning/hazard_lights_cmd"/>
+      <remap from="~/output/hazard_lights_cmd" to="/planning/behavior_path_planner/hazard_lights_cmd"/>
       <remap from="~/output/modified_goal" to="/planning/scenario_planning/modified_goal"/>
       <remap from="~/output/stop_reasons" to="/planning/scenario_planning/status/stop_reasons"/>
       <!-- params -->

launch/tier4_system_launch/launch/system.launch.xml

@@ -6,10 +6,7 @@
   <arg name="duplicated_node_checker_param_path"/>
   <arg name="mrm_comfortable_stop_operator_param_path"/>
   <arg name="mrm_emergency_stop_operator_param_path"/>
-  <arg name="system_error_monitor_param_path"/>
-  <arg name="system_error_monitor_planning_simulator_param_path"/>
-  <arg name="diagnostic_aggregator_system_param_path"/>
-  <arg name="diagnostic_aggregator_vehicle_param_path"/>
+  <arg name="hazard_lights_selector_param_path"/>
   <arg name="dummy_diag_publisher_param_path"/>
   <arg name="system_monitor_cpu_monitor_param_path"/>
   <arg name="system_monitor_gpu_monitor_param_path"/>
@@ -104,6 +101,13 @@
     </group>
   </group>
 
+  <!-- hazard selector -->
+  <group>
+    <include file="$(find-pkg-share hazard_lights_selector)/launch/hazard_lights_selector.launch.xml">
+      <arg name="param_path" value="$(var hazard_lights_selector_param_path)"/>
+    </include>
+  </group>
+
   <!-- Dummy Diag Publisher -->
   <group if="$(var launch_dummy_diag_publisher)">
     <include file="$(find-pkg-share dummy_diag_publisher)/launch/dummy_diag_publisher.launch.xml">

system/hazard_lights_selector/CMakeLists.txt

@@ -0,0 +1,19 @@
+cmake_minimum_required(VERSION 3.14)
+project(hazard_lights_selector)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+ament_auto_add_library(hazard_lights_selector_node SHARED
+  src/hazard_lights_selector_node.cpp
+)
+
+rclcpp_components_register_node(hazard_lights_selector_node
+  PLUGIN "hazard_lights_selector::HazardLightsSelectorNode"
+  EXECUTABLE hazard_lights_selector
+)
+
+ament_auto_package(INSTALL_TO_SHARE
+  launch
+  config
+)

system/hazard_lights_selector/README.md

@@ -0,0 +1,80 @@
+# Hazard lights selector
+
+## Purpose
+
+## Inner-workings / Algorithms
+
+<!-- Write how this package works. Flowcharts and figures are great. Add sub-sections as you like.
+
+Example:
+  ### Flowcharts
+
+  ...(PlantUML or something)
+
+  ### State Transitions
+
+  ...(PlantUML or something)
+
+  ### How to filter target obstacles
+
+  ...
+
+  ### How to optimize trajectory
+
+  ...
+-->
+
+## Inputs
+
+## Outputs
+
+## Parameters
+
+## Assumptions / Known limits
+
+<!-- Write assumptions and limitations of your implementation.
+
+Example:
+  This algorithm assumes obstacles are not moving, so if they rapidly move after the vehicle started to avoid them, it might collide with them.
+  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.
+-->
+
+## (Optional) Error detection and handling
+
+<!-- Write how to detect errors and how to recover from them.
+
+Example:
+  This package can handle up to 20 obstacles. If more obstacles found, this node will give up and raise diagnostic errors.
+-->
+
+## (Optional) Performance characterization
+
+<!-- Write performance information like complexity. If it wouldn't be the bottleneck, not necessary.
+
+Example:
+  ### Complexity
+
+  This algorithm is O(N).
+
+  ### Processing time
+
+  ...
+-->
+
+## (Optional) References/External links
+
+<!-- Write links you referred to when you implemented.
+
+Example:
+  [1] {link_to_a_thesis}
+  [2] {link_to_an_issue}
+-->
+
+## (Optional) Future extensions / Unimplemented parts
+
+<!-- Write future extensions of this package.
+
+Example:
+  Currently, this package can't handle the chattering obstacles well. We plan to add some probabilistic filters in the perception layer to improve it.
+  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.
+-->

system/hazard_lights_selector/config/hazard_lights_selector.param.yaml

@@ -0,0 +1,3 @@
+/**:
+  ros__parameters:
+    update_rate: 10  # hazard lights command publish rate [Hz]

system/hazard_lights_selector/include/hazard_lights_selector/hazard_lights_selector_node.hpp

@@ -0,0 +1,63 @@
+// Copyright 2021 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef HAZARD_LIGHTS_SELECTOR__HAZARD_LIGHTS_SELECTOR_NODE_HPP_
+#define HAZARD_LIGHTS_SELECTOR__HAZARD_LIGHTS_SELECTOR_NODE_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <autoware_auto_vehicle_msgs/msg/hazard_lights_command.hpp>
+
+namespace hazard_lights_selector
+{
+using autoware_auto_vehicle_msgs::msg::HazardLightsCommand;
+
+class HazardLightsSelectorNode : public rclcpp::Node
+{
+  struct Parameters
+  {
+    int update_rate;  // [Hz]
+  };
+
+public:
+  explicit HazardLightsSelectorNode(const rclcpp::NodeOptions & node_options);
+
+private:
+  // Parameter
+  Parameters params_;
+
+  // Subscriber
+  rclcpp::Subscription<HazardLightsCommand>::SharedPtr sub_hazard_lights_cmd_from_path_planner_;
+  rclcpp::Subscription<HazardLightsCommand>::SharedPtr sub_hazard_lights_cmd_from_mrm_operator_;
+
+  void onHazardLightsCommandFromPathPlanner(HazardLightsCommand::ConstSharedPtr msg);
+  void onHazardLightsCommandFromMrmOperator(HazardLightsCommand::ConstSharedPtr msg);
+
+  // Publisher
+  rclcpp::Publisher<HazardLightsCommand>::SharedPtr pub_hazard_lights_cmd_;
+
+  void publishHazardLightsCommand() const;
+
+  // Timer
+  rclcpp::TimerBase::SharedPtr timer_;
+
+  void onTimer();
+
+  // State
+  HazardLightsCommand::ConstSharedPtr hazard_lights_command_from_path_planner_;
+  HazardLightsCommand::ConstSharedPtr hazard_lights_command_from_mrm_operator_;
+};
+}  // namespace hazard_lights_selector
+
+#endif  // HAZARD_LIGHTS_SELECTOR__HAZARD_LIGHTS_SELECTOR_NODE_HPP_

system/hazard_lights_selector/launch/hazard_lights_selector.launch.xml

@@ -0,0 +1,16 @@
+<launch>
+  <!-- common param -->
+  <arg name="param_path" default="$(find-pkg-share hazard_lights_selector)/config/hazard_lights_selector.param.yaml"/>
+
+  <!-- input/output -->
+  <arg name="input_hazard_lights_cmd_from_path_planner" default="/planning/behavior_path_planner/hazard_lights_cmd"/>
+  <arg name="input_hazard_lights_cmd_from_mrm_operator" default="/system/mrm_comfortable_stop_operator/hazard_lights_cmd"/>
+  <arg name="output_hazard_lights_cmd" default="/planning/hazard_lights_cmd"/>
+
+  <node pkg="hazard_lights_selector" exec="hazard_lights_selector" name="hazard_lights_selector" output="screen">
+    <param from="$(var param_path)"/>
+    <remap from="~/input/behavior_path_planner/hazard_lights_cmd" to="$(var input_hazard_lights_cmd_from_path_planner)"/>
+    <remap from="~/input/behavior_mrm_operator/hazard_lights_cmd" to="$(var input_hazard_lights_cmd_from_mrm_operator)"/>
+    <remap from="~/output/hazard_lights_cmd" to="$(var output_hazard_lights_cmd)"/>
+  </node>
+</launch>

system/hazard_lights_selector/package.xml

@@ -0,0 +1,30 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>hazard_lights_selector</name>
+  <version>0.1.0</version>
+  <description>The hazard_lights_selector ROS2 package</description>
+  <maintainer email="makoto.kurihara@tier4.jp">Makoto Kurihara</maintainer>
+  <maintainer email="tomohito.ando@tier4.jp">Tomohito Ando</maintainer>
+  <license>Apache License 2.0</license>
+
+  <author email="makoto.kurihara@tier4.jp">Makoto Kurihara</author>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <build_depend>autoware_cmake</build_depend>
+
+  <depend>autoware_auto_vehicle_msgs</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+
+  <exec_depend>ros2cli</exec_depend>
+  <exec_depend>topic_tools</exec_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

system/hazard_lights_selector/src/hazard_lights_selector_node.cpp

@@ -0,0 +1,80 @@
+// Copyright 2021 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <hazard_lights_selector/hazard_lights_selector_node.hpp>
+
+namespace hazard_lights_selector
+{
+HazardLightsSelectorNode::HazardLightsSelectorNode(const rclcpp::NodeOptions & node_options)
+: Node("hazard_lights_selector", node_options)
+{
+  using std::placeholders::_1;
+
+  // Parameter
+  params_.update_rate = static_cast<int>(declare_parameter<int>("update_rate", 10));
+
+  // Subscriber
+  sub_hazard_lights_cmd_from_path_planner_ = this->create_subscription<HazardLightsCommand>(
+    "~/input/behavior_path_planner/hazard_lights_cmd", 1,
+    std::bind(&HazardLightsSelectorNode::onHazardLightsCommandFromPathPlanner, this, _1));
+  sub_hazard_lights_cmd_from_mrm_operator_ = this->create_subscription<HazardLightsCommand>(
+    "~/input/behavior_mrm_operator/hazard_lights_cmd", 1,
+    std::bind(&HazardLightsSelectorNode::onHazardLightsCommandFromMrmOperator, this, _1));
+
+  // Publisher
+  pub_hazard_lights_cmd_ =
+    this->create_publisher<HazardLightsCommand>("~/output/hazard_lights_cmd", 1);
+
+  // Timer
+  const auto update_period_ns = rclcpp::Rate(params_.update_rate).period();
+  timer_ = rclcpp::create_timer(
+    this, get_clock(), update_period_ns, std::bind(&HazardLightsSelectorNode::onTimer, this));
+}
+
+void HazardLightsSelectorNode::onHazardLightsCommandFromPathPlanner(
+  HazardLightsCommand::ConstSharedPtr msg)
+{
+  hazard_lights_command_from_path_planner_ = msg;
+}
+
+void HazardLightsSelectorNode::onHazardLightsCommandFromMrmOperator(
+  HazardLightsCommand::ConstSharedPtr msg)
+{
+  hazard_lights_command_from_mrm_operator_ = msg;
+}
+
+void HazardLightsSelectorNode::onTimer()
+{
+  auto hazard_lights_cmd = HazardLightsCommand();
+  hazard_lights_cmd.stamp = this->now();
+  hazard_lights_cmd.command = HazardLightsCommand::DISABLE;
+
+  if (hazard_lights_command_from_path_planner_ != nullptr) {
+    if (hazard_lights_command_from_path_planner_->command == HazardLightsCommand::ENABLE) {
+      hazard_lights_cmd.command = HazardLightsCommand::ENABLE;
+    }
+  }
+  if (hazard_lights_command_from_mrm_operator_ != nullptr) {
+    if (hazard_lights_command_from_mrm_operator_->command == HazardLightsCommand::ENABLE) {
+      hazard_lights_cmd.command = HazardLightsCommand::ENABLE;
+    }
+  }
+
+  pub_hazard_lights_cmd_->publish(hazard_lights_cmd);
+}
+
+}  // namespace hazard_lights_selector
+
+#include "rclcpp_components/register_node_macro.hpp"
+RCLCPP_COMPONENTS_REGISTER_NODE(hazard_lights_selector::HazardLightsSelectorNode)

system/mrm_comfortable_stop_operator/include/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator_core.hpp

@@ -19,6 +19,7 @@
 #include <memory>
 
 // Autoware
+#include <autoware_auto_vehicle_msgs/msg/hazard_lights_command.hpp>
 #include <tier4_planning_msgs/msg/velocity_limit.hpp>
 #include <tier4_planning_msgs/msg/velocity_limit_clear_command.hpp>
 #include <tier4_planning_msgs/msg/velocity_limit_constraints.hpp>
@@ -30,6 +31,7 @@
 
 namespace mrm_comfortable_stop_operator
 {
+using autoware_auto_vehicle_msgs::msg::HazardLightsCommand;
 
 struct Parameters
 {
@@ -60,10 +62,12 @@ class MrmComfortableStopOperator : public rclcpp::Node
   rclcpp::Publisher<tier4_planning_msgs::msg::VelocityLimit>::SharedPtr pub_velocity_limit_;
   rclcpp::Publisher<tier4_planning_msgs::msg::VelocityLimitClearCommand>::SharedPtr
     pub_velocity_limit_clear_command_;
+  rclcpp::Publisher<HazardLightsCommand>::SharedPtr pub_hazard_lights_cmd_;
 
   void publishStatus() const;
   void publishVelocityLimit() const;
   void publishVelocityLimitClearCommand() const;
+  void publishHazardLightsCommand() const;
 
   // Timer
   rclcpp::TimerBase::SharedPtr timer_;

system/mrm_comfortable_stop_operator/launch/mrm_comfortable_stop_operator.launch.py

@@ -39,6 +39,7 @@ def launch_setup(context, *args, **kwargs):
             ("~/output/mrm/comfortable_stop/status", "/system/mrm/comfortable_stop/status"),
             ("~/output/velocity_limit", "/planning/scenario_planning/max_velocity_candidates"),
             ("~/output/velocity_limit/clear", "/planning/scenario_planning/clear_velocity_limit"),
+            ("~/output/hazard_lights_cmd", "~/hazard_lights_cmd"),
         ],
     )
 

system/mrm_comfortable_stop_operator/package.xml

@@ -12,6 +12,7 @@
   <buildtool_depend>autoware_cmake</buildtool_depend>
 
   <depend>autoware_adapi_v1_msgs</depend>
+  <depend>autoware_auto_vehicle_msgs</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
   <depend>std_msgs</depend>