From 437d15e5712c797b41f0a1748d2cb11d20ba4c5c Mon Sep 17 00:00:00 2001 From: TetsuKawa <70682030+TetsuKawa@users.noreply.github.com> Date: Fri, 10 Jan 2025 18:04:15 +0900 Subject: [PATCH] feat: add control cmd switcher mrm v0.6 for gen2 (#1755) * feat: add control cmd switcher Signed-off-by: TetsuKawa * style(pre-commit): autofix * feat: delete tools and config Signed-off-by: TetsuKawa --------- Signed-off-by: TetsuKawa Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- system/control_cmd_switcher/CMakeLists.txt | 23 ++++++ system/control_cmd_switcher/README.md | 1 + .../launch/control_cmd_switcher.launch.xml | 18 +++++ system/control_cmd_switcher/package.xml | 25 ++++++ .../control_cmd_switcher.cpp | 78 +++++++++++++++++++ .../control_cmd_switcher.hpp | 51 ++++++++++++ 6 files changed, 196 insertions(+) create mode 100644 system/control_cmd_switcher/CMakeLists.txt create mode 100644 system/control_cmd_switcher/README.md create mode 100644 system/control_cmd_switcher/launch/control_cmd_switcher.launch.xml create mode 100644 system/control_cmd_switcher/package.xml create mode 100644 system/control_cmd_switcher/src/control_cmd_switcher/control_cmd_switcher.cpp create mode 100644 system/control_cmd_switcher/src/control_cmd_switcher/control_cmd_switcher.hpp diff --git a/system/control_cmd_switcher/CMakeLists.txt b/system/control_cmd_switcher/CMakeLists.txt new file mode 100644 index 0000000000000..947d79a9909f6 --- /dev/null +++ b/system/control_cmd_switcher/CMakeLists.txt @@ -0,0 +1,23 @@ +cmake_minimum_required(VERSION 3.14) +project(control_cmd_switcher) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/control_cmd_switcher/control_cmd_switcher.cpp +) + +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "ControlCmdSwitcher" + EXECUTABLE ${PROJECT_NAME}_node +) + +install(PROGRAMS + DESTINATION lib/${PROJECT_NAME} + PERMISSIONS OWNER_EXECUTE OWNER_WRITE OWNER_READ GROUP_EXECUTE GROUP_READ WORLD_EXECUTE WORLD_READ +) + +ament_auto_package(INSTALL_TO_SHARE + launch +) diff --git a/system/control_cmd_switcher/README.md b/system/control_cmd_switcher/README.md new file mode 100644 index 0000000000000..7c688a50919a6 --- /dev/null +++ b/system/control_cmd_switcher/README.md @@ -0,0 +1 @@ +# control_cmd_switcher diff --git a/system/control_cmd_switcher/launch/control_cmd_switcher.launch.xml b/system/control_cmd_switcher/launch/control_cmd_switcher.launch.xml new file mode 100644 index 0000000000000..c19abb1c69af4 --- /dev/null +++ b/system/control_cmd_switcher/launch/control_cmd_switcher.launch.xml @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + diff --git a/system/control_cmd_switcher/package.xml b/system/control_cmd_switcher/package.xml new file mode 100644 index 0000000000000..f26e9ae63a065 --- /dev/null +++ b/system/control_cmd_switcher/package.xml @@ -0,0 +1,25 @@ + + + + control_cmd_switcher + 0.1.0 + The control_cmd_switcher ROS 2 package + + Tetsuhiro Kawaguchi + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + autoware_control_msgs + rclcpp + rclcpp_components + tier4_system_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/system/control_cmd_switcher/src/control_cmd_switcher/control_cmd_switcher.cpp b/system/control_cmd_switcher/src/control_cmd_switcher/control_cmd_switcher.cpp new file mode 100644 index 0000000000000..921988ff33455 --- /dev/null +++ b/system/control_cmd_switcher/src/control_cmd_switcher/control_cmd_switcher.cpp @@ -0,0 +1,78 @@ +// Copyright 2024 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 "control_cmd_switcher.hpp" + +#include +#include +#include +#include + +ControlCmdSwitcher::ControlCmdSwitcher(const rclcpp::NodeOptions & node_options) +: Node("control_cmd_switcher", node_options) +{ + // Subscriber + sub_main_control_cmd_ = create_subscription( + "~/input/main/control_cmd", rclcpp::QoS{10}, + std::bind(&ControlCmdSwitcher::onMainControlCmd, this, std::placeholders::_1)); + + sub_sub_control_cmd_ = create_subscription( + "~/input/sub/control_cmd", rclcpp::QoS{10}, + std::bind(&ControlCmdSwitcher::onSubControlCmd, this, std::placeholders::_1)); + + sub_election_status_main_ = create_subscription( + "~/input/election/status/main", rclcpp::QoS{10}, + std::bind(&ControlCmdSwitcher::onElectionStatus, this, std::placeholders::_1)); + + sub_election_status_sub_ = create_subscription( + "~/input/election/status/sub", rclcpp::QoS{10}, + std::bind(&ControlCmdSwitcher::onElectionStatus, this, std::placeholders::_1)); + + // Publisher + pub_control_cmd_ = + create_publisher("~/output/control_cmd", rclcpp::QoS{1}); + + // Initialize + use_main_control_cmd_ = true; +} + +void ControlCmdSwitcher::onMainControlCmd( + const autoware_control_msgs::msg::Control::ConstSharedPtr msg) +{ + if (use_main_control_cmd_) { + pub_control_cmd_->publish(*msg); + } +} + +void ControlCmdSwitcher::onSubControlCmd( + const autoware_control_msgs::msg::Control::ConstSharedPtr msg) +{ + if (!use_main_control_cmd_) { + pub_control_cmd_->publish(*msg); + } +} + +void ControlCmdSwitcher::onElectionStatus( + const tier4_system_msgs::msg::ElectionStatus::ConstSharedPtr msg) +{ + if (msg->election_start_count <= 0) return; + if (msg->in_election) return; + if (((msg->path_info >> 3) & 0x01) == 1) { + use_main_control_cmd_ = true; + } else if (((msg->path_info >> 2) & 0x01) == 1) { + use_main_control_cmd_ = false; + } +} + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(ControlCmdSwitcher) diff --git a/system/control_cmd_switcher/src/control_cmd_switcher/control_cmd_switcher.hpp b/system/control_cmd_switcher/src/control_cmd_switcher/control_cmd_switcher.hpp new file mode 100644 index 0000000000000..a730c6cc630ac --- /dev/null +++ b/system/control_cmd_switcher/src/control_cmd_switcher/control_cmd_switcher.hpp @@ -0,0 +1,51 @@ +// Copyright 2024 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 CONTROL_CMD_SWITCHER__CONTROL_CMD_SWITCHER_HPP_ +#define CONTROL_CMD_SWITCHER__CONTROL_CMD_SWITCHER_HPP_ + +// Core +#include +#include +#include + +// Autoware +#include +#include + +// ROS 2 core +#include + +class ControlCmdSwitcher : public rclcpp::Node +{ +public: + explicit ControlCmdSwitcher(const rclcpp::NodeOptions & node_options); + +private: + // Subscribers + rclcpp::Subscription::SharedPtr sub_main_control_cmd_; + rclcpp::Subscription::SharedPtr sub_sub_control_cmd_; + rclcpp::Subscription::SharedPtr sub_election_status_main_; + rclcpp::Subscription::SharedPtr sub_election_status_sub_; + void onMainControlCmd(const autoware_control_msgs::msg::Control::ConstSharedPtr msg); + void onSubControlCmd(const autoware_control_msgs::msg::Control::ConstSharedPtr msg); + void onElectionStatus(const tier4_system_msgs::msg::ElectionStatus::ConstSharedPtr msg); + + // Publisher + rclcpp::Publisher::SharedPtr pub_control_cmd_; + + std::atomic use_main_control_cmd_; +}; + +#endif // CONTROL_CMD_SWITCHER__CONTROL_CMD_SWITCHER_HPP_