diff --git a/system/autoware_auto_msgs_adapter/CMakeLists.txt b/system/autoware_auto_msgs_adapter/CMakeLists.txt new file mode 100644 index 0000000000000..9b48112227cbe --- /dev/null +++ b/system/autoware_auto_msgs_adapter/CMakeLists.txt @@ -0,0 +1,31 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_auto_msgs_adapter) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +set(NODE_NAME ${PROJECT_NAME}_node) +set(EXEC_NAME ${PROJECT_NAME}_exe) +set(TEST_NAME test_${PROJECT_NAME}) + +ament_auto_add_library(${NODE_NAME} + src/include/adapter_base.hpp + src/include/adapter_base_interface.hpp + src/include/adapter_control.hpp + src/include/autoware_auto_msgs_adapter_core.hpp + src/autoware_auto_msgs_adapter_core.cpp) + +rclcpp_components_register_node(${NODE_NAME} + PLUGIN "autoware_auto_msgs_adapter::AutowareAutoMsgsAdapterNode" + EXECUTABLE ${EXEC_NAME}) + +if(BUILD_TESTING) + file(GLOB_RECURSE TEST_SOURCES test/*.cpp) + ament_add_ros_isolated_gtest(${TEST_NAME} ${TEST_SOURCES}) + target_include_directories(${TEST_NAME} PRIVATE src/include) + target_link_libraries(${TEST_NAME} ${NODE_NAME}) +endif() + +ament_auto_package(INSTALL_TO_SHARE + launch + config) diff --git a/system/autoware_auto_msgs_adapter/README.md b/system/autoware_auto_msgs_adapter/README.md new file mode 100644 index 0000000000000..85c2797074f1f --- /dev/null +++ b/system/autoware_auto_msgs_adapter/README.md @@ -0,0 +1,102 @@ +# autoware_auto_msgs_adapter + +This package is used to convert `autoware_msgs` to `autoware_auto_msgs`. + +## Purpose + +As we transition from `autoware_auto_msgs` to `autoware_msgs`, we wanted to provide flexibility and compatibility for +users who are still using `autoware_auto_msgs`. + +This adapter package allows users to easily convert messages between the two formats. + +## Capabilities + +The `autoware_auto_msgs_adapter` package provides the following capabilities: + +- Conversion of supported `autoware_msgs` messages to `autoware_auto_msgs` messages. +- Can be extended to support conversion for any message type pairs. +- Each instance is designed to convert from a single source message type to a single target message type. +- Multiple instances can be launched to convert multiple message types. +- Can be launched as a standalone node or as a component. + +## Usage + +Customize the adapter configuration by replicating and editing the `adapter_control.param.yaml` file located +in the `autoware_auto_msgs_adapter/config` directory. Example configuration: + +```yaml +/**: + ros__parameters: + msg_type_target: "autoware_auto_control_msgs/msg/AckermannControlCommand" + topic_name_source: "/control/command/control_cmd" + topic_name_target: "/control/command/control_cmd_auto" +``` + +Set the `msg_type_target` parameter to the desired target message type from `autoware_auto_msgs`. + +Make sure that the `msg_type_target` has the correspondence in either: + +- [schema/autoware_auto_msgs_adapter.schema.json](schema/autoware_auto_msgs_adapter.schema.json) +- OR [src/autoware_auto_msgs_adapter_core.cpp](src/autoware_auto_msgs_adapter_core.cpp) `AutowareAutoMsgsAdapterNode::create_adapter_map()` method. + +(If this package is maintained correctly, they should match each other.) + +Launch the adapter node by any of the following methods: + +### `ros2 launch` + +```bash +ros2 launch autoware_auto_msgs_adapter autoware_auto_msgs_adapter.launch.xml param_path:='full_path_to_param_file' +``` + +Make sure to set the `param_path` argument to the full path of the parameter file. + +Alternatively, + +- You can replicate and edit the launch file to suit to your needs. +- You can make use of the existing launch file in another launch file by providing the parameter file path as an + argument. + +### `ros2 run` + +```bash +ros2 run autoware_auto_msgs_adapter autoware_auto_msgs_adapter_exe --ros-args --params-file 'full_path_to_param_file' +``` + +Make sure to set the `param_path` argument to the full path of the parameter file. + +## Contributing + +### Current implementation details + +The entry point for the adapter executable is created with `RCLCPP_COMPONENTS_REGISTER_NODE` the [autoware_auto_msgs_adapter_core.cpp](src/Fautoware_auto_msgs_adapter_core.cpp). + +This allows it to be launched as a component or as a standalone node. + +In the `AutowareAutoMsgsAdapterNode` constructor, the adapter is selected by the type string provided in the +configuration file. The adapter is then initialized with the topic names provided. + +The constructors of the adapters are responsible for creating the publisher and subscriber (which makes use of the conversion method). + +### Adding a new message pair + +To add a new message pair, + +- Replicate and edit: + - [adapter_control.hpp](include/autoware_auto_msgs_adapter/adapter_control.hpp). + - Add the new header file to the [CMakeLists.txt](CMakeLists.txt). +- Add a new entry to the returned map instance in the `AutowareAutoMsgsAdapterNode::create_adapter_map()` method of the adapter node: + - [autoware_auto_msgs_adapter_core.cpp](src/autoware_auto_msgs_adapter_core.cpp) +- Add a new entry to the [schema/autoware_auto_msgs_adapter.schema.json](schema/autoware_auto_msgs_adapter.schema.json) file in the `definitions:autoware_auto_msgs_adapter:properties:msg_type_target:enum` section. + - Learn more about JSON schema usage in [here](https://autowarefoundation.github.io/autoware-documentation/main/contributing/coding-guidelines/ros-nodes/parameters/#json-schema). +- Create a new config file by replicating and editing: + - [adapter_control.param.yaml](config/adapter_control.param.yaml) +- Add a new test file by replicating and editing: + - [test_msg_ackermann_control_command.cpp](test/test_msg_ackermann_control_command.cpp) + - No need to edit the `CMakeLists.txt` file as it will automatically detect the new test file. + +Also make sure to test the new adapter with: + +```bash +colcon test --event-handlers console_cohesion+ --packages-select autoware_auto_msgs_adapter +``` diff --git a/system/autoware_auto_msgs_adapter/config/adapter_control.param.yaml b/system/autoware_auto_msgs_adapter/config/adapter_control.param.yaml new file mode 100644 index 0000000000000..4c6d5f101f380 --- /dev/null +++ b/system/autoware_auto_msgs_adapter/config/adapter_control.param.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + msg_type_target: "autoware_auto_control_msgs/msg/AckermannControlCommand" + topic_name_source: "/control/command/control_cmd" + topic_name_target: "/control/command/control_cmd_auto" diff --git a/system/autoware_auto_msgs_adapter/launch/autoware_auto_msgs_adapter.launch.xml b/system/autoware_auto_msgs_adapter/launch/autoware_auto_msgs_adapter.launch.xml new file mode 100755 index 0000000000000..89b58e2d60c28 --- /dev/null +++ b/system/autoware_auto_msgs_adapter/launch/autoware_auto_msgs_adapter.launch.xml @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/system/autoware_auto_msgs_adapter/package.xml b/system/autoware_auto_msgs_adapter/package.xml new file mode 100644 index 0000000000000..54a8136f5f077 --- /dev/null +++ b/system/autoware_auto_msgs_adapter/package.xml @@ -0,0 +1,27 @@ + + + + autoware_auto_msgs_adapter + 1.0.0 + Converts an autoware_msgs message to autoware_auto_msgs version and publishes it. + M. Fatih Cırıt + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + rosidl_default_generators + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + autoware_auto_control_msgs + autoware_control_msgs + rclcpp + rclcpp_components + + + ament_cmake + + diff --git a/system/autoware_auto_msgs_adapter/schema/autoware_auto_msgs_adapter.schema.json b/system/autoware_auto_msgs_adapter/schema/autoware_auto_msgs_adapter.schema.json new file mode 100644 index 0000000000000..cf853ee1da130 --- /dev/null +++ b/system/autoware_auto_msgs_adapter/schema/autoware_auto_msgs_adapter.schema.json @@ -0,0 +1,41 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for autoware_auto_msg_adapter", + "type": "object", + "definitions": { + "autoware_auto_msgs_adapter": { + "type": "object", + "properties": { + "msg_type_target": { + "type": "string", + "description": "Target message type", + "enum": ["autoware_auto_control_msgs/msg/AckermannControlCommand"], + "default": "autoware_auto_control_msgs/msg/AckermannControlCommand" + }, + "topic_name_source": { + "type": "string", + "description": "Topic name of the message to be converted.", + "default": "/control/command/control_cmd" + }, + "topic_name_target": { + "type": "string", + "description": "Target topic name which the message will be converted into.", + "default": "/control/command/control_cmd_auto" + } + }, + "required": ["msg_type_target", "topic_name_source", "topic_name_target"] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/autoware_auto_msgs_adapter" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/system/autoware_auto_msgs_adapter/src/autoware_auto_msgs_adapter_core.cpp b/system/autoware_auto_msgs_adapter/src/autoware_auto_msgs_adapter_core.cpp new file mode 100644 index 0000000000000..98fe916903a60 --- /dev/null +++ b/system/autoware_auto_msgs_adapter/src/autoware_auto_msgs_adapter_core.cpp @@ -0,0 +1,84 @@ +// Copyright 2023 The Autoware Foundation +// +// 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 "include/autoware_auto_msgs_adapter_core.hpp" + +#include "include/adapter_control.hpp" + +#include + +#include + +namespace autoware_auto_msgs_adapter +{ + +using autoware_auto_control_msgs::msg::AckermannControlCommand; +using autoware_control_msgs::msg::Control; + +using MapStringAdapter = AutowareAutoMsgsAdapterNode::MapStringAdapter; + +AutowareAutoMsgsAdapterNode::AutowareAutoMsgsAdapterNode(const rclcpp::NodeOptions & node_options) +: rclcpp::Node("autoware_auto_msgs_adapter", node_options) +{ + const std::string msg_type_target = declare_parameter("msg_type_target"); + const std::string topic_name_source = declare_parameter("topic_name_source"); + const std::string topic_name_target = declare_parameter("topic_name_target"); + + // Map of available adapters + auto map_adapter = create_adapter_map(topic_name_source, topic_name_target); + + print_adapter_options(map_adapter); + + // Initialize the adapter with the selected option + if (!initialize_adapter(map_adapter, msg_type_target)) { + RCLCPP_ERROR( + get_logger(), "Unknown msg type: %s. Please refer to previous log for available options.", + msg_type_target.c_str()); + } +} + +MapStringAdapter AutowareAutoMsgsAdapterNode::create_adapter_map( + const std::string & topic_name_source, const std::string & topic_name_target) +{ + return { + {"autoware_auto_control_msgs/msg/AckermannControlCommand", + [&] { + return std::static_pointer_cast( + std::make_shared(*this, topic_name_source, topic_name_target)); + }}, + }; +} + +void AutowareAutoMsgsAdapterNode::print_adapter_options(const MapStringAdapter & map_adapter) +{ + std::string std_options_available; + for (const auto & entry : map_adapter) { + std_options_available += entry.first + "\n"; + } + RCLCPP_INFO( + get_logger(), "Available msg_type_target options:\n%s", std_options_available.c_str()); +} + +bool AutowareAutoMsgsAdapterNode::initialize_adapter( + const MapStringAdapter & map_adapter, const std::string & msg_type_target) +{ + auto it = map_adapter.find(msg_type_target); + adapter_ = (it != map_adapter.end()) ? it->second() : nullptr; + return adapter_ != nullptr; +} + +} // namespace autoware_auto_msgs_adapter + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(autoware_auto_msgs_adapter::AutowareAutoMsgsAdapterNode) diff --git a/system/autoware_auto_msgs_adapter/src/include/adapter_base.hpp b/system/autoware_auto_msgs_adapter/src/include/adapter_base.hpp new file mode 100644 index 0000000000000..f506d66b1d8b0 --- /dev/null +++ b/system/autoware_auto_msgs_adapter/src/include/adapter_base.hpp @@ -0,0 +1,57 @@ +// Copyright 2023 The Autoware Foundation +// +// 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 ADAPTER_BASE_HPP_ +#define ADAPTER_BASE_HPP_ + +#include "adapter_base_interface.hpp" + +#include + +#include +#include + +namespace autoware_auto_msgs_adapter +{ + +template +class AdapterBase : public AdapterBaseInterface +{ +public: + RCLCPP_SHARED_PTR_DEFINITIONS(AdapterBase) + + AdapterBase( + rclcpp::Node & node, const std::string & topic_name_source, + const std::string & topic_name_target, const rclcpp::QoS & qos = rclcpp::QoS{1}) + { + pub_target_ = node.create_publisher(topic_name_target, qos); + sub_source_ = node.create_subscription( + topic_name_source, qos, std::bind(&AdapterBase::callback, this, std::placeholders::_1)); + } + +protected: + virtual TargetT convert(const SourceT & msg_source) = 0; + +private: + typename rclcpp::Publisher::SharedPtr pub_target_; + typename rclcpp::Subscription::SharedPtr sub_source_; + + void callback(const typename SourceT::SharedPtr msg_source) + { + pub_target_->publish(convert(*msg_source)); + } +}; + +} // namespace autoware_auto_msgs_adapter + +#endif // ADAPTER_BASE_HPP_ diff --git a/system/autoware_auto_msgs_adapter/src/include/adapter_base_interface.hpp b/system/autoware_auto_msgs_adapter/src/include/adapter_base_interface.hpp new file mode 100644 index 0000000000000..606993332fc4a --- /dev/null +++ b/system/autoware_auto_msgs_adapter/src/include/adapter_base_interface.hpp @@ -0,0 +1,34 @@ +// Copyright 2023 The Autoware Foundation +// +// 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 ADAPTER_BASE_INTERFACE_HPP_ +#define ADAPTER_BASE_INTERFACE_HPP_ + +#include + +#include + +namespace autoware_auto_msgs_adapter +{ + +class AdapterBaseInterface +{ +public: + RCLCPP_SHARED_PTR_DEFINITIONS(AdapterBaseInterface) + + virtual ~AdapterBaseInterface() = default; +}; + +} // namespace autoware_auto_msgs_adapter + +#endif // ADAPTER_BASE_INTERFACE_HPP_ diff --git a/system/autoware_auto_msgs_adapter/src/include/adapter_control.hpp b/system/autoware_auto_msgs_adapter/src/include/adapter_control.hpp new file mode 100644 index 0000000000000..c02f4e03877a2 --- /dev/null +++ b/system/autoware_auto_msgs_adapter/src/include/adapter_control.hpp @@ -0,0 +1,69 @@ +// Copyright 2023 The Autoware Foundation +// +// 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 ADAPTER_CONTROL_HPP_ +#define ADAPTER_CONTROL_HPP_ + +#include "adapter_base.hpp" + +#include + +#include +#include + +#include + +namespace autoware_auto_msgs_adapter +{ +using autoware_auto_control_msgs::msg::AckermannControlCommand; +using autoware_control_msgs::msg::Control; + +class AdapterControl +: public autoware_auto_msgs_adapter::AdapterBase +{ +public: + AdapterControl( + rclcpp::Node & node, const std::string & topic_name_source, + const std::string & topic_name_target, const rclcpp::QoS & qos = rclcpp::QoS{1}) + : AdapterBase(node, topic_name_source, topic_name_target, qos) + { + RCLCPP_DEBUG( + node.get_logger(), "AdapterControl is initialized to convert: %s -> %s", + topic_name_source.c_str(), topic_name_target.c_str()); + } + +protected: + AckermannControlCommand convert(const Control & msg_source) override + { + autoware_auto_control_msgs::msg::AckermannControlCommand msg_auto; + msg_auto.stamp = msg_source.stamp; + + const auto & lateral = msg_source.lateral; + auto & lateral_auto = msg_auto.lateral; + lateral_auto.stamp = lateral.stamp; + lateral_auto.steering_tire_angle = lateral.steering_tire_angle; + lateral_auto.steering_tire_rotation_rate = lateral.steering_tire_rotation_rate; + + const auto & longitudinal = msg_source.longitudinal; + auto & longitudinal_auto = msg_auto.longitudinal; + longitudinal_auto.stamp = longitudinal.stamp; + longitudinal_auto.acceleration = longitudinal.acceleration; + longitudinal_auto.jerk = longitudinal.jerk; + longitudinal_auto.speed = longitudinal.velocity; + + return msg_auto; + } +}; +} // namespace autoware_auto_msgs_adapter + +#endif // ADAPTER_CONTROL_HPP_ diff --git a/system/autoware_auto_msgs_adapter/src/include/autoware_auto_msgs_adapter_core.hpp b/system/autoware_auto_msgs_adapter/src/include/autoware_auto_msgs_adapter_core.hpp new file mode 100644 index 0000000000000..a33a88c68618f --- /dev/null +++ b/system/autoware_auto_msgs_adapter/src/include/autoware_auto_msgs_adapter_core.hpp @@ -0,0 +1,46 @@ +// Copyright 2023 The Autoware Foundation +// +// 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 AUTOWARE_AUTO_MSGS_ADAPTER_CORE_HPP_ +#define AUTOWARE_AUTO_MSGS_ADAPTER_CORE_HPP_ + +#include "adapter_control.hpp" + +#include + +#include +#include + +namespace autoware_auto_msgs_adapter +{ + +class AutowareAutoMsgsAdapterNode : public rclcpp::Node +{ +public: + explicit AutowareAutoMsgsAdapterNode(const rclcpp::NodeOptions & node_options); + using MapStringAdapter = std::map>; + +private: + AdapterBaseInterface::SharedPtr adapter_; + + MapStringAdapter create_adapter_map( + const std::string & topic_name_source, const std::string & topic_name_target); + + void print_adapter_options(const MapStringAdapter & map_adapter); + + bool initialize_adapter( + const MapStringAdapter & map_adapter, const std::string & msg_type_target); +}; +} // namespace autoware_auto_msgs_adapter + +#endif // AUTOWARE_AUTO_MSGS_ADAPTER_CORE_HPP_ diff --git a/system/autoware_auto_msgs_adapter/test/test_autoware_auto_msgs_adapter.cpp b/system/autoware_auto_msgs_adapter/test/test_autoware_auto_msgs_adapter.cpp new file mode 100644 index 0000000000000..6b19ae6e30555 --- /dev/null +++ b/system/autoware_auto_msgs_adapter/test/test_autoware_auto_msgs_adapter.cpp @@ -0,0 +1,26 @@ +// Copyright 2023 The Autoware Foundation +// +// 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 + +#include + +int main(int argc, char * argv[]) +{ + testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + bool result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/system/autoware_auto_msgs_adapter/test/test_msg_ackermann_control_command.cpp b/system/autoware_auto_msgs_adapter/test/test_msg_ackermann_control_command.cpp new file mode 100644 index 0000000000000..e33ae07e6aca1 --- /dev/null +++ b/system/autoware_auto_msgs_adapter/test/test_msg_ackermann_control_command.cpp @@ -0,0 +1,116 @@ +// Copyright 2023 The Autoware Foundation +// +// 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 + +#include + +#include + +autoware_control_msgs::msg::Control generate_control_msg() +{ + // generate deterministic random float + std::mt19937 gen(0); + std::uniform_real_distribution<> dis(-100.0, 100.0); + auto rand_float = [&dis, &gen]() { return static_cast(dis(gen)); }; + + // generate deterministic random int + std::uniform_int_distribution<> dis_int(0, 1000000); + auto rand_int = [&dis_int, &gen]() { return dis_int(gen); }; + + autoware_control_msgs::msg::Control msg_control; + msg_control.stamp = rclcpp::Time(rand_int()); + + msg_control.lateral.stamp = rclcpp::Time(rand_int()); + msg_control.lateral.steering_tire_angle = rand_float(); + msg_control.lateral.steering_tire_rotation_rate = rand_float(); + + msg_control.longitudinal.stamp = rclcpp::Time(rand_int()); + msg_control.longitudinal.velocity = rand_float(); + msg_control.longitudinal.jerk = rand_float(); + msg_control.longitudinal.acceleration = rand_float(); + return msg_control; +} + +TEST(AutowareAutoMsgsAdapter, TestMsgAckermannControlCommand) // NOLINT for gtest +{ + const std::string msg_type_target = "autoware_auto_control_msgs/msg/AckermannControlCommand"; + const std::string topic_name_source = "topic_name_source"; + const std::string topic_name_target = "topic_name_target"; + + std::cout << "Creating the adapter node..." << std::endl; + + rclcpp::NodeOptions node_options; + node_options.append_parameter_override("msg_type_target", msg_type_target); + node_options.append_parameter_override("topic_name_source", topic_name_source); + node_options.append_parameter_override("topic_name_target", topic_name_target); + + using autoware_auto_msgs_adapter::AutowareAutoMsgsAdapterNode; + AutowareAutoMsgsAdapterNode::SharedPtr node_adapter; + node_adapter = std::make_shared(node_options); + + std::cout << "Creating the subscriber node..." << std::endl; + + auto node_subscriber = std::make_shared("node_subscriber", rclcpp::NodeOptions{}); + + bool test_completed = false; + + const auto msg_control = generate_control_msg(); + auto sub = + node_subscriber->create_subscription( + topic_name_target, rclcpp::QoS{1}, + [&msg_control, &test_completed]( + const autoware_auto_control_msgs::msg::AckermannControlCommand::SharedPtr msg) { + EXPECT_EQ(msg->stamp, msg_control.stamp); + + EXPECT_EQ(msg->lateral.stamp, msg_control.lateral.stamp); + EXPECT_FLOAT_EQ(msg->lateral.steering_tire_angle, msg_control.lateral.steering_tire_angle); + EXPECT_FLOAT_EQ( + msg->lateral.steering_tire_rotation_rate, + msg_control.lateral.steering_tire_rotation_rate); + + EXPECT_EQ(msg->longitudinal.stamp, msg_control.longitudinal.stamp); + EXPECT_FLOAT_EQ(msg->longitudinal.speed, msg_control.longitudinal.velocity); + EXPECT_FLOAT_EQ(msg->longitudinal.acceleration, msg_control.longitudinal.acceleration); + EXPECT_FLOAT_EQ(msg->longitudinal.jerk, msg_control.longitudinal.jerk); + test_completed = true; + }); + + std::cout << "Creating the publisher node..." << std::endl; + + auto node_publisher = std::make_shared("node_publisher", rclcpp::NodeOptions{}); + auto pub = node_publisher->create_publisher( + topic_name_source, rclcpp::QoS{1}); + pub->publish(msg_control); + + auto start_time = std::chrono::system_clock::now(); + auto max_test_dur = std::chrono::seconds(5); + auto timed_out = false; + + while (rclcpp::ok() && !test_completed) { + rclcpp::spin_some(node_subscriber); + rclcpp::spin_some(node_adapter); + rclcpp::spin_some(node_publisher); + rclcpp::sleep_for(std::chrono::milliseconds(50)); + if (std::chrono::system_clock::now() - start_time > max_test_dur) { + timed_out = true; + break; + } + } + + EXPECT_TRUE(test_completed); + EXPECT_FALSE(timed_out); + + rclcpp::shutdown(); +}