feat(trajectory_repeater): add trajectory repeater node (#1684)

feat: add trajectory repeater node

Signed-off-by: TetsuKawa <[email protected]>

planning/trajectory_repeater/CMakeLists.txt

@@ -0,0 +1,21 @@
+cmake_minimum_required(VERSION 3.14)
+project(trajectory_repeater)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+ament_auto_add_library(trajectory_repeater SHARED
+  src/trajectory_repeater.cpp
+)
+ament_target_dependencies(trajectory_repeater)
+
+rclcpp_components_register_node(${PROJECT_NAME}
+  PLUGIN "trajectory_repeater::TrajectoryRepeater"
+  EXECUTABLE ${PROJECT_NAME}_node
+)
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+  launch
+  config
+)

planning/trajectory_repeater/README.md

@@ -0,0 +1,23 @@
+# Trajectory Repeater
+
+## Purpose
+
+## Inner-workings / Algorithms
+
+## Inputs / Outputs
+
+### Input
+
+### Output
+
+## Parameters
+
+## Assumptions / Known limits
+
+## (Optional) Error detection and handling
+
+## (Optional) Performance characterization
+
+## (Optional) References/External links
+
+## (Optional) Future extensions / Unimplemented parts

planning/trajectory_repeater/config/trajectory_repeater.param.yaml

@@ -0,0 +1,2 @@
+/**:
+  ros__parameters:

planning/trajectory_repeater/launch/trajectory_repeater.launch.xml

@@ -0,0 +1,8 @@
+<launch>
+  <arg name="trajectory_repeater_param_path" default="$(find-pkg-share trajectory_repeater)/config/trajectory_repeater.param.yaml"/>
+
+  <node pkg="trajectory_repeater" exec="trajectory_repeater_node" name="trajectory_repeater" output="screen">
+    <remap from="~/input/trajectory" to="/main/planning/scenario_planning/trajectory"/>
+    <remap from="~/output/trajectory" to="/planning/trajectory_repeater/trajectory"/>
+  </node>
+</launch>

planning/trajectory_repeater/package.xml

@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>trajectory_repeater</name>
+  <version>0.1.0</version>
+  <description>The trajectory_repeater package</description>
+  <maintainer email="[email protected]">Makoto Kurihara</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <build_depend>autoware_cmake</build_depend>
+
+  <!-- depend -->
+  <depend>autoware_planning_msgs</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</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/trajectory_repeater/src/trajectory_repeater.cpp

@@ -0,0 +1,67 @@
+// Copyright 2024 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 "trajectory_repeater.hpp"
+
+namespace trajectory_repeater
+{
+
+TrajectoryRepeater::TrajectoryRepeater(const rclcpp::NodeOptions & node_options)
+: Node("trajectory_repeater", node_options)
+{
+  // Parameter
+
+  // Subscriber
+  sub_trajectory_ = create_subscription<autoware_planning_msgs::msg::Trajectory>(
+    "~/input/trajectory", 10,
+    std::bind(&TrajectoryRepeater::onTrajectory, this, std::placeholders::_1));
+
+  // Publisher
+  pub_trajectory_ =
+    create_publisher<autoware_planning_msgs::msg::Trajectory>("~/output/trajectory", 10);
+
+  // Service
+
+  // Client
+
+  // Timer
+  using namespace std::literals::chrono_literals;
+  timer_ =
+    rclcpp::create_timer(this, get_clock(), 100ms, std::bind(&TrajectoryRepeater::onTimer, this));
+
+  // State
+
+  // Diagnostics
+}
+
+void TrajectoryRepeater::onTrajectory(
+  const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr msg)
+{
+  last_trajectory_ = msg;
+}
+
+void TrajectoryRepeater::onTimer()
+{
+  if (!last_trajectory_) {
+    RCLCPP_DEBUG(get_logger(), "No trajectory received");
+    return;
+  }
+
+  pub_trajectory_->publish(*last_trajectory_);
+}
+
+}  // namespace trajectory_repeater
+
+#include <rclcpp_components/register_node_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(trajectory_repeater::TrajectoryRepeater)

planning/trajectory_repeater/src/trajectory_repeater.hpp

@@ -0,0 +1,59 @@
+// Copyright 2024 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 TRAJECTORY_REPEATER_HPP_
+#define TRAJECTORY_REPEATER_HPP_
+
+// include
+#include <rclcpp/rclcpp.hpp>
+
+#include <autoware_planning_msgs/msg/trajectory.hpp>
+
+namespace trajectory_repeater
+{
+
+class TrajectoryRepeater : public rclcpp::Node
+{
+public:
+  explicit TrajectoryRepeater(const rclcpp::NodeOptions & node_options);
+  ~TrajectoryRepeater() = default;
+
+private:
+  // Parameter
+
+  // Subscriber
+  rclcpp::Subscription<autoware_planning_msgs::msg::Trajectory>::SharedPtr sub_trajectory_;
+
+  void onTrajectory(const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr msg);
+
+  // Publisher
+  rclcpp::Publisher<autoware_planning_msgs::msg::Trajectory>::SharedPtr pub_trajectory_;
+
+  // Service
+
+  // Client
+
+  // Timer
+  rclcpp::TimerBase::SharedPtr timer_;
+
+  void onTimer();
+
+  // State
+  autoware_planning_msgs::msg::Trajectory::ConstSharedPtr last_trajectory_;
+
+  // Diagnostics
+};
+}  // namespace trajectory_repeater
+
+#endif  // TRAJECTORY_REPEATER_HPP_