feat(planning_topic_converter): add new package (autowarefoundation#5484

)

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

planning/planning_topic_converter/CMakeLists.txt

@@ -0,0 +1,24 @@
+cmake_minimum_required(VERSION 3.5)
+project(planning_topic_converter)
+
+### Compile options
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+endif()
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic -Werror)
+endif()
+
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+ament_auto_add_library(planning_topic_converter SHARED
+  src/path_to_trajectory.cpp
+)
+
+rclcpp_components_register_node(planning_topic_converter
+  PLUGIN "planning_topic_converter::PathToTrajectory"
+  EXECUTABLE path_to_trajectory_converter
+)
+
+ament_auto_package()

planning/planning_topic_converter/README.md

@@ -0,0 +1,34 @@
+# Planning Topic Converter
+
+## Purpose
+
+This package provides tools that convert topic type among types are defined in <https://github.com/tier4/autoware_auto_msgs>.
+
+## Inner-workings / Algorithms
+
+### Usage example
+
+The tools in this package are provided as composable ROS 2 component nodes, so that they can be spawned into an existing process, launched from launch files, or invoked from the command line.
+
+```xml
+<load_composable_node target="container_name">
+  <composable_node pkg="planning_topic_converter" plugin="planning_topic_converter::PathToTrajectory" name="path_to_trajectory_converter" namespace="">
+  <!-- params -->
+  <param name="input_topic" value="foo"/>
+  <param name="output_topic" value="bar"/>
+  <!-- composable node config -->
+  <extra_arg name="use_intra_process_comms" value="false"/>
+  </composable_node>
+</load_composable_node>
+```
+
+## Parameters
+
+| Name           | Type   | Description        |
+| :------------- | :----- | :----------------- |
+| `input_topic`  | string | input topic name.  |
+| `output_topic` | string | output topic name. |
+
+## Assumptions / Known limits
+
+## Future extensions / Unimplemented parts

planning/planning_topic_converter/include/planning_topic_converter/converter_base.hpp

@@ -0,0 +1,51 @@
+// Copyright 2023 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef PLANNING_TOPIC_CONVERTER__CONVERTER_BASE_HPP_
+#define PLANNING_TOPIC_CONVERTER__CONVERTER_BASE_HPP_
+
+#include "rclcpp/rclcpp.hpp"
+
+#include <memory>
+#include <string>
+
+namespace planning_topic_converter
+{
+
+template <typename InputType, typename OutputType>
+class ConverterBase : public rclcpp::Node
+{
+public:
+  ConverterBase(const std::string & node_name, const rclcpp::NodeOptions & options)
+  : rclcpp::Node(node_name, options)
+  {
+    const auto input_topic = this->declare_parameter<std::string>("input_topic");
+    const auto output_topic = this->declare_parameter<std::string>("output_topic");
+
+    pub_ = this->create_publisher<OutputType>(output_topic, 1);
+    sub_ = this->create_subscription<InputType>(
+      input_topic, 1, std::bind(&ConverterBase::process, this, std::placeholders::_1));
+  }
+
+protected:
+  virtual void process(const typename InputType::ConstSharedPtr msg) = 0;
+  typename rclcpp::Publisher<OutputType>::SharedPtr pub_;
+  typename rclcpp::Subscription<InputType>::SharedPtr sub_;
+
+private:
+};
+
+}  // namespace planning_topic_converter
+
+#endif  // PLANNING_TOPIC_CONVERTER__CONVERTER_BASE_HPP_

planning/planning_topic_converter/include/planning_topic_converter/path_to_trajectory.hpp

