feat: add Planning Msg Adapter (autowarefoundation#5814)

* fix:planning_adapter

Signed-off-by: jack.song <jack.song@autocore.ai>

* fix:planning_adapter

Signed-off-by: jack.song <jack.song@autocore.ai>

* fix:planning_adapter

Signed-off-by: jack.song <jack.song@autocore.ai>

* fix:planning_adapter

Signed-off-by: jack.song <jack.song@autocore.ai>

* fix:add planning adapter

Signed-off-by: jack.song <jack.song@autocore.ai>

* style(pre-commit): autofix

---------

Signed-off-by: jack.song <jack.song@autocore.ai>
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>

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)
 

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"

system/autoware_auto_msgs_adapter/launch/autoware_auto_msgs_adapter.launch.xml

@@ -3,6 +3,7 @@
   <arg name="control_param_path" default="$(find-pkg-share autoware_auto_msgs_adapter)/config/adapter_control.param.yaml"/>
   <arg name="map_param_path" default="$(find-pkg-share autoware_auto_msgs_adapter)/config/adapter_map.param.yaml"/>
   <arg name="perception_param_path" default="$(find-pkg-share autoware_auto_msgs_adapter)/config/adapter_perception.param.yaml"/>
+  <arg name="planning_param_path" default="$(find-pkg-share autoware_auto_msgs_adapter)/config/adapter_planning.param.yaml"/>
 
   <node pkg="autoware_auto_msgs_adapter" exec="autoware_auto_msgs_adapter_exe" name="autoware_auto_msgs_adapter_control" output="screen">
     <param from="$(var control_param_path)"/>
@@ -13,4 +14,7 @@
   <node pkg="autoware_auto_msgs_adapter" exec="autoware_auto_msgs_adapter_exe" name="autoware_auto_msgs_adapter_perception" output="screen">
     <param from="$(var perception_param_path)"/>
   </node>
+  <node pkg="autoware_auto_msgs_adapter" exec="autoware_auto_msgs_adapter_exe" name="autoware_auto_msgs_adapter_planning" output="screen">
+    <param from="$(var planning_param_path)"/>
+  </node>
 </launch>

system/autoware_auto_msgs_adapter/package.xml

@@ -22,9 +22,11 @@
   <depend>autoware_auto_control_msgs</depend>
   <depend>autoware_auto_mapping_msgs</depend>
   <depend>autoware_auto_perception_msgs</depend>
+  <depend>autoware_auto_planning_msgs</depend>
   <depend>autoware_control_msgs</depend>
   <depend>autoware_map_msgs</depend>
   <depend>autoware_perception_msgs</depend>
+  <depend>autoware_planning_msgs</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
 

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"
         },

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<AdapterBaseInterface>(
          std::make_shared<AdapterPerception>(*this, topic_name_source, topic_name_target));
      }},
+    {"autoware_auto_planning_msgs/msg/Trajectory",
+     [&] {
+       return std::static_pointer_cast<AdapterBaseInterface>(
+         std::make_shared<AdapterPlanning>(*this, topic_name_source, topic_name_target));
+     }},
   };
 }
 

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 <rclcpp/rclcpp.hpp>
+
+#include <autoware_auto_planning_msgs/msg/trajectory.hpp>
+#include <autoware_planning_msgs/msg/trajectory.hpp>
+
+#include <string>
+
+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<Trajectory, TrajectoryAuto>
+{
+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_

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 <rclcpp/rclcpp.hpp>
 

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 <autoware_auto_msgs_adapter_core.hpp>
+
+#include <gtest/gtest.h>
+
+#include <random>
+
+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<AutowareAutoMsgsAdapterNode>(node_options);
+
+  std::cout << "Creating the subscriber node..." << std::endl;
+
+  auto node_subscriber = std::make_shared<rclcpp::Node>("node_subscriber", rclcpp::NodeOptions{});
+
+  bool test_completed = false;
+
+  const auto msg_planning = generate_planning_msg();
+  auto sub = node_subscriber->create_subscription<autoware_auto_planning_msgs::msg::Trajectory>(
+    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<rclcpp::Node>("node_publisher", rclcpp::NodeOptions{});
+  auto pub = node_publisher->create_publisher<autoware_planning_msgs::msg::Trajectory>(
+    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();
+}