From 5723f72ef462c02b28b494068bdada9073f9c402 Mon Sep 17 00:00:00 2001 From: shulanbushangshu <102840938+shulanbushangshu@users.noreply.github.com> Date: Wed, 13 Dec 2023 18:26:41 +0800 Subject: [PATCH] feat: add Planning Msg Adapter (#5814) * fix:planning_adapter Signed-off-by: jack.song * fix:planning_adapter Signed-off-by: jack.song * fix:planning_adapter Signed-off-by: jack.song * fix:planning_adapter Signed-off-by: jack.song * fix:add planning adapter Signed-off-by: jack.song * style(pre-commit): autofix --------- Signed-off-by: jack.song Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com> --- .../autoware_auto_msgs_adapter/CMakeLists.txt | 1 + .../config/adapter_planning.param.yaml | 5 + .../autoware_auto_msgs_adapter.launch.xml | 4 + system/autoware_auto_msgs_adapter/package.xml | 2 + .../autoware_auto_msgs_adapter.schema.json | 3 +- .../src/autoware_auto_msgs_adapter_core.cpp | 5 + .../src/include/adapter_planning.hpp | 70 ++++++++ .../autoware_auto_msgs_adapter_core.hpp | 1 + .../test/test_msg_planning_trajectory.cpp | 149 ++++++++++++++++++ 9 files changed, 239 insertions(+), 1 deletion(-) create mode 100644 system/autoware_auto_msgs_adapter/config/adapter_planning.param.yaml create mode 100644 system/autoware_auto_msgs_adapter/src/include/adapter_planning.hpp create mode 100644 system/autoware_auto_msgs_adapter/test/test_msg_planning_trajectory.cpp diff --git a/system/autoware_auto_msgs_adapter/CMakeLists.txt b/system/autoware_auto_msgs_adapter/CMakeLists.txt index 8b1d31ff2d01..a8b72b8da688 100644 --- a/system/autoware_auto_msgs_adapter/CMakeLists.txt +++ b/system/autoware_auto_msgs_adapter/CMakeLists.txt @@ -14,6 +14,7 @@ ament_auto_add_library(${NODE_NAME} 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) diff --git a/system/autoware_auto_msgs_adapter/config/adapter_planning.param.yaml b/system/autoware_auto_msgs_adapter/config/adapter_planning.param.yaml new file mode 100644 index 000000000000..b4a8712d4c50 --- /dev/null +++ b/system/autoware_auto_msgs_adapter/config/adapter_planning.param.yaml @@ -0,0 +1,5 @@ +/**: + 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 index d80c1bd8cbed..a15c25f38ef3 100755 --- 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 @@ -3,6 +3,7 @@ + @@ -13,4 +14,7 @@ + + + diff --git a/system/autoware_auto_msgs_adapter/package.xml b/system/autoware_auto_msgs_adapter/package.xml index 99a94fa04356..2941820550ba 100644 --- a/system/autoware_auto_msgs_adapter/package.xml +++ b/system/autoware_auto_msgs_adapter/package.xml @@ -22,9 +22,11 @@ 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 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 index f6aa87f77452..a5d6c93029d0 100644 --- 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 @@ -12,7 +12,8 @@ "enum": [ "autoware_auto_control_msgs/msg/AckermannControlCommand", "autoware_auto_mapping_msgs/msg/HADMapBin", - "autoware_auto_perception_msgs/msg/PredictedObjects" + "autoware_auto_perception_msgs/msg/PredictedObjects", + "autoware_auto_planning_msgs/msg/Trajectory" ], "default": "autoware_auto_control_msgs/msg/AckermannControlCommand" }, 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 index 15e3c90d227d..6a2397024686 100644 --- 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 @@ -67,6 +67,11 @@ MapStringAdapter AutowareAutoMsgsAdapterNode::create_adapter_map( 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)); + }}, }; } diff --git a/system/autoware_auto_msgs_adapter/src/include/adapter_planning.hpp b/system/autoware_auto_msgs_adapter/src/include/adapter_planning.hpp new file mode 100644 index 000000000000..79d73bdda057 --- /dev/null +++ b/system/autoware_auto_msgs_adapter/src/include/adapter_planning.hpp @@ -0,0 +1,70 @@ +// 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 index f0e28f48aacd..258bf0124449 100644 --- 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 @@ -17,6 +17,7 @@ #include "adapter_control.hpp" #include "adapter_map.hpp" #include "adapter_perception.hpp" +#include "adapter_planning.hpp" #include 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 new file mode 100644 index 000000000000..e2d44d6085c8 --- /dev/null +++ b/system/autoware_auto_msgs_adapter/test/test_msg_planning_trajectory.cpp @@ -0,0 +1,149 @@ +// 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(); +}