Merge pull request #1358 from TetsuKawa/feat/add-control-cmd-switcher

feat: add control cmd switcher

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

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
+)

system/control_cmd_switcher/README.md

@@ -0,0 +1 @@
+# control_cmd_switcher

system/control_cmd_switcher/config/control_cmd_switcher.yaml

@@ -0,0 +1,5 @@
+# Default configuration for mrm handler
+---
+/**:
+  ros__parameters:
+

system/control_cmd_switcher/launch/control_cmd_switcher.launch.xml

@@ -0,0 +1,16 @@
+<launch>
+  <arg name="input_main_control_cmd" default="/main/control/command/control_cmd"/>
+  <arg name="input_sub_control_cmd" default="/sub/control/command/control_cmd"/>
+  <arg name="input_election_status" default="/system/election/status"/>
+  <arg name="output_control_cmd" default="/control/command/control_cmd"/>
+
+  <arg name="config_file" default="$(find-pkg-share control_cmd_switcher)/config/control_cmd_switcher.yaml"/>
+
+  <!-- mrm_handler -->
+  <node pkg="control_cmd_switcher" exec="control_cmd_switcher_node" name="control_cmd_switcher" output="screen">
+    <remap from="~/input/main/control_cmd" to="$(var input_main_control_cmd)"/>
+    <remap from="~/input/sub/control_cmd" to="$(var input_sub_control_cmd)"/>
+    <remap from="~/input/election/status" to="$(var input_election_status)"/>
+    <remap from="~/output/control_cmd" to="$(var output_control_cmd)"/>
+  </node>
+</launch>

system/control_cmd_switcher/package.xml

@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>control_cmd_switcher</name>
+  <version>0.1.0</version>
+  <description>The control_cmd_switcher ROS 2 package</description>
+
+  <maintainer email="[email protected]">Tetsuhiro Kawaguchi</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <buildtool_depend>autoware_cmake</buildtool_depend>
+
+  <depend>autoware_auto_control_msgs</depend>
+  <depend>rclcpp</depend>
+  <depend>tier4_system_msgs</depend>
+    <depend>rclcpp_components</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

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 <chrono>
+#include <memory>
+#include <string>
+#include <utility>
+
+ControlCmdSwitcher::ControlCmdSwitcher(const rclcpp::NodeOptions & node_options) : Node("control_cmd_switcher", node_options)
+{
+  // Subscriber
+  sub_main_control_cmd_ = create_subscription<autoware_auto_control_msgs::msg::AckermannControlCommand>(
+    "~/input/main/control_cmd", rclcpp::QoS{10}, std::bind(&ControlCmdSwitcher::onMainControlCmd, this, std::placeholders::_1));
+
+  sub_sub_control_cmd_ = create_subscription<autoware_auto_control_msgs::msg::AckermannControlCommand>(
+    "~/input/sub/control_cmd", rclcpp::QoS{10}, std::bind(&ControlCmdSwitcher::onSubControlCmd, this, std::placeholders::_1));
+
+  sub_election_status = create_subscription<tier4_system_msgs::msg::ElectionStatus>(
+    "~/input/election/status", rclcpp::QoS{10}, std::bind(&ControlCmdSwitcher::onElectionStatus, this, std::placeholders::_1));
+  // Publisher
+  pub_control_cmd_ = create_publisher<autoware_auto_control_msgs::msg::AckermannControlCommand>(
+    "~/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_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(ControlCmdSwitcher)
+

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 <memory>
+#include <string>
+#include <atomic>
+
+
+// Autoware
+#include <autoware_auto_control_msgs/msg/ackermann_control_command.hpp>
+#include <tier4_system_msgs/msg/election_status.hpp>
+
+// ROS 2 core
+#include <rclcpp/rclcpp.hpp>
+
+
+class ControlCmdSwitcher : public rclcpp::Node
+{
+public:
+  explicit ControlCmdSwitcher(const rclcpp::NodeOptions & node_options);
+
+private:
+
+  // Subscribers
+  rclcpp::Subscription<autoware_auto_control_msgs::msg::AckermannControlCommand>::SharedPtr sub_main_control_cmd_;
+  rclcpp::Subscription<autoware_auto_control_msgs::msg::AckermannControlCommand>::SharedPtr sub_sub_control_cmd_;
+  rclcpp::Subscription<tier4_system_msgs::msg::ElectionStatus>::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<autoware_auto_control_msgs::msg::AckermannControlCommand>::SharedPtr pub_control_cmd_;
+
+
+  std::atomic<bool> use_main_control_cmd_;
+};
+
+#endif  // CONTROL_SWITCHER__CONTROL_SWITCHER_HPP_

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()