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..8ed70f43f51f1 --- /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 + 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..ddec94716c376 --- /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_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->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..446979ca1c2c4 --- /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_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_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..ef28badafdf70 --- /dev/null +++ b/system/control_cmd_switcher/tool/relay_trajectory.py @@ -0,0 +1,50 @@ +#!/usr/bin/env python3 + +import threading + +from autoware_auto_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()