feat: add external cmd selector package

control/external_cmd_selector/CMakeLists.txt

@@ -0,0 +1,34 @@
+cmake_minimum_required(VERSION 3.5)
+project(external_cmd_selector)
+
+### Compile options
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+  set(CMAKE_CXX_STANDARD_REQUIRED ON)
+  set(CMAKE_CXX_EXTENSIONS OFF)
+endif()
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic -Werror)
+endif()
+
+### Dependencies
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+ament_auto_add_library(external_cmd_selector_node SHARED
+  src/external_cmd_selector/external_cmd_selector_node.cpp
+)
+
+rclcpp_components_register_node(external_cmd_selector_node
+  PLUGIN "ExternalCmdSelector"
+  EXECUTABLE external_cmd_selector
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_auto_package(INSTALL_TO_SHARE
+  launch
+)

control/external_cmd_selector/include/external_cmd_selector/external_cmd_selector_node.hpp

@@ -0,0 +1,105 @@
+// Copyright 2020 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 EXTERNAL_CMD_SELECTOR__EXTERNAL_CMD_SELECTOR_NODE_HPP_
+#define EXTERNAL_CMD_SELECTOR__EXTERNAL_CMD_SELECTOR_NODE_HPP_
+
+#include <diagnostic_updater/diagnostic_updater.hpp>
+#include <diagnostic_updater/update_functions.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <autoware_auto_vehicle_msgs/msg/gear_command.hpp>
+#include <autoware_auto_vehicle_msgs/msg/hazard_lights_command.hpp>
+#include <autoware_auto_vehicle_msgs/msg/turn_indicators_command.hpp>
+#include <autoware_control_msgs/msg/external_command_selector_mode.hpp>
+#include <autoware_control_msgs/srv/external_command_select.hpp>
+#include <autoware_external_api_msgs/msg/control_command_stamped.hpp>
+#include <autoware_external_api_msgs/msg/gear_shift_stamped.hpp>
+#include <autoware_external_api_msgs/msg/heartbeat.hpp>
+#include <autoware_external_api_msgs/msg/turn_signal_stamped.hpp>
+
+#include <memory>
+
+class ExternalCmdSelector : public rclcpp::Node
+{
+public:
+  explicit ExternalCmdSelector(const rclcpp::NodeOptions & node_options);
+
+private:
+  using CommandSourceSelect = autoware_control_msgs::srv::ExternalCommandSelect;
+  using CommandSourceMode = autoware_control_msgs::msg::ExternalCommandSelectorMode;
+  using InternalGearShift = autoware_auto_vehicle_msgs::msg::GearCommand;
+  using InternalTurnSignal = autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand;
+  using InternalHazardSignal = autoware_auto_vehicle_msgs::msg::HazardLightsCommand;
+  using InternalHeartbeat = autoware_external_api_msgs::msg::Heartbeat;
+  using ExternalControlCommand = autoware_external_api_msgs::msg::ControlCommandStamped;
+  using ExternalGearShift = autoware_external_api_msgs::msg::GearShiftStamped;
+  using ExternalTurnSignal = autoware_external_api_msgs::msg::TurnSignalStamped;
+  using ExternalHeartbeat = autoware_external_api_msgs::msg::Heartbeat;
+
+  // CallbackGroups
+  rclcpp::CallbackGroup::SharedPtr callback_group_subscribers_;
+  rclcpp::CallbackGroup::SharedPtr callback_group_services_;
+
+  // Publisher
+  rclcpp::Publisher<CommandSourceMode>::SharedPtr pub_current_selector_mode_;
+  rclcpp::Publisher<ExternalControlCommand>::SharedPtr pub_control_cmd_;
+  rclcpp::Publisher<InternalGearShift>::SharedPtr pub_shift_cmd_;
+  rclcpp::Publisher<InternalTurnSignal>::SharedPtr pub_turn_signal_cmd_;
+  rclcpp::Publisher<InternalHazardSignal>::SharedPtr pub_hazard_signal_cmd_;
+  rclcpp::Publisher<InternalHeartbeat>::SharedPtr pub_heartbeat_;
+
+  // Subscriber
+  rclcpp::Subscription<ExternalControlCommand>::SharedPtr sub_local_control_cmd_;
+  rclcpp::Subscription<ExternalGearShift>::SharedPtr sub_local_shift_cmd_;
+  rclcpp::Subscription<ExternalTurnSignal>::SharedPtr sub_local_turn_signal_cmd_;
+  rclcpp::Subscription<ExternalHeartbeat>::SharedPtr sub_local_heartbeat_;
+
+  rclcpp::Subscription<ExternalControlCommand>::SharedPtr sub_remote_control_cmd_;
+  rclcpp::Subscription<ExternalGearShift>::SharedPtr sub_remote_shift_cmd_;
+  rclcpp::Subscription<ExternalTurnSignal>::SharedPtr sub_remote_turn_signal_cmd_;
+  rclcpp::Subscription<ExternalHeartbeat>::SharedPtr sub_remote_heartbeat_;
+
+  void onLocalControlCmd(const ExternalControlCommand::ConstSharedPtr msg);
+  void onLocalShiftCmd(const ExternalGearShift::ConstSharedPtr msg);
+  void onLocalTurnSignalCmd(const ExternalTurnSignal::ConstSharedPtr msg);
+  void onLocalHeartbeat(const ExternalHeartbeat::ConstSharedPtr msg);
+
+  void onRemoteControlCmd(const ExternalControlCommand::ConstSharedPtr msg);
+  void onRemoteShiftCmd(const ExternalGearShift::ConstSharedPtr msg);
+  void onRemoteTurnSignalCmd(const ExternalTurnSignal::ConstSharedPtr msg);
+  void onRemoteHeartbeat(const ExternalHeartbeat::ConstSharedPtr msg);
+
+  // Service
+  rclcpp::Service<CommandSourceSelect>::SharedPtr srv_select_external_command_;
+  CommandSourceMode current_selector_mode_;
+
+  bool onSelectExternalCommandService(
+    const CommandSourceSelect::Request::SharedPtr req,
+    const CommandSourceSelect::Response::SharedPtr res);
+
+  // Timer
+  rclcpp::TimerBase::SharedPtr timer_;
+
+  void onTimer();
+
+  // Converter
+  static InternalGearShift convert(const ExternalGearShift & command);
+  static InternalHeartbeat convert(const ExternalHeartbeat & command);
+
+  // Diagnostics Updater
+  diagnostic_updater::Updater updater_{this};
+};
+
+#endif  // EXTERNAL_CMD_SELECTOR__EXTERNAL_CMD_SELECTOR_NODE_HPP_

control/external_cmd_selector/launch/external_cmd_selector.launch.py

@@ -0,0 +1,123 @@
+# Copyright 2021 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.
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import LoadComposableNodes
+from launch_ros.descriptions import ComposableNode
+
+
+def _create_mapping_tuple(name):
+    return ("~/" + name, LaunchConfiguration(name))
+
+
+def generate_launch_description():
+
+    arguments = [
+        # component
+        DeclareLaunchArgument("use_intra_process"),
+        DeclareLaunchArgument("target_container"),
+        # settings
+        DeclareLaunchArgument(
+            "initial_selector_mode", default_value="local", choices=["local", "remote"]
+        ),
+        # service
+        DeclareLaunchArgument(
+            "service/select_external_command", default_value="~/select_external_command"
+        ),
+        # local input
+        DeclareLaunchArgument(
+            "input/local/control_cmd", default_value="/api/external/set/command/local/control"
+        ),
+        DeclareLaunchArgument(
+            "input/local/shift_cmd", default_value="/api/external/set/command/local/shift"
+        ),
+        DeclareLaunchArgument(
+            "input/local/turn_signal_cmd",
+            default_value="/api/external/set/command/local/turn_signal",
+        ),
+        DeclareLaunchArgument(
+            "input/local/heartbeat", default_value="/api/external/set/command/local/heartbeat"
+        ),
+        # remote input
+        DeclareLaunchArgument(
+            "input/remote/control_cmd", default_value="/api/external/set/command/remote/control"
+        ),
+        DeclareLaunchArgument(
+            "input/remote/shift_cmd", default_value="/api/external/set/command/remote/shift"
+        ),
+        DeclareLaunchArgument(
+            "input/remote/turn_signal_cmd",
+            default_value="/api/external/set/command/remote/turn_signal",
+        ),
+        DeclareLaunchArgument(
+            "input/remote/heartbeat", default_value="/api/external/set/command/remote/heartbeat"
+        ),
+        # output
+        DeclareLaunchArgument(
+            "output/control_cmd", default_value="/external/selected/external_control_cmd"
+        ),
+        DeclareLaunchArgument("output/gear_cmd", default_value="/external/selected/gear_cmd"),
+        DeclareLaunchArgument(
+            "output/turn_indicators_cmd", default_value="/external/selected/turn_indicators_cmd"
+        ),
+        DeclareLaunchArgument(
+            "output/hazard_lights_cmd", default_value="/external/selected/hazard_lights_cmd"
+        ),
+        DeclareLaunchArgument("output/heartbeat", default_value="/external/selected/heartbeat"),
+        DeclareLaunchArgument(
+            "output/current_selector_mode", default_value="~/current_selector_mode"
+        ),
+    ]
+
+    component = ComposableNode(
+        package="external_cmd_selector",
+        plugin="ExternalCmdSelector",
+        name="external_cmd_selector",
+        remappings=[
+            _create_mapping_tuple("service/select_external_command"),
+            _create_mapping_tuple("input/local/control_cmd"),
+            _create_mapping_tuple("input/local/shift_cmd"),
+            _create_mapping_tuple("input/local/turn_signal_cmd"),
+            _create_mapping_tuple("input/local/heartbeat"),
+            _create_mapping_tuple("input/remote/control_cmd"),
+            _create_mapping_tuple("input/remote/shift_cmd"),
+            _create_mapping_tuple("input/remote/turn_signal_cmd"),
+            _create_mapping_tuple("input/remote/heartbeat"),
+            _create_mapping_tuple("output/control_cmd"),
+            _create_mapping_tuple("output/gear_cmd"),
+            _create_mapping_tuple("output/turn_indicators_cmd"),
+            _create_mapping_tuple("output/hazard_lights_cmd"),
+            _create_mapping_tuple("output/heartbeat"),
+            _create_mapping_tuple("output/current_selector_mode"),
+        ],
+        parameters=[
+            {
+                "initial_selector_mode": LaunchConfiguration("initial_selector_mode"),
+            }
+        ],
+        extra_arguments=[
+            {
+                "use_intra_process_comms": LaunchConfiguration("use_intra_process"),
+            }
+        ],
+    )
+
+    loader = LoadComposableNodes(
+        composable_node_descriptions=[component],
+        target_container=LaunchConfiguration("target_container"),
+    )
+
+    return LaunchDescription(arguments + [loader])

control/external_cmd_selector/launch/external_cmd_selector.launch.xml

@@ -0,0 +1,49 @@
+<launch>
+
+  <!-- settings -->
+  <arg name="initial_selector_mode" default="local" description="local, remote" />
+
+  <!-- service -->
+  <arg name="service/select_external_command" default="~/select_external_command" />
+
+  <!-- input local -->
+  <arg name="input/local/control_cmd" default="/api/external/set/command/local/control" />
+  <arg name="input/local/shift_cmd" default="/api/external/set/command/local/shift" />
+  <arg name="input/local/turn_signal_cmd" default="/api/external/set/command/local/turn_signal" />
+  <arg name="input/local/heartbeat" default="/api/external/set/command/local/heartbeat" />
+
+  <!-- input remote -->
+  <arg name="input/remote/control_cmd" default="/api/external/set/command/remote/control" />
+  <arg name="input/remote/shift_cmd" default="/api/external/set/command/remote/shift" />
+  <arg name="input/remote/turn_signal_cmd" default="/api/external/set/command/remote/turn_signal" />
+  <arg name="input/remote/heartbeat" default="/api/external/set/command/remote/heartbeat" />
+
+  <!-- output -->
+  <arg name="output/control_cmd" default="/external/selected/external_control_cmd" />
+  <arg name="output/gear_cmd" default="/external/selected/gear_cmd" />
+  <arg name="output/turn_indicators_cmd" default="/external/selected/turn_indicators_cmd" />
+  <arg name="output/hazard_lights_cmd" default="/external/selected/hazard_lights_cmd" />
+  <arg name="output/heartbeat" default="/external/selected/heartbeat" />
+  <arg name="output/current_selector_mode" default="~/current_selector_mode" />
+
+  <!-- node -->
+  <node pkg="external_cmd_selector" exec="external_cmd_selector" name="external_cmd_selector" output="screen">
+    <remap from="~/service/select_external_command" to="$(var service/select_external_command)" />
+    <remap from="~/input/local/control_cmd" to="$(var input/local/control_cmd)" />
+    <remap from="~/input/local/shift_cmd" to="$(var input/local/shift_cmd)" />
+    <remap from="~/input/local/turn_signal_cmd" to="$(var input/local/turn_signal_cmd)" />
+    <remap from="~/input/local/heartbeat" to="$(var input/local/heartbeat)" />
+    <remap from="~/input/remote/control_cmd" to="$(var input/remote/control_cmd)" />
+    <remap from="~/input/remote/shift_cmd" to="$(var input/remote/shift_cmd)" />
+    <remap from="~/input/remote/turn_signal_cmd" to="$(var input/remote/turn_signal_cmd)" />
+    <remap from="~/input/remote/heartbeat" to="$(var input/remote/heartbeat)" />
+    <remap from="~/output/control_cmd" to="$(var output/control_cmd)" />
+    <remap from="~/output/gear_cmd" to="$(var output/gear_cmd)" />
+    <remap from="~/output/turn_indicators_cmd" to="$(var output/turn_indicators_cmd)" />
+    <remap from="~/output/hazard_lights_cmd" to="$(var output/hazard_lights_cmd)" />
+    <remap from="~/output/heartbeat" to="$(var output/heartbeat)" />
+    <remap from="~/output/current_selector_mode" to="$(var output/current_selector_mode)" />
+    <param name="initial_selector_mode" value="$(var initial_selector_mode)" />
+  </node>
+
+</launch>

control/external_cmd_selector/package.xml

@@ -0,0 +1,28 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>external_cmd_selector</name>
+  <version>0.1.0</version>
+  <description>The external_cmd_selector package</description>
+  <maintainer email="[email protected]">Kenji Miyake</maintainer>
+  <author email="[email protected]">Kenji Miyake</author>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <depend>autoware_auto_vehicle_msgs</depend>
+  <depend>autoware_control_msgs</depend>
+  <depend>autoware_external_api_msgs</depend>
+  <depend>autoware_iv_auto_msgs_converter</depend>
+  <depend>diagnostic_updater</depend>
+  <depend>geometry_msgs</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>std_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>