tier4 · TetsuKawa · Dec 13, 2023 · Dec 13, 2023 · Dec 14, 2023 · Dec 14, 2023
diff --git a/system/remapper/CMakeLists.txt b/system/remapper/CMakeLists.txt
@@ -0,0 +1,15 @@
+cmake_minimum_required(VERSION 3.14)
+project(remapper)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+ament_auto_add_executable(remapper
+  src/remapper_node.cpp
+  src/remapper_core.cpp
+)
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+  launch
+)
diff --git a/system/remapper/README.md b/system/remapper/README.md
@@ -0,0 +1,51 @@
+# remapper
+
+## Purpose
+
+Remapper is a node to remap some topics to topics added hostname prefix for multiple ECU configuration.
+
+## Inputs / Outputs
+
+### Input
+
+| Name                                     | Type                                                           |
+| ---------------------------------------- | -------------------------------------------------------------- |
+| `/system/operation_mode/availability`    | `tier4_system_msgs::msg::OperationModeAvailability`            |
+| `/j6/to_can_bus`                         | `can_msgs::msg::Frame`                                         |
+| `/control/command/control_cmd`           | `autoware_auto_control_msgs::msg::AckermannControlCommand`     |
+| `/tf`                                    | `tf2_msgs::msg::TFMessage`                                     |
+| `/api/operation_mode/state`              | `autoware_adapi_v1_msgs::msg::OperationModeState`              |
+| `/api/localization/initialization_state` | `autoware_adapi_v1_msgs::msg::LocalizationInitializationState` |
+| `/localization/pose_with_covariance`     | `geometry_msgs::msg::PoseWithCovarianceStamped`                |
+| `/api/routing/state`                     | `autoware_adapi_v1_msgs::msg::RouteState`                      |
+| `/api/routing/route`                     | `autoware_adapi_v1_msgs::msg::Route`                           |
+| `/autoware/state`                        | `autoware_auto_system_msgs::msg::AutowareState`                |
+| `/vehicle/status/control_mode`           | `autoware_auto_vehicle_msgs::msg::ControlModeReport`           |
+| `/api/external/get/emergency`            | `tier4_external_api_msgs::msg::Emergency`                      |
+| `/api/fail_safe/mrm_state`               | `autoware_adapi_v1_msgs::msg::MrmState`                        |
+| `/diagnostics_graph`                     | `tier4_system_msgs::msg::DiagnosticGraph`                      |
+| `/diagnostics_graph/supervisor`          | `tier4_system_msgs::msg::DiagnosticGraph`                      |
+
+### Output
+
+| Name                                                   | Type                                                           |
+| ------------------------------------------------------ | -------------------------------------------------------------- |
+| `/(main or sub)/system/operation_mode/availability`    | `tier4_system_msgs::msg::OperationModeAvailability`            |
+| `/(main or sub)/j6/to_can_bus`                         | `can_msgs::msg::Frame`                                         |
+| `/(main or sub)/control/command/control_cmd`           | `autoware_auto_control_msgs::msg::AckermannControlCommand`     |
+| `/(main or sub)/tf`                                    | `tf2_msgs::msg::TFMessage`                                     |
+| `/(main or sub)/api/operation_mode/state`              | `autoware_adapi_v1_msgs::msg::OperationModeState`              |
+| `/(main or sub)/api/localization/initialization_state` | `autoware_adapi_v1_msgs::msg::LocalizationInitializationState` |
+| `/(main or sub)/localization/pose_with_covariance`     | `geometry_msgs::msg::PoseWithCovarianceStamped`                |
+| `/(main or sub)/api/routing/state`                     | `autoware_adapi_v1_msgs::msg::RouteState`                      |
+| `/(main or sub)/api/routing/route`                     | `autoware_adapi_v1_msgs::msg::Route`                           |
+| `/(main or sub)/autoware/state`                        | `autoware_auto_system_msgs::msg::AutowareState`                |
+| `/(main or sub)/vehicle/status/control_mode`           | `autoware_auto_vehicle_msgs::msg::ControlModeReport`           |
+| `/(main or sub)/api/external/get/emergency`            | `tier4_external_api_msgs::msg::Emergency`                      |
+| `/(main or sub)/api/fail_safe/mrm_state`               | `autoware_adapi_v1_msgs::msg::MrmState`                        |
+| `/(main or sub)/diagnostics_graph`                     | `tier4_system_msgs::msg::DiagnosticGraph`                      |
+| `/(main or sub)/diagnostics_graph/supervisor`          | `tier4_system_msgs::msg::DiagnosticGraph`                      |
+
+## Parameters
+
+none
diff --git a/system/remapper/include/remapper/remapper_core.hpp b/system/remapper/include/remapper/remapper_core.hpp
@@ -0,0 +1,120 @@
+// Copyright 2023 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 REMAPPER__REMAPPER_CORE_HPP_
+#define REMAPPER__REMAPPER_CORE_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <autoware_adapi_v1_msgs/msg/localization_initialization_state.hpp>
+#include <autoware_adapi_v1_msgs/msg/mrm_state.hpp>
+#include <autoware_adapi_v1_msgs/msg/operation_mode_state.hpp>
+#include <autoware_adapi_v1_msgs/msg/route.hpp>
+#include <autoware_adapi_v1_msgs/msg/route_state.hpp>
+#include <autoware_auto_control_msgs/msg/ackermann_control_command.hpp>
+#include <autoware_auto_system_msgs/msg/autoware_state.hpp>
+#include <autoware_auto_vehicle_msgs/msg/control_mode_report.hpp>
+#include <can_msgs/msg/frame.hpp>
+#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
+#include <tf2_msgs/msg/tf_message.hpp>
+#include <tier4_external_api_msgs/msg/emergency.hpp>
+#include <tier4_system_msgs/msg/diagnostic_graph.hpp>
+#include <tier4_system_msgs/msg/operation_mode_availability.hpp>
+
+class Remapper : public rclcpp::Node
+{
+public:
+  Remapper();
+
+private:
+  // Subscriber
+  rclcpp::Subscription<tier4_system_msgs::msg::OperationModeAvailability>::SharedPtr
+    sub_operation_mode_availability_;
+  rclcpp::Subscription<can_msgs::msg::Frame>::SharedPtr sub_to_can_bus_;
+  rclcpp::Subscription<autoware_auto_control_msgs::msg::AckermannControlCommand>::SharedPtr
+    sub_control_cmd_;
+  rclcpp::Subscription<tf2_msgs::msg::TFMessage>::SharedPtr sub_tf_;
+
+  rclcpp::Subscription<autoware_adapi_v1_msgs::msg::OperationModeState>::SharedPtr
+    sub_operation_mode_state_;
+  rclcpp::Subscription<autoware_adapi_v1_msgs::msg::LocalizationInitializationState>::SharedPtr
+    sub_initialization_state_;
+  rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr
+    sub_pose_with_covariance_;
+  rclcpp::Subscription<autoware_adapi_v1_msgs::msg::RouteState>::SharedPtr sub_routing_state_;
+  rclcpp::Subscription<autoware_adapi_v1_msgs::msg::Route>::SharedPtr sub_routing_route_;
+
+  rclcpp::Subscription<autoware_auto_system_msgs::msg::AutowareState>::SharedPtr
+    sub_autoware_state_;
+  rclcpp::Subscription<autoware_auto_vehicle_msgs::msg::ControlModeReport>::SharedPtr
+    sub_control_mode_report_;
+  rclcpp::Subscription<tier4_external_api_msgs::msg::Emergency>::SharedPtr sub_get_emergency_;
+  rclcpp::Subscription<autoware_adapi_v1_msgs::msg::MrmState>::SharedPtr sub_mrm_state_;
+  rclcpp::Subscription<tier4_system_msgs::msg::DiagnosticGraph>::SharedPtr sub_diagnostics_graph_;
+  rclcpp::Subscription<tier4_system_msgs::msg::DiagnosticGraph>::SharedPtr
+    sub_diagnostics_graph_supervisor_;
+
+  // Publisher
+  rclcpp::Publisher<tier4_system_msgs::msg::OperationModeAvailability>::SharedPtr
+    pub_operation_mode_availability_;
+  rclcpp::Publisher<can_msgs::msg::Frame>::SharedPtr pub_to_can_bus_;
+  rclcpp::Publisher<autoware_auto_control_msgs::msg::AckermannControlCommand>::SharedPtr
+    pub_control_cmd_;
+  rclcpp::Publisher<tf2_msgs::msg::TFMessage>::SharedPtr pub_tf_;
+
+  rclcpp::Publisher<autoware_adapi_v1_msgs::msg::OperationModeState>::SharedPtr
+    pub_operation_mode_state_;
+  rclcpp::Publisher<autoware_adapi_v1_msgs::msg::LocalizationInitializationState>::SharedPtr
+    pub_initialization_state_;
+  rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr
+    pub_pose_with_covariance_;
+  rclcpp::Publisher<autoware_adapi_v1_msgs::msg::RouteState>::SharedPtr pub_routing_state_;
+  rclcpp::Publisher<autoware_adapi_v1_msgs::msg::Route>::SharedPtr pub_routing_route_;
+
+  rclcpp::Publisher<autoware_auto_system_msgs::msg::AutowareState>::SharedPtr pub_autoware_state_;
+  rclcpp::Publisher<autoware_auto_vehicle_msgs::msg::ControlModeReport>::SharedPtr
+    pub_control_mode_report_;
+  rclcpp::Publisher<tier4_external_api_msgs::msg::Emergency>::SharedPtr pub_get_emergency_;
+  rclcpp::Publisher<autoware_adapi_v1_msgs::msg::MrmState>::SharedPtr pub_mrm_state_;
+  rclcpp::Publisher<tier4_system_msgs::msg::DiagnosticGraph>::SharedPtr pub_diagnostics_graph_;
+  rclcpp::Publisher<tier4_system_msgs::msg::DiagnosticGraph>::SharedPtr
+    pub_diagnostics_graph_supervisor_;
+
+  void onOperationModeAvailability(
+    const tier4_system_msgs::msg::OperationModeAvailability::ConstSharedPtr msg);
+  void onToCanBus(const can_msgs::msg::Frame::ConstSharedPtr msg);
+  void onControlCmd(
+    const autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr msg);
+  void onTransform(const tf2_msgs::msg::TFMessage::ConstSharedPtr msg);
+
+  void onOperationModeState(
+    const autoware_adapi_v1_msgs::msg::OperationModeState::ConstSharedPtr msg);
+  void onLocalizationInitializationState(
+    const autoware_adapi_v1_msgs::msg::LocalizationInitializationState::ConstSharedPtr msg);
+  void onPoseWithCovarianceStamped(
+    const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr msg);
+  void onRouteState(const autoware_adapi_v1_msgs::msg::RouteState::ConstSharedPtr msg);
+  void onRoute(const autoware_adapi_v1_msgs::msg::Route::ConstSharedPtr msg);
+
+  void onAutowareState(const autoware_auto_system_msgs::msg::AutowareState::ConstSharedPtr msg);
+  void onControlModeReport(
+    const autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg);
+  void onEmergency(const tier4_external_api_msgs::msg::Emergency::ConstSharedPtr msg);
+  void onMrmState(const autoware_adapi_v1_msgs::msg::MrmState::ConstSharedPtr msg);
+  void onDiagnosticGraph(const tier4_system_msgs::msg::DiagnosticGraph::ConstSharedPtr msg);
+  void onDiagnosticGraphSupervisor(
+    const tier4_system_msgs::msg::DiagnosticGraph::ConstSharedPtr msg);
+};
+
+#endif  // REMAPPER__REMAPPER_CORE_HPP_
diff --git a/system/remapper/launch/remapper.launch.xml b/system/remapper/launch/remapper.launch.xml
@@ -0,0 +1,64 @@
+<launch>
+  <arg name="ecu_name" default="main"/>
+  <arg name="input_operation_mode_availability" default="/system/operation_mode/availability"/>
+  <arg name="input_to_can_bus" default="/j6/to_can_bus"/>
+  <arg name="input_control_cmd" default="/control/command/control_cmd"/>
+  <arg name="input_tf" default="/tf"/>
+
+  <arg name="input_operation_mode_state" default="/api/operation_mode/state"/>
+  <arg name="input_initialization_state" default="/api/localization/initialization_state"/>
+  <arg name="input_pose_with_covariance" default="/localization/pose_with_covariance"/>
+  <arg name="input_routing_state" default="/api/routing/state"/>
+  <arg name="input_routing_route" default="/api/routing/route"/>
+
+  <arg name="input_autoware_state" default="/autoware/state"/>
+  <!-- <arg name="input_initialization_state" default="/api/localization/initialization_state"/> -->
+  <!-- <arg name="input_routing_state" default="/api/routing/state"/> -->
+  <!-- <arg name="input_operation_mode_state" default="/api/operation_mode/state"/> -->
+  <arg name="input_control_mode" default="/vehicle/status/control_mode"/>
+  <arg name="input_get_emergency" default="/api/external/get/emergency"/>
+  <arg name="input_mrm_state" default="/api/fail_safe/mrm_state"/>
+  <arg name="input_diagnostics_graph" default="/diagnostics_graph"/>
+  <arg name="input_diagnostics_graph_supervisor" default="/diagnostics_graph/supervisor"/>
+
+  <group>
+    <node pkg="remapper" exec="remapper" name="remapper" output="screen">
+      <!-- input -->
+      <remap from="~/input/system/operation_mode/availability" to="$(var input_operation_mode_availability)"/>
+      <remap from="~/input/j6/to_can_bus" to="$(var input_to_can_bus)"/>
+      <remap from="~/input/control/command/control_cmd" to="$(var input_control_cmd)"/>
+      <remap from="~/input/tf" to="$(var input_tf)"/>
+
+      <remap from="~/input/api/operation_mode/state" to="$(var input_operation_mode_state)"/>
+      <remap from="~/input/api/localization/initialization_state" to="$(var input_initialization_state)"/>
+      <remap from="~/input/localization/pose_with_covariance" to="$(var input_pose_with_covariance)"/>
+      <remap from="~/input/api/routing/state" to="$(var input_routing_state)"/>
+      <remap from="~/input/api/routing/route" to="$(var input_routing_route)"/>
+
+      <remap from="~/input/autoware/state" to="$(var input_autoware_state)"/>
+      <remap from="~/input/vehicle/status/control_mode" to="$(var input_control_mode)"/>
+      <remap from="~/input/api/external/get/emergency" to="$(var input_get_emergency)"/>
+      <remap from="~/input/api/fail_safe/mrm_state" to="$(var input_mrm_state)"/>
+      <remap from="~/input/diagnostics_graph" to="$(var input_diagnostics_graph)"/>
+      <remap from="~/input/diagnostics_graph/supervisor" to="$(var input_diagnostics_graph_supervisor)"/>
+      <!-- output -->
+      <remap from="~/output/system/operation_mode/availability" to="/$(var ecu_name)$(var input_operation_mode_availability)"/>
+      <remap from="~/output/j6/to_can_bus" to="/$(var ecu_name)$(var input_to_can_bus)"/>
+      <remap from="~/output/control/command/control_cmd" to="/$(var ecu_name)$(var input_control_cmd)"/>
+      <remap from="~/output/tf" to="/$(var ecu_name)$(var input_tf)"/>
+
+      <remap from="~/output/api/operation_mode/state" to="/$(var ecu_name)$(var input_operation_mode_state)"/>
+      <remap from="~/output/api/localization/initialization_state" to="/$(var ecu_name)$(var input_initialization_state)"/>
+      <remap from="~/output/localization/pose_with_covariance" to="/$(var ecu_name)$(var input_pose_with_covariance)"/>
+      <remap from="~/output/api/routing/state" to="/$(var ecu_name)$(var input_routing_state)"/>
+      <remap from="~/output/api/routing/route" to="/$(var ecu_name)$(var input_routing_route)"/>
+
+      <remap from="~/output/autoware/state" to="/$(var ecu_name)$(var input_autoware_state)"/>
+      <remap from="~/output/vehicle/status/control_mode" to="/$(var ecu_name)$(var input_control_mode)"/>
+      <remap from="~/output/api/external/get/emergency" to="/$(var ecu_name)$(var input_get_emergency)"/>
+      <remap from="~/output/api/fail_safe/mrm_state" to="/$(var ecu_name)$(var input_mrm_state)"/>
+      <remap from="~/output/diagnostics_graph" to="/$(var ecu_name)$(var input_diagnostics_graph)"/>
+      <remap from="~/output/diagnostics_graph/supervisor" to="/$(var ecu_name)$(var input_diagnostics_graph_supervisor)"/>
+    </node>
+  </group>
+</launch>
diff --git a/system/remapper/package.xml b/system/remapper/package.xml
@@ -0,0 +1,30 @@
+<?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>remapper</name>
+  <version>1.0.0</version>
+  <description>The remapper ROS 2 package for multiple ECU</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_adapi_v1_msgs</depend>
+  <depend>autoware_auto_control_msgs</depend>
+  <depend>autoware_auto_system_msgs</depend>
+  <depend>autoware_auto_vehicle_msgs</depend>
+  <depend>can_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>rclcpp</depend>
+  <depend>tf2_msgs</depend>
+  <depend>tier4_external_api_msgs</depend>
+  <depend>tier4_system_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>