From 3c5ffa9c673857bad6a44cd3a68bf9c092705ad2 Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Wed, 13 Dec 2023 16:13:13 +0900 Subject: [PATCH 1/9] feat: add remapper node --- system/remapper/CMakeLists.txt | 15 ++ system/remapper/README.md | 2 + .../include/remapper/remapper_core.hpp | 103 ++++++++++++++ system/remapper/launch/remapper.launch.xml | 71 ++++++++++ system/remapper/package.xml | 30 ++++ system/remapper/src/remapper_core.cpp | 132 ++++++++++++++++++ system/remapper/src/remapper_node.cpp | 32 +++++ 7 files changed, 385 insertions(+) create mode 100644 system/remapper/CMakeLists.txt create mode 100644 system/remapper/README.md create mode 100644 system/remapper/include/remapper/remapper_core.hpp create mode 100644 system/remapper/launch/remapper.launch.xml create mode 100644 system/remapper/package.xml create mode 100644 system/remapper/src/remapper_core.cpp create mode 100644 system/remapper/src/remapper_node.cpp diff --git a/system/remapper/CMakeLists.txt b/system/remapper/CMakeLists.txt new file mode 100644 index 0000000000000..1b370d10fda3c --- /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 +) \ No newline at end of file diff --git a/system/remapper/README.md b/system/remapper/README.md new file mode 100644 index 0000000000000..7c8b2f33e53e8 --- /dev/null +++ b/system/remapper/README.md @@ -0,0 +1,2 @@ +# remapper + diff --git a/system/remapper/include/remapper/remapper_core.hpp b/system/remapper/include/remapper/remapper_core.hpp new file mode 100644 index 0000000000000..a4725b0e298b5 --- /dev/null +++ b/system/remapper/include/remapper/remapper_core.hpp @@ -0,0 +1,103 @@ +// 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: + explicit 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_controle_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_controle_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 autoware_adapi_v1_msgs::msg::OperationModeState::ConstSharedPtr msg); + + void onOperationModeState(const autoware_adapi_v1_msgs::msg::LocalizationInitializationState::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_ \ No newline at end of file diff --git a/system/remapper/launch/remapper.launch.xml b/system/remapper/launch/remapper.launch.xml new file mode 100644 index 0000000000000..a18e18eeaf4af --- /dev/null +++ b/system/remapper/launch/remapper.launch.xml @@ -0,0 +1,71 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/system/remapper/package.xml b/system/remapper/package.xml new file mode 100644 index 0000000000000..9ef71567e5981 --- /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 + + rclcpp + can_msgs + watchdog_system_msgs + tier4_system_msgs + tf2_msgs + autoware_adapi_v1_msgs + geometry_msgs + autoware_auto_system_msgs + autoware_auto_vehicle_msgs + tier4_external_api_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..a8770650323d5 --- /dev/null +++ b/system/remapper/src/remapper_core.cpp @@ -0,0 +1,132 @@ +// 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(&EmergencyHandler::onOperationModeAvailability, this, _1)); + sub_to_can_bus_ = create_subscription( + "~/input/j6/to_can_bus", rclcpp::QoS{1}, std::bind(&EmergencyHandler::onToCanBus, this, _1)); + sub_control_cmd_ = create_subscription( + "~/input/control/command/control_cmd", rclcpp::QoS{1}, std::bind(&EmergencyHandler::onControlCmd, this, _1)); + sub_tf_ = create_subscription( + "~/input/tf", rclcpp::QoS{1}, std::bind(&EmergencyHandler::onTransform, this, _1)); + sub_operation_mode_state_ = create_subscription( + "~/input/api/operation_mode/state", rclcpp::QoS{1}, std::bind(&EmergencyHandler::onOperationModeState, this, _1)); + sub_initialization_state_ = create_subscription( + "~/input/api/localization/initialization_state", rclcpp::QoS{1}, std::bind(&EmergencyHandler::onLocalizationInitializationState, this, _1)); + sub_pose_with_covariance_ = create_subscription( + "~/input/localization/pose_with_covariance", rclcpp::QoS{1}, std::bind(&EmergencyHandler::onPoseWithCovarianceStamped, this, _1)); + sub_routing_state_ = create_subscription( + "~/input/api/routing/state", rclcpp::QoS{1}, std::bind(&EmergencyHandler::onRouteState, this, _1)); + sub_routing_route_ = create_subscription( + "~/input/api/routing/route", rclcpp::QoS{1}, std::bind(&EmergencyHandler::onRoute, this, _1)); + sub_autoware_state_ = create_subscription( + "~/input/autoware/state", rclcpp::QoS{1}, std::bind(&EmergencyHandler::onAutowareState, this, _1)); + sub_controle_mode_report_ = create_subscription( + "~/input/vehicle/status/control_mode", rclcpp::QoS{1}, std::bind(&EmergencyHandler::onControlModeReport, this, _1)); + sub_get_emergency_ = create_subscription( + "~/input/api/external/get/emergency", rclcpp::QoS{1}, std::bind(&EmergencyHandler::onEmergency, this, _1)); + sub_mrm_state_ = create_subscription( + "~/input/api/fail_safe/mrm_state", rclcpp::QoS{1}, std::bind(&EmergencyHandler::onMrmState, this, _1)); + sub_diagnostics_graph_ = create_subscription( + "~/input/diagnostics_graph", rclcpp::QoS{1}, std::bind(&EmergencyHandler::onDiagnosticGraph, this, _1)); + sub_diagnostics_graph_supervisor_ = create_subscription( + "~/input/diagnostics_graph/supervisor", rclcpp::QoS{1}, std::bind(&EmergencyHandler::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_controle_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 autoware_adapi_v1_msgs::msg::OperationModeState::ConstSharedPtr msg) { + pub_tf_->publish(*msg); +} + +void Remapper::onOperationModeState(const autoware_adapi_v1_msgs::msg::LocalizationInitializationState::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_controle_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; +} From 85aff3e3041dfeeea14e163e50ef1e9d1f6d8e37 Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Wed, 13 Dec 2023 18:30:17 +0900 Subject: [PATCH 2/9] feat: fix mistakes --- .../include/remapper/remapper_core.hpp | 8 ++--- system/remapper/package.xml | 2 +- system/remapper/src/remapper_core.cpp | 34 +++++++++---------- 3 files changed, 22 insertions(+), 22 deletions(-) diff --git a/system/remapper/include/remapper/remapper_core.hpp b/system/remapper/include/remapper/remapper_core.hpp index a4725b0e298b5..74d6a1a2e5308 100644 --- a/system/remapper/include/remapper/remapper_core.hpp +++ b/system/remapper/include/remapper/remapper_core.hpp @@ -17,9 +17,9 @@ # include -#include -#include #include +#include +#include #include #include @@ -83,9 +83,9 @@ class Remapper : public rclcpp::Node 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 autoware_adapi_v1_msgs::msg::OperationModeState::ConstSharedPtr msg); + void onTransform(const tf2_msgs::msg::TFMessage::ConstSharedPtr msg); - void onOperationModeState(const autoware_adapi_v1_msgs::msg::LocalizationInitializationState::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); diff --git a/system/remapper/package.xml b/system/remapper/package.xml index 9ef71567e5981..eeda94a0534fe 100644 --- a/system/remapper/package.xml +++ b/system/remapper/package.xml @@ -12,7 +12,7 @@ rclcpp can_msgs - watchdog_system_msgs + autoware_auto_control_msgs tier4_system_msgs tf2_msgs autoware_adapi_v1_msgs diff --git a/system/remapper/src/remapper_core.cpp b/system/remapper/src/remapper_core.cpp index a8770650323d5..1bbed085898f3 100644 --- a/system/remapper/src/remapper_core.cpp +++ b/system/remapper/src/remapper_core.cpp @@ -21,35 +21,35 @@ Remapper::Remapper() //Subscriber sub_operation_mode_availability_ = create_subscription( - "~/input/system/operation_mode/availability", rclcpp::QoS{1}, std::bind(&EmergencyHandler::onOperationModeAvailability, this, _1)); + "~/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(&EmergencyHandler::onToCanBus, this, _1)); + "~/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(&EmergencyHandler::onControlCmd, this, _1)); + "~/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(&EmergencyHandler::onTransform, this, _1)); + "~/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(&EmergencyHandler::onOperationModeState, this, _1)); + "~/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(&EmergencyHandler::onLocalizationInitializationState, this, _1)); + "~/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(&EmergencyHandler::onPoseWithCovarianceStamped, this, _1)); + "~/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(&EmergencyHandler::onRouteState, this, _1)); + "~/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(&EmergencyHandler::onRoute, this, _1)); + "~/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(&EmergencyHandler::onAutowareState, this, _1)); + "~/input/autoware/state", rclcpp::QoS{1}, std::bind(&Remapper::onAutowareState, this, _1)); sub_controle_mode_report_ = create_subscription( - "~/input/vehicle/status/control_mode", rclcpp::QoS{1}, std::bind(&EmergencyHandler::onControlModeReport, this, _1)); + "~/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(&EmergencyHandler::onEmergency, this, _1)); + "~/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(&EmergencyHandler::onMrmState, this, _1)); + "~/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(&EmergencyHandler::onDiagnosticGraph, this, _1)); + "~/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(&EmergencyHandler::onDiagnosticGraphSupervisor, this, _1)); + "~/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}); @@ -81,11 +81,11 @@ void Remapper::onControlCmd(const autoware_auto_control_msgs::msg::AckermannCont pub_control_cmd_->publish(*msg); } -void Remapper::onTransform(const autoware_adapi_v1_msgs::msg::OperationModeState::ConstSharedPtr msg) { +void Remapper::onTransform(const tf2_msgs::msg::TFMessage::ConstSharedPtr msg) { pub_tf_->publish(*msg); } -void Remapper::onOperationModeState(const autoware_adapi_v1_msgs::msg::LocalizationInitializationState::ConstSharedPtr msg) { +void Remapper::onOperationModeState(const autoware_adapi_v1_msgs::msg::OperationModeState::ConstSharedPtr msg) { pub_operation_mode_state_->publish(*msg); } From 009e0f0de9434e2774ae601ce637a86789815dbe Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Thu, 14 Dec 2023 11:53:34 +0900 Subject: [PATCH 3/9] feat: fix launch mistakes --- system/remapper/launch/remapper.launch.xml | 35 ++++++++++------------ 1 file changed, 16 insertions(+), 19 deletions(-) diff --git a/system/remapper/launch/remapper.launch.xml b/system/remapper/launch/remapper.launch.xml index a18e18eeaf4af..fd7d125378531 100644 --- a/system/remapper/launch/remapper.launch.xml +++ b/system/remapper/launch/remapper.launch.xml @@ -1,7 +1,6 @@ - @@ -24,12 +23,10 @@ - - + - @@ -49,23 +46,23 @@ - - - - + + + + - - - - - + + + + + - - - - - - + + + + + + \ No newline at end of file From 18f9d4083bdd3ca40d074ee38f2999fb29d6fe2a Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Thu, 14 Dec 2023 12:20:45 +0900 Subject: [PATCH 4/9] feat: add Readme --- system/remapper/README.md | 48 +++++++++++++++++++++++++++++++++++++++ 1 file changed, 48 insertions(+) diff --git a/system/remapper/README.md b/system/remapper/README.md index 7c8b2f33e53e8..93ba33175b4a7 100644 --- a/system/remapper/README.md +++ b/system/remapper/README.md @@ -1,2 +1,50 @@ # 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 \ No newline at end of file From 2c6a5e6ed02943a38c119b69e8bbb92067e9b0dc Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 14 Dec 2023 04:00:34 +0000 Subject: [PATCH 5/9] style(pre-commit): autofix --- system/remapper/CMakeLists.txt | 2 +- system/remapper/README.md | 71 ++++---- .../include/remapper/remapper_core.hpp | 93 +++++----- system/remapper/launch/remapper.launch.xml | 14 +- system/remapper/package.xml | 12 +- system/remapper/src/remapper_core.cpp | 159 ++++++++++++------ 6 files changed, 208 insertions(+), 143 deletions(-) diff --git a/system/remapper/CMakeLists.txt b/system/remapper/CMakeLists.txt index 1b370d10fda3c..cbaf563f9d641 100644 --- a/system/remapper/CMakeLists.txt +++ b/system/remapper/CMakeLists.txt @@ -12,4 +12,4 @@ ament_auto_add_executable(remapper ament_auto_package( INSTALL_TO_SHARE launch -) \ No newline at end of file +) diff --git a/system/remapper/README.md b/system/remapper/README.md index 93ba33175b4a7..8f8db7e34a325 100644 --- a/system/remapper/README.md +++ b/system/remapper/README.md @@ -8,43 +8,44 @@ Remapper is a node to remap some topics to topics added hostname prefix for mult ### 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` | +| 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` | +| 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 \ No newline at end of file + +none diff --git a/system/remapper/include/remapper/remapper_core.hpp b/system/remapper/include/remapper/remapper_core.hpp index 74d6a1a2e5308..0e04c39b8da43 100644 --- a/system/remapper/include/remapper/remapper_core.hpp +++ b/system/remapper/include/remapper/remapper_core.hpp @@ -15,26 +15,22 @@ #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 #include #include +#include +#include +#include #include -#include #include - - +#include class Remapper : public rclcpp::Node { @@ -43,61 +39,82 @@ class Remapper : public rclcpp::Node private: // Subscriber - rclcpp::Subscription::SharedPtr sub_operation_mode_availability_; + rclcpp::Subscription::SharedPtr + sub_operation_mode_availability_; rclcpp::Subscription::SharedPtr sub_to_can_bus_; - rclcpp::Subscription::SharedPtr sub_control_cmd_; + 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_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_controle_mode_report_; + rclcpp::Subscription::SharedPtr + sub_autoware_state_; + rclcpp::Subscription::SharedPtr + sub_controle_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_; + rclcpp::Subscription::SharedPtr + sub_diagnostics_graph_supervisor_; // Publisher - rclcpp::Publisher::SharedPtr pub_operation_mode_availability_; + rclcpp::Publisher::SharedPtr + pub_operation_mode_availability_; rclcpp::Publisher::SharedPtr pub_to_can_bus_; - rclcpp::Publisher::SharedPtr pub_control_cmd_; + 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_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_controle_mode_report_; + rclcpp::Publisher::SharedPtr + pub_controle_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_; + rclcpp::Publisher::SharedPtr + pub_diagnostics_graph_supervisor_; - void onOperationModeAvailability(const tier4_system_msgs::msg::OperationModeAvailability::ConstSharedPtr msg); + 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 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 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 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); - + void onDiagnosticGraphSupervisor( + const tier4_system_msgs::msg::DiagnosticGraph::ConstSharedPtr msg); }; -#endif // REMAPPER__REMAPPER_CORE_HPP_ \ No newline at end of file +#endif // REMAPPER__REMAPPER_CORE_HPP_ diff --git a/system/remapper/launch/remapper.launch.xml b/system/remapper/launch/remapper.launch.xml index fd7d125378531..56095b0cceb29 100644 --- a/system/remapper/launch/remapper.launch.xml +++ b/system/remapper/launch/remapper.launch.xml @@ -21,14 +21,14 @@ - - + + - - + + @@ -45,8 +45,8 @@ - - + + @@ -65,4 +65,4 @@ - \ No newline at end of file + diff --git a/system/remapper/package.xml b/system/remapper/package.xml index eeda94a0534fe..1ef2dd56cb6c5 100644 --- a/system/remapper/package.xml +++ b/system/remapper/package.xml @@ -10,16 +10,16 @@ ament_cmake_auto autoware_cmake - rclcpp - can_msgs - autoware_auto_control_msgs - tier4_system_msgs - tf2_msgs autoware_adapi_v1_msgs - geometry_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 diff --git a/system/remapper/src/remapper_core.cpp b/system/remapper/src/remapper_core.cpp index 1bbed085898f3..8bb0219c570ab 100644 --- a/system/remapper/src/remapper_core.cpp +++ b/system/remapper/src/remapper_core.cpp @@ -14,119 +14,166 @@ #include "remapper/remapper_core.hpp" -Remapper::Remapper() -: Node("remapper") +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)); + // 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)); + "~/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)); + "~/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)); + "~/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)); + "~/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)); + "~/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)); + "~/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)); + "~/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_controle_mode_report_ = create_subscription( - "~/input/vehicle/status/control_mode", rclcpp::QoS{1}, std::bind(&Remapper::onControlModeReport, this, _1)); + "~/input/autoware/state", rclcpp::QoS{1}, std::bind(&Remapper::onAutowareState, this, _1)); + sub_controle_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)); + "~/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)); + "~/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)); + "~/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}); + "~/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_controle_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_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_controle_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) { +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) { +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) { +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) { +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) { +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) { +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) { +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) { +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) { +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) { +void Remapper::onControlModeReport( + const autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg) +{ pub_controle_mode_report_->publish(*msg); } -void Remapper::onEmergency(const tier4_external_api_msgs::msg::Emergency::ConstSharedPtr 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) { +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) { +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) { +void Remapper::onDiagnosticGraphSupervisor( + const tier4_system_msgs::msg::DiagnosticGraph::ConstSharedPtr msg) +{ pub_diagnostics_graph_supervisor_->publish(*msg); } - - From 98a42423ad554c01100ddf67507dcf2d8c5bc21a Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Thu, 14 Dec 2023 16:34:00 +0900 Subject: [PATCH 6/9] feat: remove explicit --- system/remapper/include/remapper/remapper_core.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/system/remapper/include/remapper/remapper_core.hpp b/system/remapper/include/remapper/remapper_core.hpp index 0e04c39b8da43..77a93aa5ce824 100644 --- a/system/remapper/include/remapper/remapper_core.hpp +++ b/system/remapper/include/remapper/remapper_core.hpp @@ -35,7 +35,7 @@ class Remapper : public rclcpp::Node { public: - explicit Remapper(); + Remapper(); private: // Subscriber From 6321e1e904219039575f8865586760072f90f7fc Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Fri, 15 Dec 2023 18:03:07 +0900 Subject: [PATCH 7/9] feat: remove hostname arg tag --- system/remapper/launch/remapper.launch.xml | 5 ----- 1 file changed, 5 deletions(-) diff --git a/system/remapper/launch/remapper.launch.xml b/system/remapper/launch/remapper.launch.xml index 56095b0cceb29..7b25b0567e566 100644 --- a/system/remapper/launch/remapper.launch.xml +++ b/system/remapper/launch/remapper.launch.xml @@ -1,6 +1,4 @@ - - @@ -23,9 +21,6 @@ - - - From e81c9a2a91ea13bf6e5dac6098f9d50c6eb56f49 Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Sat, 16 Dec 2023 15:09:48 +0900 Subject: [PATCH 8/9] fix: respond to spell checker --- system/remapper/include/remapper/remapper_core.hpp | 4 ++-- system/remapper/launch/remapper.launch.xml | 6 +++--- system/remapper/src/remapper_core.cpp | 6 +++--- 3 files changed, 8 insertions(+), 8 deletions(-) diff --git a/system/remapper/include/remapper/remapper_core.hpp b/system/remapper/include/remapper/remapper_core.hpp index 77a93aa5ce824..51240a398b5a7 100644 --- a/system/remapper/include/remapper/remapper_core.hpp +++ b/system/remapper/include/remapper/remapper_core.hpp @@ -58,7 +58,7 @@ class Remapper : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_autoware_state_; rclcpp::Subscription::SharedPtr - sub_controle_mode_report_; + sub_control_mode_report_; rclcpp::Subscription::SharedPtr sub_get_emergency_; rclcpp::Subscription::SharedPtr sub_mrm_state_; rclcpp::Subscription::SharedPtr sub_diagnostics_graph_; @@ -84,7 +84,7 @@ class Remapper : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_autoware_state_; rclcpp::Publisher::SharedPtr - pub_controle_mode_report_; + pub_control_mode_report_; rclcpp::Publisher::SharedPtr pub_get_emergency_; rclcpp::Publisher::SharedPtr pub_mrm_state_; rclcpp::Publisher::SharedPtr pub_diagnostics_graph_; diff --git a/system/remapper/launch/remapper.launch.xml b/system/remapper/launch/remapper.launch.xml index 7b25b0567e566..a50e013dad8e1 100644 --- a/system/remapper/launch/remapper.launch.xml +++ b/system/remapper/launch/remapper.launch.xml @@ -14,7 +14,7 @@ - + @@ -35,7 +35,7 @@ - + @@ -53,7 +53,7 @@ - + diff --git a/system/remapper/src/remapper_core.cpp b/system/remapper/src/remapper_core.cpp index 8bb0219c570ab..c877f99c2c847 100644 --- a/system/remapper/src/remapper_core.cpp +++ b/system/remapper/src/remapper_core.cpp @@ -46,7 +46,7 @@ Remapper::Remapper() : Node("remapper") "~/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_controle_mode_report_ = + sub_control_mode_report_ = create_subscription( "~/input/vehicle/status/control_mode", rclcpp::QoS{1}, std::bind(&Remapper::onControlModeReport, this, _1)); @@ -83,7 +83,7 @@ Remapper::Remapper() : Node("remapper") "~/output/api/routing/route", rclcpp::QoS{1}); pub_autoware_state_ = create_publisher( "~/output/autoware/state", rclcpp::QoS{1}); - pub_controle_mode_report_ = create_publisher( + 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}); @@ -154,7 +154,7 @@ void Remapper::onAutowareState( void Remapper::onControlModeReport( const autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg) { - pub_controle_mode_report_->publish(*msg); + pub_control_mode_report_->publish(*msg); } void Remapper::onEmergency(const tier4_external_api_msgs::msg::Emergency::ConstSharedPtr msg) From 8d3851f228aaa47b008bd140f8f4bf5937c57cc4 Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Sat, 16 Dec 2023 15:47:21 +0900 Subject: [PATCH 9/9] feat: add default ecu_name --- system/remapper/launch/remapper.launch.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/system/remapper/launch/remapper.launch.xml b/system/remapper/launch/remapper.launch.xml index a50e013dad8e1..9cba6478baa44 100644 --- a/system/remapper/launch/remapper.launch.xml +++ b/system/remapper/launch/remapper.launch.xml @@ -1,4 +1,5 @@ +