@@ -0,0 +1,45 @@
+// Copyright 2023 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef PLANNING_TOPIC_CONVERTER__PATH_TO_TRAJECTORY_HPP_
+#define PLANNING_TOPIC_CONVERTER__PATH_TO_TRAJECTORY_HPP_
+
+#include "planning_topic_converter/converter_base.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+#include <autoware_auto_planning_msgs/msg/path.hpp>
+#include <autoware_auto_planning_msgs/msg/trajectory.hpp>
+
+#include <string>
+
+namespace planning_topic_converter
+{
+
+using autoware_auto_planning_msgs::msg::Path;
+using autoware_auto_planning_msgs::msg::PathPoint;
+using autoware_auto_planning_msgs::msg::Trajectory;
+using autoware_auto_planning_msgs::msg::TrajectoryPoint;
+
+class PathToTrajectory : public ConverterBase<Path, Trajectory>
+{
+public:
+  explicit PathToTrajectory(const rclcpp::NodeOptions & options);
+
+private:
+  void process(const Path::ConstSharedPtr msg) override;
+};
+
+}  // namespace planning_topic_converter
+
+#endif  // PLANNING_TOPIC_CONVERTER__PATH_TO_TRAJECTORY_HPP_

planning/planning_topic_converter/package.xml

@@ -0,0 +1,29 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>planning_topic_converter</name>
+  <version>0.1.0</version>
+  <description>The planning_topic_converter package</description>
+
+  <maintainer email="satoshi.ota@tier4.jp">Satoshi OTA</maintainer>
+  <maintainer email="shumpei.wakabayashi@tier4.jp">Shumpei Wakabayashi</maintainer>
+  <maintainer email="kosuke.takeuchi@tier4.jp">Kosuke Takeuchi</maintainer>
+
+  <license>Apache License 2.0</license>
+
+  <author email="satoshi.ota@tier4.jp">Satoshi OTA</author>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <depend>autoware_auto_planning_msgs</depend>
+  <depend>motion_utils</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>tier4_autoware_utils</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

planning/planning_topic_converter/src/path_to_trajectory.cpp

@@ -0,0 +1,69 @@
+// Copyright 2023 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "planning_topic_converter/path_to_trajectory.hpp"
+
+#include <motion_utils/trajectory/tmp_conversion.hpp>
+#include <tier4_autoware_utils/geometry/geometry.hpp>
+
+namespace planning_topic_converter
+{
+namespace
+{
+TrajectoryPoint convertToTrajectoryPoint(const PathPoint & point)
+{
+  TrajectoryPoint traj_point;
+  traj_point.pose = tier4_autoware_utils::getPose(point);
+  traj_point.longitudinal_velocity_mps = point.longitudinal_velocity_mps;
+  traj_point.lateral_velocity_mps = point.lateral_velocity_mps;
+  traj_point.heading_rate_rps = point.heading_rate_rps;
+  return traj_point;
+}
+
+std::vector<TrajectoryPoint> convertToTrajectoryPoints(const std::vector<PathPoint> & points)
+{
+  std::vector<TrajectoryPoint> traj_points;
+  for (const auto & point : points) {
+    const auto traj_point = convertToTrajectoryPoint(point);
+    traj_points.push_back(traj_point);
+  }
+  return traj_points;
+}
+
+Trajectory createTrajectory(
+  const std_msgs::msg::Header & header, const std::vector<TrajectoryPoint> & trajectory_points)
+{
+  auto trajectory = motion_utils::convertToTrajectory(trajectory_points);
+  trajectory.header = header;
+
+  return trajectory;
+}
+}  // namespace
+
+PathToTrajectory::PathToTrajectory(const rclcpp::NodeOptions & options)
+: ConverterBase("path_to_trajectory_converter", options)
+{
+}
+
+void PathToTrajectory::process(const Path::ConstSharedPtr msg)
+{
+  const auto trajectory_points = convertToTrajectoryPoints(msg->points);
+  const auto output = createTrajectory(msg->header, trajectory_points);
+  pub_->publish(output);
+}
+
+}  // namespace planning_topic_converter
+
+#include <rclcpp_components/register_node_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(planning_topic_converter::PathToTrajectory)