From ee9f62d32593dd60be1ed1b594f836cffaaed44f Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Fri, 6 Dec 2024 21:27:13 +0900 Subject: [PATCH 1/2] feat: add control cmd switcher Signed-off-by: TetsuKawa --- system/control_cmd_switcher/CMakeLists.txt | 25 ++++++ system/control_cmd_switcher/README.md | 1 + .../config/control_cmd_switcher.yaml | 4 + .../launch/control_cmd_switcher.launch.xml | 18 +++++ system/control_cmd_switcher/package.xml | 25 ++++++ .../control_cmd_switcher.cpp | 80 +++++++++++++++++++ .../control_cmd_switcher.hpp | 56 +++++++++++++ .../tool/relay_trajectory.py | 50 ++++++++++++ 8 files changed, 259 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/config/control_cmd_switcher.yaml 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 create mode 100755 system/control_cmd_switcher/tool/relay_trajectory.py diff --git a/system/control_cmd_switcher/CMakeLists.txt b/system/control_cmd_switcher/CMakeLists.txt new file mode 100644 index 0000000000000..2bd58dbeedaaa --- /dev/null +++ b/system/control_cmd_switcher/CMakeLists.txt @@ -0,0 +1,25 @@ +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 + tool/relay_trajectory.py + 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 + config +) 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/config/control_cmd_switcher.yaml b/system/control_cmd_switcher/config/control_cmd_switcher.yaml new file mode 100644 index 0000000000000..b908163301d69 --- /dev/null +++ b/system/control_cmd_switcher/config/control_cmd_switcher.yaml @@ -0,0 +1,4 @@ +# Default configuration for mrm handler +--- +/**: + ros__parameters: 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..c7405c34f88ba --- /dev/null +++ b/system/control_cmd_switcher/src/control_cmd_switcher/control_cmd_switcher.cpp @@ -0,0 +1,80 @@ +// 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..ea118f3d869bb --- /dev/null +++ b/system/control_cmd_switcher/src/control_cmd_switcher/control_cmd_switcher.hpp @@ -0,0 +1,56 @@ +// 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_ diff --git a/system/control_cmd_switcher/tool/relay_trajectory.py b/system/control_cmd_switcher/tool/relay_trajectory.py new file mode 100755 index 0000000000000..9633b854af69d --- /dev/null +++ b/system/control_cmd_switcher/tool/relay_trajectory.py @@ -0,0 +1,50 @@ +#!/usr/bin/env python3 + +import threading + +from autoware_planning_msgs.msg import Trajectory +import rclpy +from rclpy.node import Node + + +class RelayTrajectoryNode(Node): + def __init__(self): + super().__init__("relay_trajectory") + self.subscription = self.create_subscription( + Trajectory, "/tmp/planning/scenario_planning/trajectory", self.listener_callback, 10 + ) + self.publisher = self.create_publisher( + Trajectory, "/planning/scenario_planning/trajectory", 10 + ) + self.running = True + + def listener_callback(self, msg): + if self.running: + self.publisher.publish(msg) + + +def main(args=None): + rclpy.init(args=args) + node = RelayTrajectoryNode() + + def input_thread(): + nonlocal node + while True: + user_input = input("Enter 'y' to stop publishing: ") + if user_input.lower() == "y": + node.running = False + print("Publishing stopped.") + break + + thread = threading.Thread(target=input_thread) + thread.start() + + rclpy.spin(node) + + thread.join() + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() From 38e35d5af90fb01f2424efda6d849e9e092a0c57 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 6 Dec 2024 13:00:03 +0000 Subject: [PATCH 2/2] style(pre-commit): autofix --- .../control_cmd_switcher.cpp | 18 ++++++++---------- .../control_cmd_switcher.hpp | 15 +++++---------- 2 files changed, 13 insertions(+), 20 deletions(-) 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 index c7405c34f88ba..921988ff33455 100644 --- 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 @@ -22,15 +22,13 @@ 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_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_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}, @@ -41,8 +39,8 @@ ControlCmdSwitcher::ControlCmdSwitcher(const rclcpp::NodeOptions & node_options) std::bind(&ControlCmdSwitcher::onElectionStatus, this, std::placeholders::_1)); // Publisher - pub_control_cmd_ = create_publisher( - "~/output/control_cmd", rclcpp::QoS{1}); + pub_control_cmd_ = + create_publisher("~/output/control_cmd", rclcpp::QoS{1}); // Initialize use_main_control_cmd_ = true; 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 index ea118f3d869bb..a730c6cc630ac 100644 --- 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 @@ -34,21 +34,16 @@ class ControlCmdSwitcher : public rclcpp::Node private: // Subscribers - rclcpp::Subscription::SharedPtr - sub_main_control_cmd_; - rclcpp::Subscription::SharedPtr - sub_sub_control_cmd_; + 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 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_; + rclcpp::Publisher::SharedPtr pub_control_cmd_; std::atomic use_main_control_cmd_; };