feat(control_cmd_switcher): mrm v0.6 add control cmd switcher based o…

…n x2 v3.0.0 (#1385)

* feat: add control cmd switcher node

Signed-off-by: TetsuKawa <[email protected]>

* fix: typo

Signed-off-by: TetsuKawa <[email protected]>

* style(pre-commit): autofix

---------

Signed-off-by: TetsuKawa <[email protected]>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

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

system/control_cmd_switcher/README.md

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

system/control_cmd_switcher/config/control_cmd_switcher.yaml

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

system/control_cmd_switcher/launch/control_cmd_switcher.launch.xml

@@ -0,0 +1,18 @@
+<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_main" default="/system/election/status"/>
+  <arg name="input_election_status_sub" default="/sub/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/main" to="$(var input_election_status_main)"/>
+    <remap from="~/input/election/status/sub" to="$(var input_election_status_sub)"/>
+    <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>rclcpp_components</depend>
+  <depend>tier4_system_msgs</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,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 <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_main_ = create_subscription<tier4_system_msgs::msg::ElectionStatus>(
+    "~/input/election/status/main", rclcpp::QoS{10},
+    std::bind(&ControlCmdSwitcher::onElectionStatus, this, std::placeholders::_1));
+
+  sub_election_status_sub_ = create_subscription<tier4_system_msgs::msg::ElectionStatus>(
+    "~/input/election/status/sub", 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->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_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(ControlCmdSwitcher)

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 <atomic>
+#include <memory>
+#include <string>
+
+// 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_main_;
+  rclcpp::Subscription<tier4_system_msgs::msg::ElectionStatus>::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<autoware_auto_control_msgs::msg::AckermannControlCommand>::SharedPtr
+    pub_control_cmd_;
+
+  std::atomic<bool> use_main_control_cmd_;
+};
+
+#endif  // CONTROL_CMD_SWITCHER__CONTROL_CMD_SWITCHER_HPP_

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