Skip to content

Commit 6f3dcda

Browse files
TetsuKawaTetsuKawa
committed
feat(trajectory_repeater): add trajectory repeater node (#1684)
feat: add trajectory repeater node Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com>
1 parent a5ebaa6 commit 6f3dcda

7 files changed

+205
-0
lines changed

‎planning/trajectory_repeater/CMakeLists.txt

+21
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,21 @@
1+
cmake_minimum_required(VERSION 3.14)
2+
project(trajectory_repeater)
3+
4+
find_package(autoware_cmake REQUIRED)
5+
autoware_package()
6+
7+
ament_auto_add_library(trajectory_repeater SHARED
8+
src/trajectory_repeater.cpp
9+
)
10+
ament_target_dependencies(trajectory_repeater)
11+
12+
rclcpp_components_register_node(${PROJECT_NAME}
13+
PLUGIN "trajectory_repeater::TrajectoryRepeater"
14+
EXECUTABLE ${PROJECT_NAME}_node
15+
)
16+
17+
ament_auto_package(
18+
INSTALL_TO_SHARE
19+
launch
20+
config
21+
)

‎planning/trajectory_repeater/README.md

+23
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,23 @@
1+
# Trajectory Repeater
2+
3+
## Purpose
4+
5+
## Inner-workings / Algorithms
6+
7+
## Inputs / Outputs
8+
9+
### Input
10+
11+
### Output
12+
13+
## Parameters
14+
15+
## Assumptions / Known limits
16+
17+
## (Optional) Error detection and handling
18+
19+
## (Optional) Performance characterization
20+
21+
## (Optional) References/External links
22+
23+
## (Optional) Future extensions / Unimplemented parts

‎planning/trajectory_repeater/config/trajectory_repeater.param.yaml

+2
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,2 @@
1+
/**:
2+
ros__parameters:

‎planning/trajectory_repeater/launch/trajectory_repeater.launch.xml

+8
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,8 @@
1+
<launch>
2+
<arg name="trajectory_repeater_param_path" default="$(find-pkg-share trajectory_repeater)/config/trajectory_repeater.param.yaml"/>
3+
4+
<node pkg="trajectory_repeater" exec="trajectory_repeater_node" name="trajectory_repeater" output="screen">
5+
<remap from="~/input/trajectory" to="/main/planning/scenario_planning/trajectory"/>
6+
<remap from="~/output/trajectory" to="/planning/trajectory_repeater/trajectory"/>
7+
</node>
8+
</launch>

‎planning/trajectory_repeater/package.xml

+25
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,25 @@
1+
<?xml version="1.0"?>
2+
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
3+
<package format="3">
4+
<name>trajectory_repeater</name>
5+
<version>0.1.0</version>
6+
<description>The trajectory_repeater package</description>
7+
<maintainer email="makoto.kurihara@tier4.jp">Makoto Kurihara</maintainer>
8+
<license>Apache License 2.0</license>
9+
10+
<buildtool_depend>ament_cmake_auto</buildtool_depend>
11+
12+
<build_depend>autoware_cmake</build_depend>
13+
14+
<!-- depend -->
15+
<depend>autoware_planning_msgs</depend>
16+
<depend>rclcpp</depend>
17+
<depend>rclcpp_components</depend>
18+
19+
<test_depend>ament_lint_auto</test_depend>
20+
<test_depend>autoware_lint_common</test_depend>
21+
22+
<export>
23+
<build_type>ament_cmake</build_type>
24+
</export>
25+
</package>

‎planning/trajectory_repeater/src/trajectory_repeater.cpp

+67
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,67 @@
1+
// Copyright 2024 TIER IV, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#include "trajectory_repeater.hpp"
16+
17+
namespace trajectory_repeater
18+
{
19+
20+
TrajectoryRepeater::TrajectoryRepeater(const rclcpp::NodeOptions & node_options)
21+
: Node("trajectory_repeater", node_options)
22+
{
23+
// Parameter
24+
25+
// Subscriber
26+
sub_trajectory_ = create_subscription<autoware_planning_msgs::msg::Trajectory>(
27+
"~/input/trajectory", 10,
28+
std::bind(&TrajectoryRepeater::onTrajectory, this, std::placeholders::_1));
29+
30+
// Publisher
31+
pub_trajectory_ =
32+
create_publisher<autoware_planning_msgs::msg::Trajectory>("~/output/trajectory", 10);
33+
34+
// Service
35+
36+
// Client
37+
38+
// Timer
39+
using namespace std::literals::chrono_literals;
40+
timer_ =
41+
rclcpp::create_timer(this, get_clock(), 100ms, std::bind(&TrajectoryRepeater::onTimer, this));
42+
43+
// State
44+
45+
// Diagnostics
46+
}
47+
48+
void TrajectoryRepeater::onTrajectory(
49+
const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr msg)
50+
{
51+
last_trajectory_ = msg;
52+
}
53+
54+
void TrajectoryRepeater::onTimer()
55+
{
56+
if (!last_trajectory_) {
57+
RCLCPP_DEBUG(get_logger(), "No trajectory received");
58+
return;
59+
}
60+
61+
pub_trajectory_->publish(*last_trajectory_);
62+
}
63+
64+
} // namespace trajectory_repeater
65+
66+
#include <rclcpp_components/register_node_macro.hpp>
67+
RCLCPP_COMPONENTS_REGISTER_NODE(trajectory_repeater::TrajectoryRepeater)

‎planning/trajectory_repeater/src/trajectory_repeater.hpp

+59
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,59 @@
1+
// Copyright 2024 TIER IV, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef TRAJECTORY_REPEATER_HPP_
16+
#define TRAJECTORY_REPEATER_HPP_
17+
18+
// include
19+
#include <rclcpp/rclcpp.hpp>
20+
21+
#include <autoware_planning_msgs/msg/trajectory.hpp>
22+
23+
namespace trajectory_repeater
24+
{
25+
26+
class TrajectoryRepeater : public rclcpp::Node
27+
{
28+
public:
29+
explicit TrajectoryRepeater(const rclcpp::NodeOptions & node_options);
30+
~TrajectoryRepeater() = default;
31+
32+
private:
33+
// Parameter
34+
35+
// Subscriber
36+
rclcpp::Subscription<autoware_planning_msgs::msg::Trajectory>::SharedPtr sub_trajectory_;
37+
38+
void onTrajectory(const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr msg);
39+
40+
// Publisher
41+
rclcpp::Publisher<autoware_planning_msgs::msg::Trajectory>::SharedPtr pub_trajectory_;
42+
43+
// Service
44+
45+
// Client
46+
47+
// Timer
48+
rclcpp::TimerBase::SharedPtr timer_;
49+
50+
void onTimer();
51+
52+
// State
53+
autoware_planning_msgs::msg::Trajectory::ConstSharedPtr last_trajectory_;
54+
55+
// Diagnostics
56+
};
57+
} // namespace trajectory_repeater
58+
59+
#endif // TRAJECTORY_REPEATER_HPP_

0 commit comments

Comments
 (0)