diff --git a/planning/trajectory_repeater/CMakeLists.txt b/planning/trajectory_repeater/CMakeLists.txt new file mode 100644 index 0000000000000..41185c3495afe --- /dev/null +++ b/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 +) diff --git a/planning/trajectory_repeater/README.md b/planning/trajectory_repeater/README.md new file mode 100644 index 0000000000000..b484c80639437 --- /dev/null +++ b/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 diff --git a/planning/trajectory_repeater/config/trajectory_repeater.param.yaml b/planning/trajectory_repeater/config/trajectory_repeater.param.yaml new file mode 100644 index 0000000000000..a20dbd7ffd3d9 --- /dev/null +++ b/planning/trajectory_repeater/config/trajectory_repeater.param.yaml @@ -0,0 +1,2 @@ +/**: + ros__parameters: diff --git a/planning/trajectory_repeater/launch/trajectory_repeater.launch.xml b/planning/trajectory_repeater/launch/trajectory_repeater.launch.xml new file mode 100644 index 0000000000000..ebbde691c32d4 --- /dev/null +++ b/planning/trajectory_repeater/launch/trajectory_repeater.launch.xml @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/planning/trajectory_repeater/package.xml b/planning/trajectory_repeater/package.xml new file mode 100644 index 0000000000000..897d77284f0eb --- /dev/null +++ b/planning/trajectory_repeater/package.xml @@ -0,0 +1,25 @@ + + + + trajectory_repeater + 0.1.0 + The trajectory_repeater package + Makoto Kurihara + Apache License 2.0 + + ament_cmake_auto + + autoware_cmake + + + autoware_planning_msgs + rclcpp + rclcpp_components + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/trajectory_repeater/src/trajectory_repeater.cpp b/planning/trajectory_repeater/src/trajectory_repeater.cpp new file mode 100644 index 0000000000000..29151bc3e584b --- /dev/null +++ b/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( + "~/input/trajectory", 10, + std::bind(&TrajectoryRepeater::onTrajectory, this, std::placeholders::_1)); + + // Publisher + pub_trajectory_ = + create_publisher("~/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(trajectory_repeater::TrajectoryRepeater) diff --git a/planning/trajectory_repeater/src/trajectory_repeater.hpp b/planning/trajectory_repeater/src/trajectory_repeater.hpp new file mode 100644 index 0000000000000..c6ee54ed6e478 --- /dev/null +++ b/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 + +#include + +namespace trajectory_repeater +{ + +class TrajectoryRepeater : public rclcpp::Node +{ +public: + explicit TrajectoryRepeater(const rclcpp::NodeOptions & node_options); + ~TrajectoryRepeater() = default; + +private: + // Parameter + + // Subscriber + rclcpp::Subscription::SharedPtr sub_trajectory_; + + void onTrajectory(const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr msg); + + // Publisher + rclcpp::Publisher::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_