diff --git a/system/remapper/CMakeLists.txt b/system/remapper/CMakeLists.txt new file mode 100644 index 0000000000000..cbaf563f9d641 --- /dev/null +++ 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 new file mode 100644 index 0000000000000..8f8db7e34a325 --- /dev/null +++ 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 new file mode 100644 index 0000000000000..51240a398b5a7 --- /dev/null +++ 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 + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +class Remapper : public rclcpp::Node +{ +public: + Remapper(); + +private: + // Subscriber + rclcpp::Subscription::SharedPtr + sub_operation_mode_availability_; + rclcpp::Subscription::SharedPtr sub_to_can_bus_; + rclcpp::Subscription::SharedPtr + sub_control_cmd_; + rclcpp::Subscription::SharedPtr sub_tf_; + + rclcpp::Subscription::SharedPtr + sub_operation_mode_state_; + rclcpp::Subscription::SharedPtr + sub_initialization_state_; + rclcpp::Subscription::SharedPtr + sub_pose_with_covariance_; + rclcpp::Subscription::SharedPtr sub_routing_state_; + rclcpp::Subscription::SharedPtr sub_routing_route_; + + rclcpp::Subscription::SharedPtr + sub_autoware_state_; + rclcpp::Subscription::SharedPtr + sub_control_mode_report_; + rclcpp::Subscription::SharedPtr sub_get_emergency_; + rclcpp::Subscription::SharedPtr sub_mrm_state_; + rclcpp::Subscription::SharedPtr sub_diagnostics_graph_; + rclcpp::Subscription::SharedPtr + sub_diagnostics_graph_supervisor_; + + // Publisher + rclcpp::Publisher::SharedPtr + pub_operation_mode_availability_; + rclcpp::Publisher::SharedPtr pub_to_can_bus_; + rclcpp::Publisher::SharedPtr + pub_control_cmd_; + rclcpp::Publisher::SharedPtr pub_tf_; + + rclcpp::Publisher::SharedPtr + pub_operation_mode_state_; + rclcpp::Publisher::SharedPtr + pub_initialization_state_; + rclcpp::Publisher::SharedPtr + pub_pose_with_covariance_; + rclcpp::Publisher::SharedPtr pub_routing_state_; + rclcpp::Publisher::SharedPtr pub_routing_route_; + + rclcpp::Publisher::SharedPtr pub_autoware_state_; + rclcpp::Publisher::SharedPtr + pub_control_mode_report_; + rclcpp::Publisher::SharedPtr pub_get_emergency_; + rclcpp::Publisher::SharedPtr pub_mrm_state_; + rclcpp::Publisher::SharedPtr pub_diagnostics_graph_; + rclcpp::Publisher::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 new file mode 100644 index 0000000000000..9cba6478baa44 --- /dev/null +++ b/system/remapper/launch/remapper.launch.xml @@ -0,0 +1,64 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/system/remapper/package.xml b/system/remapper/package.xml new file mode 100644 index 0000000000000..1ef2dd56cb6c5 --- /dev/null +++ b/system/remapper/package.xml @@ -0,0 +1,30 @@ + + + + remapper + 1.0.0 + The remapper ROS 2 package for multiple ECU + Tetsuhiro Kawaguchi + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + autoware_adapi_v1_msgs + autoware_auto_control_msgs + autoware_auto_system_msgs + autoware_auto_vehicle_msgs + can_msgs + geometry_msgs + rclcpp + tf2_msgs + tier4_external_api_msgs + tier4_system_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/system/remapper/src/remapper_core.cpp b/system/remapper/src/remapper_core.cpp new file mode 100644 index 0000000000000..c877f99c2c847 --- /dev/null +++ b/system/remapper/src/remapper_core.cpp @@ -0,0 +1,179 @@ +// 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. + +#include "remapper/remapper_core.hpp" + +Remapper::Remapper() : Node("remapper") +{ + using std::placeholders::_1; + + // Subscriber + sub_operation_mode_availability_ = + create_subscription( + "~/input/system/operation_mode/availability", rclcpp::QoS{1}, + std::bind(&Remapper::onOperationModeAvailability, this, _1)); + sub_to_can_bus_ = create_subscription( + "~/input/j6/to_can_bus", rclcpp::QoS{1}, std::bind(&Remapper::onToCanBus, this, _1)); + sub_control_cmd_ = create_subscription( + "~/input/control/command/control_cmd", rclcpp::QoS{1}, + std::bind(&Remapper::onControlCmd, this, _1)); + sub_tf_ = create_subscription( + "~/input/tf", rclcpp::QoS{1}, std::bind(&Remapper::onTransform, this, _1)); + sub_operation_mode_state_ = create_subscription( + "~/input/api/operation_mode/state", rclcpp::QoS{1}, + std::bind(&Remapper::onOperationModeState, this, _1)); + sub_initialization_state_ = + create_subscription( + "~/input/api/localization/initialization_state", rclcpp::QoS{1}, + std::bind(&Remapper::onLocalizationInitializationState, this, _1)); + sub_pose_with_covariance_ = create_subscription( + "~/input/localization/pose_with_covariance", rclcpp::QoS{1}, + std::bind(&Remapper::onPoseWithCovarianceStamped, this, _1)); + sub_routing_state_ = create_subscription( + "~/input/api/routing/state", rclcpp::QoS{1}, std::bind(&Remapper::onRouteState, this, _1)); + sub_routing_route_ = create_subscription( + "~/input/api/routing/route", rclcpp::QoS{1}, std::bind(&Remapper::onRoute, this, _1)); + sub_autoware_state_ = create_subscription( + "~/input/autoware/state", rclcpp::QoS{1}, std::bind(&Remapper::onAutowareState, this, _1)); + sub_control_mode_report_ = + create_subscription( + "~/input/vehicle/status/control_mode", rclcpp::QoS{1}, + std::bind(&Remapper::onControlModeReport, this, _1)); + sub_get_emergency_ = create_subscription( + "~/input/api/external/get/emergency", rclcpp::QoS{1}, + std::bind(&Remapper::onEmergency, this, _1)); + sub_mrm_state_ = create_subscription( + "~/input/api/fail_safe/mrm_state", rclcpp::QoS{1}, std::bind(&Remapper::onMrmState, this, _1)); + sub_diagnostics_graph_ = create_subscription( + "~/input/diagnostics_graph", rclcpp::QoS{1}, std::bind(&Remapper::onDiagnosticGraph, this, _1)); + sub_diagnostics_graph_supervisor_ = create_subscription( + "~/input/diagnostics_graph/supervisor", rclcpp::QoS{1}, + std::bind(&Remapper::onDiagnosticGraphSupervisor, this, _1)); + + // Publisher + pub_operation_mode_availability_ = + create_publisher( + "~/output/system/operation_mode/availability", rclcpp::QoS{1}); + pub_to_can_bus_ = + create_publisher("~/output/j6/to_can_bus", rclcpp::QoS{1}); + pub_control_cmd_ = create_publisher( + "~/output/control/command/control_cmd", rclcpp::QoS{1}); + pub_tf_ = create_publisher("~/output/tf", rclcpp::QoS{1}); + pub_operation_mode_state_ = create_publisher( + "~/output/api/operation_mode/state", rclcpp::QoS{1}); + pub_initialization_state_ = + create_publisher( + "~/output/api/localization/initialization_state", rclcpp::QoS{1}); + pub_pose_with_covariance_ = create_publisher( + "~/output/localization/pose_with_covariance", rclcpp::QoS{1}); + pub_routing_state_ = create_publisher( + "~/output/api/routing/state", rclcpp::QoS{1}); + pub_routing_route_ = create_publisher( + "~/output/api/routing/route", rclcpp::QoS{1}); + pub_autoware_state_ = create_publisher( + "~/output/autoware/state", rclcpp::QoS{1}); + pub_control_mode_report_ = create_publisher( + "~/output/vehicle/status/control_mode", rclcpp::QoS{1}); + pub_get_emergency_ = create_publisher( + "~/output/api/external/get/emergency", rclcpp::QoS{1}); + pub_mrm_state_ = create_publisher( + "~/output/api/fail_safe/mrm_state", rclcpp::QoS{1}); + pub_diagnostics_graph_ = create_publisher( + "~/output/diagnostics_graph", rclcpp::QoS{1}); + pub_diagnostics_graph_supervisor_ = create_publisher( + "~/output/diagnostics_graph/supervisor", rclcpp::QoS{1}); +} + +void Remapper::onOperationModeAvailability( + const tier4_system_msgs::msg::OperationModeAvailability::ConstSharedPtr msg) +{ + pub_operation_mode_availability_->publish(*msg); +} + +void Remapper::onToCanBus(const can_msgs::msg::Frame::ConstSharedPtr msg) +{ + pub_to_can_bus_->publish(*msg); +} + +void Remapper::onControlCmd( + const autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr msg) +{ + pub_control_cmd_->publish(*msg); +} + +void Remapper::onTransform(const tf2_msgs::msg::TFMessage::ConstSharedPtr msg) +{ + pub_tf_->publish(*msg); +} + +void Remapper::onOperationModeState( + const autoware_adapi_v1_msgs::msg::OperationModeState::ConstSharedPtr msg) +{ + pub_operation_mode_state_->publish(*msg); +} + +void Remapper::onLocalizationInitializationState( + const autoware_adapi_v1_msgs::msg::LocalizationInitializationState::ConstSharedPtr msg) +{ + pub_initialization_state_->publish(*msg); +} + +void Remapper::onPoseWithCovarianceStamped( + const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr msg) +{ + pub_pose_with_covariance_->publish(*msg); +} + +void Remapper::onRouteState(const autoware_adapi_v1_msgs::msg::RouteState::ConstSharedPtr msg) +{ + pub_routing_state_->publish(*msg); +} + +void Remapper::onRoute(const autoware_adapi_v1_msgs::msg::Route::ConstSharedPtr msg) +{ + pub_routing_route_->publish(*msg); +} + +void Remapper::onAutowareState( + const autoware_auto_system_msgs::msg::AutowareState::ConstSharedPtr msg) +{ + pub_autoware_state_->publish(*msg); +} + +void Remapper::onControlModeReport( + const autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg) +{ + pub_control_mode_report_->publish(*msg); +} + +void Remapper::onEmergency(const tier4_external_api_msgs::msg::Emergency::ConstSharedPtr msg) +{ + pub_get_emergency_->publish(*msg); +} + +void Remapper::onMrmState(const autoware_adapi_v1_msgs::msg::MrmState::ConstSharedPtr msg) +{ + pub_mrm_state_->publish(*msg); +} + +void Remapper::onDiagnosticGraph(const tier4_system_msgs::msg::DiagnosticGraph::ConstSharedPtr msg) +{ + pub_diagnostics_graph_->publish(*msg); +} + +void Remapper::onDiagnosticGraphSupervisor( + const tier4_system_msgs::msg::DiagnosticGraph::ConstSharedPtr msg) +{ + pub_diagnostics_graph_supervisor_->publish(*msg); +} diff --git a/system/remapper/src/remapper_node.cpp b/system/remapper/src/remapper_node.cpp new file mode 100644 index 0000000000000..8845475c792a8 --- /dev/null +++ b/system/remapper/src/remapper_node.cpp @@ -0,0 +1,32 @@ +// 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. + +#include "remapper/remapper_core.hpp" + +#include + +#include + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::executors::MultiThreadedExecutor executor; + auto node = std::make_shared(); + executor.add_node(node); + executor.spin(); + executor.remove_node(node); + rclcpp::shutdown(); + + return 0; +}