From e9904ba4902575ee791007356265ebab8e29e604 Mon Sep 17 00:00:00 2001 From: Makoto Kurihara Date: Fri, 24 Mar 2023 22:56:33 +0900 Subject: [PATCH 1/2] 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 * chore(hazard_lights_selector): implement a header file Signed-off-by: Makoto Kurihara * chore(hazard_lights_selector): implement the hazard_lights_selector package Signed-off-by: Makoto Kurihara * chore(hazard_lights_selector): remove a current hazard lights command state Signed-off-by: Makoto Kurihara * refacotr(hazard_lights_selector): remove extra lines Signed-off-by: Makoto Kurihara * feat(mrm_comfortable_stop_operator): add hazard lights publisher Signed-off-by: Makoto Kurihara * chore(hazard_lights_selector): add a maintainer Signed-off-by: Makoto Kurihara * style: pre-commit fixes Signed-off-by: Tomohito Ando --------- Signed-off-by: Makoto Kurihara Signed-off-by: Tomohito Ando Co-authored-by: Tomohito Ando --- .../hazard_lights_selector/CMakeLists.txt | 19 +++++ planning/hazard_lights_selector/README.md | 80 +++++++++++++++++++ .../config/hazard_lights_selector.param.yaml | 3 + .../hazard_lights_selector_node.hpp | 63 +++++++++++++++ .../launch/hazard_lights_selector.launch.xml | 16 ++++ planning/hazard_lights_selector/package.xml | 30 +++++++ .../src/hazard_lights_selector_node.cpp | 80 +++++++++++++++++++ .../mrm_comfortable_stop_operator_core.hpp | 4 + .../mrm_comfortable_stop_operator.launch.py | 1 + .../mrm_comfortable_stop_operator/package.xml | 1 + .../mrm_comfortable_stop_operator_core.cpp | 15 ++++ 11 files changed, 312 insertions(+) create mode 100644 planning/hazard_lights_selector/CMakeLists.txt create mode 100644 planning/hazard_lights_selector/README.md create mode 100644 planning/hazard_lights_selector/config/hazard_lights_selector.param.yaml create mode 100644 planning/hazard_lights_selector/include/hazard_lights_selector/hazard_lights_selector_node.hpp create mode 100644 planning/hazard_lights_selector/launch/hazard_lights_selector.launch.xml create mode 100644 planning/hazard_lights_selector/package.xml create mode 100644 planning/hazard_lights_selector/src/hazard_lights_selector_node.cpp diff --git a/planning/hazard_lights_selector/CMakeLists.txt b/planning/hazard_lights_selector/CMakeLists.txt new file mode 100644 index 0000000000000..c8339ef14ac08 --- /dev/null +++ b/planning/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 +) diff --git a/planning/hazard_lights_selector/README.md b/planning/hazard_lights_selector/README.md new file mode 100644 index 0000000000000..2e55b25cc417b --- /dev/null +++ b/planning/hazard_lights_selector/README.md @@ -0,0 +1,80 @@ +# Hazard lights selector + +## Purpose + +## Inner-workings / Algorithms + + + +## Inputs + +## Outputs + +## Parameters + +## Assumptions / Known limits + + + +## (Optional) Error detection and handling + + + +## (Optional) Performance characterization + + + +## (Optional) References/External links + + + +## (Optional) Future extensions / Unimplemented parts + + diff --git a/planning/hazard_lights_selector/config/hazard_lights_selector.param.yaml b/planning/hazard_lights_selector/config/hazard_lights_selector.param.yaml new file mode 100644 index 0000000000000..68273fd99018f --- /dev/null +++ b/planning/hazard_lights_selector/config/hazard_lights_selector.param.yaml @@ -0,0 +1,3 @@ +/**: + ros__parameters: + update_rate: 10 # hazard lights command publish rate [Hz] diff --git a/planning/hazard_lights_selector/include/hazard_lights_selector/hazard_lights_selector_node.hpp b/planning/hazard_lights_selector/include/hazard_lights_selector/hazard_lights_selector_node.hpp new file mode 100644 index 0000000000000..c984e9e8f4b19 --- /dev/null +++ b/planning/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 + +#include + +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::SharedPtr sub_hazard_lights_cmd_from_path_planner_; + rclcpp::Subscription::SharedPtr sub_hazard_lights_cmd_from_mrm_operator_; + + void onHazardLightsCommandFromPathPlanner(HazardLightsCommand::ConstSharedPtr msg); + void onHazardLightsCommandFromMrmOperator(HazardLightsCommand::ConstSharedPtr msg); + + // Publisher + rclcpp::Publisher::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_ diff --git a/planning/hazard_lights_selector/launch/hazard_lights_selector.launch.xml b/planning/hazard_lights_selector/launch/hazard_lights_selector.launch.xml new file mode 100644 index 0000000000000..09d542249d7a3 --- /dev/null +++ b/planning/hazard_lights_selector/launch/hazard_lights_selector.launch.xml @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/planning/hazard_lights_selector/package.xml b/planning/hazard_lights_selector/package.xml new file mode 100644 index 0000000000000..91a7bf3465e4b --- /dev/null +++ b/planning/hazard_lights_selector/package.xml @@ -0,0 +1,30 @@ + + + + hazard_lights_selector + 0.1.0 + The hazard_lights_selector ROS2 package + Makoto Kurihara + Tomohito Ando + Apache License 2.0 + + Makoto Kurihara + + ament_cmake_auto + + autoware_cmake + + autoware_auto_vehicle_msgs + rclcpp + rclcpp_components + + ros2cli + topic_tools + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/hazard_lights_selector/src/hazard_lights_selector_node.cpp b/planning/hazard_lights_selector/src/hazard_lights_selector_node.cpp new file mode 100644 index 0000000000000..17f034539d715 --- /dev/null +++ b/planning/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 + +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(declare_parameter("update_rate", 10)); + + // Subscriber + sub_hazard_lights_cmd_from_path_planner_ = this->create_subscription( + "~/input/behavior_path_planner/hazard_lights_cmd", 1, + std::bind(&HazardLightsSelectorNode::onHazardLightsCommandFromPathPlanner, this, _1)); + sub_hazard_lights_cmd_from_mrm_operator_ = this->create_subscription( + "~/input/behavior_mrm_operator/hazard_lights_cmd", 1, + std::bind(&HazardLightsSelectorNode::onHazardLightsCommandFromMrmOperator, this, _1)); + + // Publisher + pub_hazard_lights_cmd_ = + this->create_publisher("~/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) diff --git a/system/mrm_comfortable_stop_operator/include/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator_core.hpp b/system/mrm_comfortable_stop_operator/include/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator_core.hpp index 7856c23fa3158..12a7cf8b318c0 100644 --- a/system/mrm_comfortable_stop_operator/include/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator_core.hpp +++ b/system/mrm_comfortable_stop_operator/include/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator_core.hpp @@ -19,6 +19,7 @@ #include // Autoware +#include #include #include #include @@ -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::SharedPtr pub_velocity_limit_; rclcpp::Publisher::SharedPtr pub_velocity_limit_clear_command_; + rclcpp::Publisher::SharedPtr pub_hazard_lights_cmd_; void publishStatus() const; void publishVelocityLimit() const; void publishVelocityLimitClearCommand() const; + void publishHazardLightsCommand() const; // Timer rclcpp::TimerBase::SharedPtr timer_; diff --git a/system/mrm_comfortable_stop_operator/launch/mrm_comfortable_stop_operator.launch.py b/system/mrm_comfortable_stop_operator/launch/mrm_comfortable_stop_operator.launch.py index 9181af398e3d6..95b3466854665 100644 --- a/system/mrm_comfortable_stop_operator/launch/mrm_comfortable_stop_operator.launch.py +++ b/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"), ], ) diff --git a/system/mrm_comfortable_stop_operator/package.xml b/system/mrm_comfortable_stop_operator/package.xml index ca65de87d762a..f81753ec77368 100644 --- a/system/mrm_comfortable_stop_operator/package.xml +++ b/system/mrm_comfortable_stop_operator/package.xml @@ -12,6 +12,7 @@ autoware_cmake autoware_adapi_v1_msgs + autoware_auto_vehicle_msgs rclcpp rclcpp_components std_msgs diff --git a/system/mrm_comfortable_stop_operator/src/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator_core.cpp b/system/mrm_comfortable_stop_operator/src/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator_core.cpp index 5c9562463f891..67d876c136a10 100644 --- a/system/mrm_comfortable_stop_operator/src/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator_core.cpp +++ b/system/mrm_comfortable_stop_operator/src/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator_core.cpp @@ -40,6 +40,7 @@ MrmComfortableStopOperator::MrmComfortableStopOperator(const rclcpp::NodeOptions pub_velocity_limit_clear_command_ = create_publisher( "~/output/velocity_limit/clear", rclcpp::QoS{1}.transient_local()); + pub_hazard_lights_cmd_ = create_publisher("~/output/hazard_lights_cmd", 1); // Timer const auto update_period_ns = rclcpp::Rate(params_.update_rate).period(); @@ -96,9 +97,23 @@ void MrmComfortableStopOperator::publishVelocityLimitClearCommand() const pub_velocity_limit_clear_command_->publish(velocity_limit_clear_command); } +void MrmComfortableStopOperator::publishHazardLightsCommand() const +{ + auto hazard_lights_cmd = HazardLightsCommand(); + hazard_lights_cmd.stamp = this->now(); + hazard_lights_cmd.command = HazardLightsCommand::DISABLE; + + if (status_.state == tier4_system_msgs::msg::MrmBehaviorStatus::OPERATING) { + hazard_lights_cmd.command = HazardLightsCommand::ENABLE; + } + + pub_hazard_lights_cmd_->publish(hazard_lights_cmd); +} + void MrmComfortableStopOperator::onTimer() const { publishStatus(); + publishHazardLightsCommand(); } } // namespace mrm_comfortable_stop_operator From cbc0dd1bd043cc08dabdfddea9a4bf4a6e436afb Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Mon, 26 Feb 2024 14:51:11 +0900 Subject: [PATCH 2/2] migrate to latest Signed-off-by: Mamoru Sobue --- .../behavior_planning/behavior_planning.launch.xml | 2 +- launch/tier4_system_launch/launch/system.launch.xml | 12 ++++++++---- .../hazard_lights_selector/CMakeLists.txt | 0 .../hazard_lights_selector/README.md | 0 .../config/hazard_lights_selector.param.yaml | 0 .../hazard_lights_selector_node.hpp | 0 .../launch/hazard_lights_selector.launch.xml | 0 .../hazard_lights_selector/package.xml | 0 .../src/hazard_lights_selector_node.cpp | 0 9 files changed, 9 insertions(+), 5 deletions(-) rename {planning => system}/hazard_lights_selector/CMakeLists.txt (100%) rename {planning => system}/hazard_lights_selector/README.md (100%) rename {planning => system}/hazard_lights_selector/config/hazard_lights_selector.param.yaml (100%) rename {planning => system}/hazard_lights_selector/include/hazard_lights_selector/hazard_lights_selector_node.hpp (100%) rename {planning => system}/hazard_lights_selector/launch/hazard_lights_selector.launch.xml (100%) rename {planning => system}/hazard_lights_selector/package.xml (100%) rename {planning => system}/hazard_lights_selector/src/hazard_lights_selector_node.cpp (100%) diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml index 04ec688de6f60..23c04794fad70 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml @@ -193,7 +193,7 @@ - + diff --git a/launch/tier4_system_launch/launch/system.launch.xml b/launch/tier4_system_launch/launch/system.launch.xml index 2de6a61547498..e4e6e4eee31cd 100644 --- a/launch/tier4_system_launch/launch/system.launch.xml +++ b/launch/tier4_system_launch/launch/system.launch.xml @@ -6,10 +6,7 @@ - - - - + @@ -104,6 +101,13 @@ + + + + + + + diff --git a/planning/hazard_lights_selector/CMakeLists.txt b/system/hazard_lights_selector/CMakeLists.txt similarity index 100% rename from planning/hazard_lights_selector/CMakeLists.txt rename to system/hazard_lights_selector/CMakeLists.txt diff --git a/planning/hazard_lights_selector/README.md b/system/hazard_lights_selector/README.md similarity index 100% rename from planning/hazard_lights_selector/README.md rename to system/hazard_lights_selector/README.md diff --git a/planning/hazard_lights_selector/config/hazard_lights_selector.param.yaml b/system/hazard_lights_selector/config/hazard_lights_selector.param.yaml similarity index 100% rename from planning/hazard_lights_selector/config/hazard_lights_selector.param.yaml rename to system/hazard_lights_selector/config/hazard_lights_selector.param.yaml diff --git a/planning/hazard_lights_selector/include/hazard_lights_selector/hazard_lights_selector_node.hpp b/system/hazard_lights_selector/include/hazard_lights_selector/hazard_lights_selector_node.hpp similarity index 100% rename from planning/hazard_lights_selector/include/hazard_lights_selector/hazard_lights_selector_node.hpp rename to system/hazard_lights_selector/include/hazard_lights_selector/hazard_lights_selector_node.hpp diff --git a/planning/hazard_lights_selector/launch/hazard_lights_selector.launch.xml b/system/hazard_lights_selector/launch/hazard_lights_selector.launch.xml similarity index 100% rename from planning/hazard_lights_selector/launch/hazard_lights_selector.launch.xml rename to system/hazard_lights_selector/launch/hazard_lights_selector.launch.xml diff --git a/planning/hazard_lights_selector/package.xml b/system/hazard_lights_selector/package.xml similarity index 100% rename from planning/hazard_lights_selector/package.xml rename to system/hazard_lights_selector/package.xml diff --git a/planning/hazard_lights_selector/src/hazard_lights_selector_node.cpp b/system/hazard_lights_selector/src/hazard_lights_selector_node.cpp similarity index 100% rename from planning/hazard_lights_selector/src/hazard_lights_selector_node.cpp rename to system/hazard_lights_selector/src/hazard_lights_selector_node.cpp