diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 5c4ad77e756fa..1ef667376b616 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -218,7 +218,6 @@ simulator/fault_injection/** keisuke.shima@tier4.jp simulator/learning_based_vehicle_model/** maxime.clement@tier4.jp nagy.tomas@tier4.jp simulator/simple_planning_simulator/** mamoru.sobue@tier4.jp maxime.clement@tier4.jp takamasa.horibe@tier4.jp temkei.kem@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp simulator/vehicle_door_simulator/** isamu.takagi@tier4.jp -system/autoware_auto_msgs_adapter/** isamu.takagi@tier4.jp mfc@leodrive.ai system/bluetooth_monitor/** fumihito.ito@tier4.jp system/component_state_monitor/** isamu.takagi@tier4.jp system/default_ad_api/** isamu.takagi@tier4.jp ryohsuke.mitsudome@tier4.jp yukihiro.saito@tier4.jp diff --git a/system/autoware_auto_msgs_adapter/CMakeLists.txt b/system/autoware_auto_msgs_adapter/CMakeLists.txt deleted file mode 100644 index a8b72b8da688c..0000000000000 --- a/system/autoware_auto_msgs_adapter/CMakeLists.txt +++ /dev/null @@ -1,34 +0,0 @@ -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/adapter_map.hpp - src/include/adapter_perception.hpp - src/include/adapter_planning.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 deleted file mode 100644 index 67fabaaa46c9a..0000000000000 --- a/system/autoware_auto_msgs_adapter/README.md +++ /dev/null @@ -1,102 +0,0 @@ -# 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 `autoware_auto_msgs_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: - - [autoware_auto_msgs_adapter_control.param.yaml](config/autoware_auto_msgs_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/autoware_auto_msgs_adapter_control.param.yaml b/system/autoware_auto_msgs_adapter/config/autoware_auto_msgs_adapter_control.param.yaml deleted file mode 100644 index 4c6d5f101f380..0000000000000 --- a/system/autoware_auto_msgs_adapter/config/autoware_auto_msgs_adapter_control.param.yaml +++ /dev/null @@ -1,5 +0,0 @@ -/**: - 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/config/autoware_auto_msgs_adapter_map.param.yaml b/system/autoware_auto_msgs_adapter/config/autoware_auto_msgs_adapter_map.param.yaml deleted file mode 100644 index dcaa49e089b44..0000000000000 --- a/system/autoware_auto_msgs_adapter/config/autoware_auto_msgs_adapter_map.param.yaml +++ /dev/null @@ -1,5 +0,0 @@ -/**: - ros__parameters: - msg_type_target: "autoware_auto_mapping_msgs/msg/HADMapBin" - topic_name_source: "/map/vector_map" - topic_name_target: "/map/vector_map_auto" diff --git a/system/autoware_auto_msgs_adapter/config/autoware_auto_msgs_adapter_perception.param.yaml b/system/autoware_auto_msgs_adapter/config/autoware_auto_msgs_adapter_perception.param.yaml deleted file mode 100644 index d8b6e31543fad..0000000000000 --- a/system/autoware_auto_msgs_adapter/config/autoware_auto_msgs_adapter_perception.param.yaml +++ /dev/null @@ -1,5 +0,0 @@ -/**: - ros__parameters: - msg_type_target: "autoware_auto_perception_msgs/msg/PredictedObjects" - topic_name_source: "/perception/object_recognition/objects" - topic_name_target: "/perception/object_recognition/objects_auto" diff --git a/system/autoware_auto_msgs_adapter/config/autoware_auto_msgs_adapter_planning.param.yaml b/system/autoware_auto_msgs_adapter/config/autoware_auto_msgs_adapter_planning.param.yaml deleted file mode 100644 index b4a8712d4c505..0000000000000 --- a/system/autoware_auto_msgs_adapter/config/autoware_auto_msgs_adapter_planning.param.yaml +++ /dev/null @@ -1,5 +0,0 @@ -/**: - ros__parameters: - msg_type_target: "autoware_auto_planning_msgs/msg/Trajectory" - topic_name_source: "/planning/scenario_planning/trajectory" - topic_name_target: "/planning/scenario_planning/trajectory_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 deleted file mode 100755 index 58f2fb4e06238..0000000000000 --- a/system/autoware_auto_msgs_adapter/launch/autoware_auto_msgs_adapter.launch.xml +++ /dev/null @@ -1,20 +0,0 @@ - - - - - - - - - - - - - - - - - - - - diff --git a/system/autoware_auto_msgs_adapter/package.xml b/system/autoware_auto_msgs_adapter/package.xml deleted file mode 100644 index 2941820550ba0..0000000000000 --- a/system/autoware_auto_msgs_adapter/package.xml +++ /dev/null @@ -1,36 +0,0 @@ - - - - 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 - Takagi, Isamu - - 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_auto_mapping_msgs - autoware_auto_perception_msgs - autoware_auto_planning_msgs - autoware_control_msgs - autoware_map_msgs - autoware_perception_msgs - autoware_planning_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 deleted file mode 100644 index a5d6c93029d09..0000000000000 --- a/system/autoware_auto_msgs_adapter/schema/autoware_auto_msgs_adapter.schema.json +++ /dev/null @@ -1,46 +0,0 @@ -{ - "$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", - "autoware_auto_mapping_msgs/msg/HADMapBin", - "autoware_auto_perception_msgs/msg/PredictedObjects", - "autoware_auto_planning_msgs/msg/Trajectory" - ], - "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 deleted file mode 100644 index 6a23970246866..0000000000000 --- a/system/autoware_auto_msgs_adapter/src/autoware_auto_msgs_adapter_core.cpp +++ /dev/null @@ -1,99 +0,0 @@ -// 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)); - }}, - {"autoware_auto_mapping_msgs/msg/HADMapBin", - [&] { - return std::static_pointer_cast( - std::make_shared(*this, topic_name_source, topic_name_target)); - }}, - {"autoware_auto_perception_msgs/msg/PredictedObjects", - [&] { - return std::static_pointer_cast( - std::make_shared(*this, topic_name_source, topic_name_target)); - }}, - {"autoware_auto_planning_msgs/msg/Trajectory", - [&] { - 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 deleted file mode 100644 index f506d66b1d8b0..0000000000000 --- a/system/autoware_auto_msgs_adapter/src/include/adapter_base.hpp +++ /dev/null @@ -1,57 +0,0 @@ -// 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 deleted file mode 100644 index 606993332fc4a..0000000000000 --- a/system/autoware_auto_msgs_adapter/src/include/adapter_base_interface.hpp +++ /dev/null @@ -1,34 +0,0 @@ -// 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 deleted file mode 100644 index c02f4e03877a2..0000000000000 --- a/system/autoware_auto_msgs_adapter/src/include/adapter_control.hpp +++ /dev/null @@ -1,69 +0,0 @@ -// 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/adapter_map.hpp b/system/autoware_auto_msgs_adapter/src/include/adapter_map.hpp deleted file mode 100644 index 8150b7d7dba08..0000000000000 --- a/system/autoware_auto_msgs_adapter/src/include/adapter_map.hpp +++ /dev/null @@ -1,59 +0,0 @@ -// 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_MAP_HPP_ -#define ADAPTER_MAP_HPP_ - -#include "adapter_base.hpp" - -#include - -#include -#include - -#include - -namespace autoware_auto_msgs_adapter -{ -using autoware_auto_mapping_msgs::msg::HADMapBin; -using autoware_map_msgs::msg::LaneletMapBin; - -class AdapterMap : public autoware_auto_msgs_adapter::AdapterBase -{ -public: - AdapterMap( - 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(), "AdapterMap is initialized to convert: %s -> %s", - topic_name_source.c_str(), topic_name_target.c_str()); - } - -protected: - HADMapBin convert(const LaneletMapBin & msg_source) override - { - autoware_auto_mapping_msgs::msg::HADMapBin msg_auto; - msg_auto.header = msg_source.header; - msg_auto.format_version = msg_source.version_map_format; - msg_auto.map_version = msg_source.version_map; - msg_auto.map_format = 0; - msg_auto.data = msg_source.data; - - return msg_auto; - } -}; -} // namespace autoware_auto_msgs_adapter - -#endif // ADAPTER_MAP_HPP_ diff --git a/system/autoware_auto_msgs_adapter/src/include/adapter_perception.hpp b/system/autoware_auto_msgs_adapter/src/include/adapter_perception.hpp deleted file mode 100644 index 3821bbad8ce38..0000000000000 --- a/system/autoware_auto_msgs_adapter/src/include/adapter_perception.hpp +++ /dev/null @@ -1,97 +0,0 @@ -// 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_PERCEPTION_HPP_ -#define ADAPTER_PERCEPTION_HPP_ - -#include "adapter_base.hpp" - -#include - -#include -#include - -#include - -namespace autoware_auto_msgs_adapter -{ -using Objects_auto = autoware_auto_perception_msgs::msg::PredictedObjects; -using Objects = autoware_perception_msgs::msg::PredictedObjects; - -class AdapterPerception : public autoware_auto_msgs_adapter::AdapterBase -{ -public: - AdapterPerception( - 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(), "AdapterPerception is initialized to convert: %s -> %s", - topic_name_source.c_str(), topic_name_target.c_str()); - } - -protected: - Objects_auto convert(const Objects & msg_source) override - { - Objects_auto msg_auto; - msg_auto.header = msg_source.header; - - autoware_auto_perception_msgs::msg::PredictedObject object_auto; - for (size_t it_of_objects = 0; it_of_objects < msg_source.objects.size(); it_of_objects++) { - // convert id and probability - object_auto.object_id = msg_source.objects[it_of_objects].object_id; - object_auto.existence_probability = msg_source.objects[it_of_objects].existence_probability; - // convert classification - autoware_auto_perception_msgs::msg::ObjectClassification classification; - for (size_t i = 0; i < msg_source.objects[it_of_objects].classification.size(); i++) { - classification.label = msg_source.objects[it_of_objects].classification[i].label; - classification.probability = - msg_source.objects[it_of_objects].classification[i].probability; - object_auto.classification.push_back(classification); - } - // convert kinematics - object_auto.kinematics.initial_pose_with_covariance = - msg_source.objects[it_of_objects].kinematics.initial_pose_with_covariance; - object_auto.kinematics.initial_twist_with_covariance = - msg_source.objects[it_of_objects].kinematics.initial_twist_with_covariance; - object_auto.kinematics.initial_acceleration_with_covariance = - msg_source.objects[it_of_objects].kinematics.initial_acceleration_with_covariance; - for (size_t j = 0; j < msg_source.objects[it_of_objects].kinematics.predicted_paths.size(); - j++) { - autoware_auto_perception_msgs::msg::PredictedPath predicted_path; - for (size_t k = 0; - k < msg_source.objects[it_of_objects].kinematics.predicted_paths[j].path.size(); k++) { - predicted_path.path.push_back( - msg_source.objects[it_of_objects].kinematics.predicted_paths[j].path[k]); - } - predicted_path.time_step = - msg_source.objects[it_of_objects].kinematics.predicted_paths[j].time_step; - predicted_path.confidence = - msg_source.objects[it_of_objects].kinematics.predicted_paths[j].confidence; - object_auto.kinematics.predicted_paths.push_back(predicted_path); - } - // convert shape - object_auto.shape.type = msg_source.objects[it_of_objects].shape.type; - object_auto.shape.footprint = msg_source.objects[it_of_objects].shape.footprint; - object_auto.shape.dimensions = msg_source.objects[it_of_objects].shape.dimensions; - - // add to objects list - msg_auto.objects.push_back(object_auto); - } - return msg_auto; - } -}; -} // namespace autoware_auto_msgs_adapter - -#endif // ADAPTER_PERCEPTION_HPP_ diff --git a/system/autoware_auto_msgs_adapter/src/include/adapter_planning.hpp b/system/autoware_auto_msgs_adapter/src/include/adapter_planning.hpp deleted file mode 100644 index 79d73bdda0575..0000000000000 --- a/system/autoware_auto_msgs_adapter/src/include/adapter_planning.hpp +++ /dev/null @@ -1,70 +0,0 @@ -// 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_PLANNING_HPP_ -#define ADAPTER_PLANNING_HPP_ - -#include "adapter_base.hpp" - -#include - -#include -#include - -#include - -namespace autoware_auto_msgs_adapter -{ -using TrajectoryAuto = autoware_auto_planning_msgs::msg::Trajectory; -using PointAuto = autoware_auto_planning_msgs::msg::TrajectoryPoint; -using Trajectory = autoware_planning_msgs::msg::Trajectory; - -class AdapterPlanning : public autoware_auto_msgs_adapter::AdapterBase -{ -public: - AdapterPlanning( - 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(), "AdapterPlanning is initialized to convert: %s -> %s", - topic_name_source.c_str(), topic_name_target.c_str()); - } - -protected: - TrajectoryAuto convert(const Trajectory & msg_source) override - { - TrajectoryAuto msg_auto; - msg_auto.header = msg_source.header; - PointAuto trajectory_point_auto; - msg_auto.points.reserve(msg_source.points.size()); - for (size_t i = 0; i < msg_source.points.size(); i++) { - trajectory_point_auto.time_from_start = msg_source.points.at(i).time_from_start; - trajectory_point_auto.pose = msg_source.points.at(i).pose; - trajectory_point_auto.longitudinal_velocity_mps = - msg_source.points.at(i).longitudinal_velocity_mps; - trajectory_point_auto.lateral_velocity_mps = msg_source.points.at(i).lateral_velocity_mps; - trajectory_point_auto.acceleration_mps2 = msg_source.points.at(i).acceleration_mps2; - trajectory_point_auto.heading_rate_rps = msg_source.points.at(i).heading_rate_rps; - trajectory_point_auto.front_wheel_angle_rad = msg_source.points.at(i).front_wheel_angle_rad; - trajectory_point_auto.rear_wheel_angle_rad = msg_source.points.at(i).rear_wheel_angle_rad; - msg_auto.points.push_back(trajectory_point_auto); - } - - return msg_auto; - } -}; -} // namespace autoware_auto_msgs_adapter - -#endif // ADAPTER_PLANNING_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 deleted file mode 100644 index 258bf01244499..0000000000000 --- a/system/autoware_auto_msgs_adapter/src/include/autoware_auto_msgs_adapter_core.hpp +++ /dev/null @@ -1,49 +0,0 @@ -// 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 "adapter_map.hpp" -#include "adapter_perception.hpp" -#include "adapter_planning.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 deleted file mode 100644 index 6b19ae6e30555..0000000000000 --- a/system/autoware_auto_msgs_adapter/test/test_autoware_auto_msgs_adapter.cpp +++ /dev/null @@ -1,26 +0,0 @@ -// 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 deleted file mode 100644 index 430b9a26b60e1..0000000000000 --- a/system/autoware_auto_msgs_adapter/test/test_msg_ackermann_control_command.cpp +++ /dev/null @@ -1,116 +0,0 @@ -// 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(); -} diff --git a/system/autoware_auto_msgs_adapter/test/test_msg_had_map_bin.cpp b/system/autoware_auto_msgs_adapter/test/test_msg_had_map_bin.cpp deleted file mode 100644 index b312bd144f6f3..0000000000000 --- a/system/autoware_auto_msgs_adapter/test/test_msg_had_map_bin.cpp +++ /dev/null @@ -1,104 +0,0 @@ -// 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_map_msgs::msg::LaneletMapBin generate_map_msg() -{ - // generate deterministic random int - std::mt19937 gen(0); - std::uniform_int_distribution<> dis_int(0, 1000000); - auto rand_int = [&dis_int, &gen]() { return dis_int(gen); }; - - autoware_map_msgs::msg::LaneletMapBin msg_map; - msg_map.header.stamp = rclcpp::Time(rand_int()); - msg_map.header.frame_id = "test_frame"; - - msg_map.version_map_format = "1.1.1"; - msg_map.version_map = "1.0.0"; - msg_map.name_map = "florence-prato-city-center"; - msg_map.data.push_back(rand_int()); - - return msg_map; -} - -TEST(AutowareAutoMsgsAdapter, TestHADMapBin) // NOLINT for gtest -{ - const std::string msg_type_target = "autoware_auto_mapping_msgs/msg/HADMapBin"; - 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_map = generate_map_msg(); - auto sub = node_subscriber->create_subscription( - topic_name_target, rclcpp::QoS{1}, - [&msg_map, &test_completed](const autoware_auto_mapping_msgs::msg::HADMapBin::SharedPtr msg) { - EXPECT_EQ(msg->header.stamp, msg_map.header.stamp); - EXPECT_EQ(msg->header.frame_id, msg_map.header.frame_id); - - EXPECT_EQ(msg->map_format, 0); - EXPECT_EQ(msg->format_version, msg_map.version_map_format); - EXPECT_EQ(msg->map_version, msg_map.version_map); - EXPECT_EQ(msg->data, msg_map.data); - - 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_map); - - 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(); -} diff --git a/system/autoware_auto_msgs_adapter/test/test_msg_planning_trajectory.cpp b/system/autoware_auto_msgs_adapter/test/test_msg_planning_trajectory.cpp deleted file mode 100644 index e2d44d6085c8a..0000000000000 --- a/system/autoware_auto_msgs_adapter/test/test_msg_planning_trajectory.cpp +++ /dev/null @@ -1,149 +0,0 @@ -// 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_planning_msgs::msg::Trajectory generate_planning_msg() -{ - using TrajectoryPoint = autoware_planning_msgs::msg::TrajectoryPoint; - // generate deterministic random int - std::mt19937 gen(0); - std::uniform_int_distribution<> dis_int(0, 1000000); - auto rand_int = [&dis_int, &gen]() { return dis_int(gen); }; - - autoware_planning_msgs::msg::Trajectory msg_planning; - msg_planning.header.stamp = rclcpp::Time(rand_int()); - msg_planning.header.frame_id = "test_frame"; - - TrajectoryPoint point; - geometry_msgs::msg::Pose pose; - pose.position.x = 0.0; - pose.position.y = 0.0; - pose.position.z = 0.0; - pose.orientation.x = 0.0; - pose.orientation.y = 0.0; - pose.orientation.z = 0.0; - pose.orientation.w = 1.0; - - for (size_t i = 0; i < 100; i++) { - point.longitudinal_velocity_mps = 1.0; - point.time_from_start = rclcpp::Duration::from_seconds(0.0); - point.pose = pose; - point.longitudinal_velocity_mps = 20.0; - point.lateral_velocity_mps = 0.0; - point.acceleration_mps2 = 1.0; - point.heading_rate_rps = 2.0; - point.front_wheel_angle_rad = 8.0; - point.rear_wheel_angle_rad = 10.0; - - msg_planning.points.push_back(point); - } - - return msg_planning; -} - -TEST(AutowareAutoMsgsAdapter, TestTrajectory) // NOLINT for gtest -{ - const std::string msg_type_target = "autoware_auto_planning_msgs/msg/Trajectory"; - 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_planning = generate_planning_msg(); - auto sub = node_subscriber->create_subscription( - topic_name_target, rclcpp::QoS{1}, - [&msg_planning, - &test_completed](const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg) { - EXPECT_EQ(msg->header.stamp, msg_planning.header.stamp); - EXPECT_EQ(msg->header.frame_id, msg_planning.header.frame_id); - for (size_t i = 0; i < msg_planning.points.size(); i++) { - EXPECT_FLOAT_EQ( - msg->points.at(i).pose.position.x, msg_planning.points.at(i).pose.position.x); - EXPECT_FLOAT_EQ( - msg->points.at(i).pose.position.y, msg_planning.points.at(i).pose.position.y); - EXPECT_FLOAT_EQ( - msg->points.at(i).pose.position.z, msg_planning.points.at(i).pose.position.z); - EXPECT_FLOAT_EQ( - msg->points.at(i).pose.orientation.x, msg_planning.points.at(i).pose.orientation.x); - EXPECT_FLOAT_EQ( - msg->points.at(i).pose.orientation.y, msg_planning.points.at(i).pose.orientation.y); - EXPECT_FLOAT_EQ( - msg->points.at(i).pose.orientation.z, msg_planning.points.at(i).pose.orientation.z); - EXPECT_FLOAT_EQ( - msg->points.at(i).pose.orientation.w, msg_planning.points.at(i).pose.orientation.w); - EXPECT_FLOAT_EQ( - msg->points.at(i).longitudinal_velocity_mps, - msg_planning.points.at(i).longitudinal_velocity_mps); - EXPECT_FLOAT_EQ( - msg->points.at(i).lateral_velocity_mps, msg_planning.points.at(i).lateral_velocity_mps); - EXPECT_FLOAT_EQ( - msg->points.at(i).acceleration_mps2, msg_planning.points.at(i).acceleration_mps2); - EXPECT_FLOAT_EQ( - msg->points.at(i).heading_rate_rps, msg_planning.points.at(i).heading_rate_rps); - EXPECT_FLOAT_EQ( - msg->points.at(i).front_wheel_angle_rad, msg_planning.points.at(i).front_wheel_angle_rad); - EXPECT_FLOAT_EQ( - msg->points.at(i).rear_wheel_angle_rad, msg_planning.points.at(i).rear_wheel_angle_rad); - } - - 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_planning); - - 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(); -} diff --git a/system/autoware_auto_msgs_adapter/test/test_msg_predicted_objects.cpp b/system/autoware_auto_msgs_adapter/test/test_msg_predicted_objects.cpp deleted file mode 100644 index a84985770059d..0000000000000 --- a/system/autoware_auto_msgs_adapter/test/test_msg_predicted_objects.cpp +++ /dev/null @@ -1,270 +0,0 @@ -// 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_perception_msgs::msg::PredictedObjects generate_perception_msg() -{ - // generate deterministic random int - std::mt19937 gen(0); - std::uniform_int_distribution<> dis_int(0, 1000000); - auto rand_int = [&dis_int, &gen]() { return dis_int(gen); }; - - autoware_perception_msgs::msg::PredictedObjects msg_perception; - msg_perception.header.stamp = rclcpp::Time(rand_int()); - msg_perception.header.frame_id = "test_frame"; - - autoware_perception_msgs::msg::PredictedObject obj; - // // { - unique_identifier_msgs::msg::UUID uuid_; - std::independent_bits_engine bit_eng(gen); - std::generate(uuid_.uuid.begin(), uuid_.uuid.end(), bit_eng); - obj.object_id = uuid_; - obj.existence_probability = 0.5; - autoware_perception_msgs::msg::ObjectClassification obj_class; - obj_class.label = 0; - obj_class.probability = 0.5; - obj.classification.push_back(obj_class); - - // { - autoware_perception_msgs::msg::PredictedObjectKinematics kin; - kin.initial_pose_with_covariance.pose.position.x = 10; - kin.initial_pose_with_covariance.pose.position.y = 10; - kin.initial_pose_with_covariance.pose.position.z = 0; - kin.initial_pose_with_covariance.pose.orientation.x = 0; - kin.initial_pose_with_covariance.pose.orientation.y = 0; - kin.initial_pose_with_covariance.pose.orientation.z = 0; - kin.initial_pose_with_covariance.pose.orientation.w = 1; - - kin.initial_twist_with_covariance.twist.linear.x = 1; - kin.initial_twist_with_covariance.twist.linear.y = 0; - kin.initial_twist_with_covariance.twist.linear.z = 0; - kin.initial_twist_with_covariance.twist.angular.x = 0; - kin.initial_twist_with_covariance.twist.angular.y = 0; - kin.initial_twist_with_covariance.twist.angular.z = 0; - - kin.initial_acceleration_with_covariance.accel.linear.x = 0; - kin.initial_acceleration_with_covariance.accel.linear.y = 0; - kin.initial_acceleration_with_covariance.accel.linear.z = 0; - kin.initial_acceleration_with_covariance.accel.angular.x = 0; - kin.initial_acceleration_with_covariance.accel.angular.y = 0; - kin.initial_acceleration_with_covariance.accel.angular.z = 0; - - constexpr size_t path_size = 10; - kin.predicted_paths.resize(1); - kin.predicted_paths[0].path.resize(path_size); - for (size_t i = 0; i < path_size; i++) { - kin.predicted_paths[0].path[i].position.x = i; - kin.predicted_paths[0].path[i].position.y = 0; - kin.predicted_paths[0].path[i].position.z = 0; - } - obj.kinematics = kin; - // } - // { - autoware_perception_msgs::msg::Shape s; - s.type = 1; - geometry_msgs::msg::Point32 p; - p.x = 9.0f; - p.y = 11.0f; - p.z = 0.0f; - s.footprint.points.push_back(p); - p.x = 11.0f; - p.y = 11.0f; - p.z = 0.0f; - s.footprint.points.push_back(p); - p.x = 11.0f; - p.y = 9.0f; - p.z = 0.0f; - s.footprint.points.push_back(p); - p.x = 9.0f; - p.y = 9.0f; - p.z = 0.0f; - s.footprint.points.push_back(p); - - s.dimensions.x = 2; - s.dimensions.y = 2; - s.dimensions.z = 2; - - obj.shape = s; - // } - // } - - msg_perception.objects.push_back(obj); - return msg_perception; -} - -TEST(AutowareAutoMsgsAdapter, TestPredictedObjects) // NOLINT for gtest -{ - const std::string msg_type_target = "autoware_auto_perception_msgs/msg/PredictedObjects"; - 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_perception = generate_perception_msg(); - auto sub = node_subscriber->create_subscription< - autoware_auto_perception_msgs::msg::PredictedObjects>( - topic_name_target, rclcpp::QoS{1}, - [&msg_perception, - &test_completed](const autoware_auto_perception_msgs::msg::PredictedObjects::SharedPtr msg) { - EXPECT_EQ(msg->header.stamp, msg_perception.header.stamp); - EXPECT_EQ(msg->header.frame_id, msg_perception.header.frame_id); - EXPECT_EQ(msg->objects[0].object_id.uuid, msg_perception.objects[0].object_id.uuid); - EXPECT_FLOAT_EQ( - msg->objects[0].existence_probability, msg_perception.objects[0].existence_probability); - EXPECT_EQ( - msg->objects[0].classification[0].label, msg_perception.objects[0].classification[0].label); - EXPECT_FLOAT_EQ( - msg->objects[0].classification[0].probability, - msg_perception.objects[0].classification[0].probability); - EXPECT_FLOAT_EQ( - msg->objects[0].kinematics.initial_pose_with_covariance.pose.position.x, - msg_perception.objects[0].kinematics.initial_pose_with_covariance.pose.position.x); - EXPECT_FLOAT_EQ( - msg->objects[0].kinematics.initial_pose_with_covariance.pose.position.y, - msg_perception.objects[0].kinematics.initial_pose_with_covariance.pose.position.y); - EXPECT_FLOAT_EQ( - msg->objects[0].kinematics.initial_pose_with_covariance.pose.position.z, - msg_perception.objects[0].kinematics.initial_pose_with_covariance.pose.position.z); - EXPECT_FLOAT_EQ( - msg->objects[0].kinematics.initial_pose_with_covariance.pose.orientation.x, - msg_perception.objects[0].kinematics.initial_pose_with_covariance.pose.orientation.x); - EXPECT_FLOAT_EQ( - msg->objects[0].kinematics.initial_pose_with_covariance.pose.orientation.y, - msg_perception.objects[0].kinematics.initial_pose_with_covariance.pose.orientation.y); - EXPECT_FLOAT_EQ( - msg->objects[0].kinematics.initial_pose_with_covariance.pose.orientation.z, - msg_perception.objects[0].kinematics.initial_pose_with_covariance.pose.orientation.z); - EXPECT_FLOAT_EQ( - msg->objects[0].kinematics.initial_pose_with_covariance.pose.orientation.w, - msg_perception.objects[0].kinematics.initial_pose_with_covariance.pose.orientation.w); - - EXPECT_FLOAT_EQ( - msg->objects[0].kinematics.initial_twist_with_covariance.twist.linear.x, - msg_perception.objects[0].kinematics.initial_twist_with_covariance.twist.linear.x); - EXPECT_FLOAT_EQ( - msg->objects[0].kinematics.initial_twist_with_covariance.twist.linear.y, - msg_perception.objects[0].kinematics.initial_twist_with_covariance.twist.linear.y); - EXPECT_FLOAT_EQ( - msg->objects[0].kinematics.initial_twist_with_covariance.twist.linear.z, - msg_perception.objects[0].kinematics.initial_twist_with_covariance.twist.linear.z); - EXPECT_FLOAT_EQ( - msg->objects[0].kinematics.initial_twist_with_covariance.twist.angular.x, - msg_perception.objects[0].kinematics.initial_twist_with_covariance.twist.angular.x); - EXPECT_FLOAT_EQ( - msg->objects[0].kinematics.initial_twist_with_covariance.twist.angular.y, - msg_perception.objects[0].kinematics.initial_twist_with_covariance.twist.angular.y); - EXPECT_FLOAT_EQ( - msg->objects[0].kinematics.initial_twist_with_covariance.twist.angular.z, - msg_perception.objects[0].kinematics.initial_twist_with_covariance.twist.angular.z); - - EXPECT_FLOAT_EQ( - msg->objects[0].kinematics.initial_acceleration_with_covariance.accel.linear.x, - msg_perception.objects[0].kinematics.initial_acceleration_with_covariance.accel.linear.x); - EXPECT_FLOAT_EQ( - msg->objects[0].kinematics.initial_acceleration_with_covariance.accel.linear.y, - msg_perception.objects[0].kinematics.initial_acceleration_with_covariance.accel.linear.y); - EXPECT_FLOAT_EQ( - msg->objects[0].kinematics.initial_acceleration_with_covariance.accel.linear.z, - msg_perception.objects[0].kinematics.initial_acceleration_with_covariance.accel.linear.z); - EXPECT_FLOAT_EQ( - msg->objects[0].kinematics.initial_acceleration_with_covariance.accel.angular.x, - msg_perception.objects[0].kinematics.initial_acceleration_with_covariance.accel.angular.x); - EXPECT_FLOAT_EQ( - msg->objects[0].kinematics.initial_acceleration_with_covariance.accel.angular.y, - msg_perception.objects[0].kinematics.initial_acceleration_with_covariance.accel.angular.y); - EXPECT_FLOAT_EQ( - msg->objects[0].kinematics.initial_acceleration_with_covariance.accel.angular.z, - msg_perception.objects[0].kinematics.initial_acceleration_with_covariance.accel.angular.z); - - for (size_t i = 0; i < msg->objects[0].kinematics.predicted_paths[0].path.size(); i++) { - EXPECT_FLOAT_EQ( - msg->objects[0].kinematics.predicted_paths[0].path[i].position.x, - msg_perception.objects[0].kinematics.predicted_paths[0].path[i].position.x); - EXPECT_FLOAT_EQ( - msg->objects[0].kinematics.predicted_paths[0].path[i].position.y, - msg_perception.objects[0].kinematics.predicted_paths[0].path[i].position.y); - EXPECT_FLOAT_EQ( - msg->objects[0].kinematics.predicted_paths[0].path[i].position.z, - msg_perception.objects[0].kinematics.predicted_paths[0].path[i].position.z); - } - - EXPECT_EQ(msg->objects[0].shape.type, msg_perception.objects[0].shape.type); - for (size_t i = 0; i < msg_perception.objects[0].shape.footprint.points.size(); i++) { - EXPECT_FLOAT_EQ( - msg->objects[0].shape.footprint.points[i].x, - msg_perception.objects[0].shape.footprint.points[i].x); - EXPECT_FLOAT_EQ( - msg->objects[0].shape.footprint.points[i].y, - msg_perception.objects[0].shape.footprint.points[i].y); - EXPECT_FLOAT_EQ( - msg->objects[0].shape.footprint.points[i].z, - msg_perception.objects[0].shape.footprint.points[i].z); - } - EXPECT_FLOAT_EQ( - msg->objects[0].shape.dimensions.x, msg_perception.objects[0].shape.dimensions.x); - EXPECT_FLOAT_EQ( - msg->objects[0].shape.dimensions.y, msg_perception.objects[0].shape.dimensions.y); - EXPECT_FLOAT_EQ( - msg->objects[0].shape.dimensions.z, msg_perception.objects[0].shape.dimensions.z); - - 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_perception); - - 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(); -}