From da201e44257b4cbeed52d23169f818d09ffd0d5e Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Thu, 27 Jun 2024 21:02:55 +0900 Subject: [PATCH 1/3] feat: add control_cmd_switcher Signed-off-by: TetsuKawa --- system/control_cmd_switcher/CMakeLists.txt | 19 ++++++ system/control_cmd_switcher/README.md | 1 + .../config/control_cmd_switcher.yaml | 5 ++ .../launch/control_cmd_switcher.launch.xml | 17 +++++ system/control_cmd_switcher/package.xml | 25 +++++++ .../control_cmd_switcher.cpp | 65 +++++++++++++++++++ .../control_cmd_switcher.hpp | 55 ++++++++++++++++ 7 files changed, 187 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 diff --git a/system/control_cmd_switcher/CMakeLists.txt b/system/control_cmd_switcher/CMakeLists.txt new file mode 100644 index 0000000000000..42ec827f855cc --- /dev/null +++ b/system/control_cmd_switcher/CMakeLists.txt @@ -0,0 +1,19 @@ +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 +) + +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..f1e018734e2d1 --- /dev/null +++ b/system/control_cmd_switcher/README.md @@ -0,0 +1 @@ +# control_cmd_switcher \ No newline at end of file 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..e083b38a5ebe9 --- /dev/null +++ b/system/control_cmd_switcher/config/control_cmd_switcher.yaml @@ -0,0 +1,5 @@ +# 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..9717a57970a3e --- /dev/null +++ b/system/control_cmd_switcher/launch/control_cmd_switcher.launch.xml @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/system/control_cmd_switcher/package.xml b/system/control_cmd_switcher/package.xml new file mode 100644 index 0000000000000..7e0fa2bf2e97b --- /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_auto_control_msgs + rclcpp + tier4_system_msgs + rclcpp_components + + 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..b038264a7af99 --- /dev/null +++ b/system/control_cmd_switcher/src/control_cmd_switcher/control_cmd_switcher.cpp @@ -0,0 +1,65 @@ +// 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 = create_subscription( + "~/input/election/status", 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_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr msg) +{ + if (use_main_control_cmd_) { + pub_control_cmd_->publish(*msg); + } +} + +void ControlCmdSwitcher::onSubControlCmd(const autoware_auto_control_msgs::msg::AckermannControlCommand::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->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..1ebdf0b51551d --- /dev/null +++ b/system/control_cmd_switcher/src/control_cmd_switcher/control_cmd_switcher.hpp @@ -0,0 +1,55 @@ +// 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_SWITCHER__CONTROL_CMD_SWITCHER_HPP_ +#define CONTROL_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; + void onMainControlCmd(const autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr msg); + void onSubControlCmd(const autoware_auto_control_msgs::msg::AckermannControlCommand::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_SWITCHER__CONTROL_SWITCHER_HPP_ From b0adbf87c4df651b265bf20e828e4b6f969fb581 Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Fri, 28 Jun 2024 01:05:49 +0900 Subject: [PATCH 2/3] modify: launch file mistakes Signed-off-by: TetsuKawa --- .../launch/control_cmd_switcher.launch.xml | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/system/control_cmd_switcher/launch/control_cmd_switcher.launch.xml b/system/control_cmd_switcher/launch/control_cmd_switcher.launch.xml index 9717a57970a3e..b35bc2d726bc7 100644 --- a/system/control_cmd_switcher/launch/control_cmd_switcher.launch.xml +++ b/system/control_cmd_switcher/launch/control_cmd_switcher.launch.xml @@ -7,11 +7,10 @@ - + - From 67eb555ebe4941784f170b68a2d2c7fd11d2c7a8 Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Fri, 28 Jun 2024 10:37:24 +0900 Subject: [PATCH 3/3] feat: add rely script Signed-off-by: TetsuKawa --- .../simple_planning_simulator/CMakeLists.txt | 6 +++ .../tool/rely_trajectry.py | 47 +++++++++++++++++++ 2 files changed, 53 insertions(+) create mode 100755 system/control_cmd_switcher/tool/rely_trajectry.py diff --git a/simulator/simple_planning_simulator/CMakeLists.txt b/simulator/simple_planning_simulator/CMakeLists.txt index 87d0f7e5fef0b..5531ad8f2f754 100644 --- a/simulator/simple_planning_simulator/CMakeLists.txt +++ b/simulator/simple_planning_simulator/CMakeLists.txt @@ -29,6 +29,12 @@ rclcpp_components_register_node(${PROJECT_NAME} EXECUTABLE ${PROJECT_NAME}_exe ) +install(PROGRAMS + tool/rely_trajectry.py + DESTINATION lib/${PROJECT_NAME} + PERMISSIONS OWNER_EXECUTE OWNER_WRITE OWNER_READ GROUP_EXECUTE GROUP_READ WORLD_EXECUTE WORLD_READ +) + if(BUILD_TESTING) ament_add_ros_isolated_gtest(test_simple_planning_simulator test/test_simple_planning_simulator.cpp diff --git a/system/control_cmd_switcher/tool/rely_trajectry.py b/system/control_cmd_switcher/tool/rely_trajectry.py new file mode 100755 index 0000000000000..b2dbef5ba4965 --- /dev/null +++ b/system/control_cmd_switcher/tool/rely_trajectry.py @@ -0,0 +1,47 @@ +#!/usr/bin/env python3 + +import rclpy +from rclpy.node import Node +from autoware_auto_planning_msgs.msg import Trajectory +import threading + +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()