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/system/hazard_lights_selector/CMakeLists.txt b/system/hazard_lights_selector/CMakeLists.txt new file mode 100644 index 0000000000000..c8339ef14ac08 --- /dev/null +++ b/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 +) diff --git a/system/hazard_lights_selector/README.md b/system/hazard_lights_selector/README.md new file mode 100644 index 0000000000000..2e55b25cc417b --- /dev/null +++ b/system/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/system/hazard_lights_selector/config/hazard_lights_selector.param.yaml b/system/hazard_lights_selector/config/hazard_lights_selector.param.yaml new file mode 100644 index 0000000000000..68273fd99018f --- /dev/null +++ b/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] diff --git a/system/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 new file mode 100644 index 0000000000000..c984e9e8f4b19 --- /dev/null +++ b/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 + +#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/system/hazard_lights_selector/launch/hazard_lights_selector.launch.xml b/system/hazard_lights_selector/launch/hazard_lights_selector.launch.xml new file mode 100644 index 0000000000000..09d542249d7a3 --- /dev/null +++ b/system/hazard_lights_selector/launch/hazard_lights_selector.launch.xml @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/system/hazard_lights_selector/package.xml b/system/hazard_lights_selector/package.xml new file mode 100644 index 0000000000000..91a7bf3465e4b --- /dev/null +++ b/system/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/system/hazard_lights_selector/src/hazard_lights_selector_node.cpp b/system/hazard_lights_selector/src/hazard_lights_selector_node.cpp new file mode 100644 index 0000000000000..17f034539d715 --- /dev/null +++ b/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 + +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