Skip to content

Commit 54af299

Browse files
interimaddTakahisa.Ishikawa
interimadd
and
Takahisa.Ishikawa
authored
refactor(autoware_trajectory_modifier): split per-frame inputs from long-lived context (#12550)
* refactor(autoware_trajectory_modifier): pass per-frame inputs as method arguments Signed-off-by: Takahisa.Ishikawa <takahisa.ishikawa@tier4.jp> * refactor(autoware_trajectory_modifier): rename FrameInputs to InputData Rename the per-frame plugin-input struct and its header file from FrameInputs/frame_inputs.hpp to InputData/input_data.hpp, plus the associated factory method make_frame_inputs() to make_input_data(). Signed-off-by: Takahisa.Ishikawa <takahisa.ishikawa@tier4.jp> * refactor(autoware_trajectory_modifier): rename TrajectoryModifierData to TrajectoryModifierContext Rename the long-lived plugin-shared struct and its header file from TrajectoryModifierData/trajectory_modifier_structs.hpp to TrajectoryModifierContext/trajectory_modifier_context.hpp, and rename the corresponding member/parameter (data_/data) to context_/context. Signed-off-by: Takahisa.Ishikawa <takahisa.ishikawa@tier4.jp> * chore(autoware_trajectory_modifier): remove redundant comments Signed-off-by: Takahisa.Ishikawa <takahisa.ishikawa@tier4.jp> * test(autoware_trajectory_modifier): declare input variables using helper functions Signed-off-by: Takahisa.Ishikawa <takahisa.ishikawa@tier4.jp> * refactor(autoware_trajectory_modifier): rename `inputs` parameter to `input` The parameter name `inputs` (plural) was misleading because the type is a singular `InputData` struct, not a collection. The `s` suffix conventionally suggests a vector/container, so rename to `input` to match the type. Test helper struct `DataInputs` and the `apply_inputs` function keep their plural form since they intentionally aggregate multiple data streams. Signed-off-by: Takahisa.Ishikawa <takahisa.ishikawa@tier4.jp> * chore(autoware_trajectory_modifier): remove unused includes Signed-off-by: Takahisa.Ishikawa <takahisa.ishikawa@tier4.jp> --------- Signed-off-by: Takahisa.Ishikawa <takahisa.ishikawa@tier4.jp> Co-authored-by: Takahisa.Ishikawa <takahisa.ishikawa@tier4.jp>
1 parent 989ef89 commit 54af299

12 files changed

Lines changed: 271 additions & 265 deletions

File tree

‎planning/autoware_trajectory_modifier/include/autoware/trajectory_modifier/trajectory_modifier.hpp‎

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -15,9 +15,9 @@
1515
#ifndef AUTOWARE__TRAJECTORY_MODIFIER__TRAJECTORY_MODIFIER_HPP_
1616
#define AUTOWARE__TRAJECTORY_MODIFIER__TRAJECTORY_MODIFIER_HPP_
1717

18-
#include "autoware/trajectory_modifier/trajectory_modifier_plugins/stop_point_fixer.hpp"
18+
#include "autoware/trajectory_modifier/trajectory_modifier_context.hpp"
19+
#include "autoware/trajectory_modifier/trajectory_modifier_plugins/input_data.hpp"
1920
#include "autoware/trajectory_modifier/trajectory_modifier_plugins/trajectory_modifier_plugin_base.hpp"
20-
#include "autoware/trajectory_modifier/trajectory_modifier_structs.hpp"
2121

2222
#include <autoware_trajectory_modifier/trajectory_modifier_param.hpp>
2323
#include <autoware_utils_debug/debug_publisher.hpp>
@@ -60,7 +60,7 @@ class TrajectoryModifier : public rclcpp::Node
6060
void on_traj(const CandidateTrajectories::ConstSharedPtr msg);
6161
void load_plugin(const std::string & name);
6262
void unload_plugin(const std::string & name);
63-
void set_data();
63+
plugin::InputData make_input_data();
6464
bool initialized_modifiers_{false};
6565

6666
std::unique_ptr<trajectory_modifier_params::ParamListener> param_listener_;
@@ -88,7 +88,7 @@ class TrajectoryModifier : public rclcpp::Node
8888
pluginlib::ClassLoader<plugin::TrajectoryModifierPluginBase> plugin_loader_;
8989
std::vector<std::shared_ptr<plugin::TrajectoryModifierPluginBase>> plugins_;
9090

91-
std::shared_ptr<TrajectoryModifierData> data_;
91+
std::shared_ptr<TrajectoryModifierContext> context_;
9292
};
9393

9494
} // namespace autoware::trajectory_modifier

‎planning/autoware_trajectory_modifier/include/autoware/trajectory_modifier/trajectory_modifier_context.hpp‎

Lines changed: 38 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,38 @@
1+
// Copyright 2025 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 AUTOWARE__TRAJECTORY_MODIFIER__TRAJECTORY_MODIFIER_CONTEXT_HPP_
16+
#define AUTOWARE__TRAJECTORY_MODIFIER__TRAJECTORY_MODIFIER_CONTEXT_HPP_
17+
#include <autoware_vehicle_info_utils/vehicle_info_utils.hpp>
18+
19+
#include <tf2_ros/buffer.h>
20+
#include <tf2_ros/transform_listener.h>
21+
22+
namespace autoware::trajectory_modifier
23+
{
24+
struct TrajectoryModifierContext
25+
{
26+
explicit TrajectoryModifierContext(rclcpp::Node * node)
27+
: vehicle_info(autoware::vehicle_info_utils::VehicleInfoUtils(*node).getVehicleInfo()),
28+
tf_buffer{node->get_clock()},
29+
tf_listener{tf_buffer}
30+
{
31+
}
32+
33+
autoware::vehicle_info_utils::VehicleInfo vehicle_info;
34+
tf2_ros::Buffer tf_buffer;
35+
tf2_ros::TransformListener tf_listener;
36+
};
37+
} // namespace autoware::trajectory_modifier
38+
#endif // AUTOWARE__TRAJECTORY_MODIFIER__TRAJECTORY_MODIFIER_CONTEXT_HPP_

‎planning/autoware_trajectory_modifier/include/autoware/trajectory_modifier/trajectory_modifier_plugins/input_data.hpp‎

Lines changed: 38 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,38 @@
1+
// Copyright 2026 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+
// NOLINTNEXTLINE
16+
#ifndef AUTOWARE__TRAJECTORY_MODIFIER__TRAJECTORY_MODIFIER_PLUGINS__INPUT_DATA_HPP_
17+
// NOLINTNEXTLINE
18+
#define AUTOWARE__TRAJECTORY_MODIFIER__TRAJECTORY_MODIFIER_PLUGINS__INPUT_DATA_HPP_
19+
20+
#include <autoware_perception_msgs/msg/predicted_objects.hpp>
21+
#include <geometry_msgs/msg/accel_with_covariance_stamped.hpp>
22+
#include <nav_msgs/msg/odometry.hpp>
23+
#include <sensor_msgs/msg/point_cloud2.hpp>
24+
25+
namespace autoware::trajectory_modifier::plugin
26+
{
27+
struct InputData
28+
{
29+
nav_msgs::msg::Odometry::ConstSharedPtr current_odometry = nullptr;
30+
geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr current_acceleration = nullptr;
31+
autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr predicted_objects = nullptr;
32+
sensor_msgs::msg::PointCloud2::ConstSharedPtr obstacle_pointcloud = nullptr;
33+
};
34+
35+
} // namespace autoware::trajectory_modifier::plugin
36+
37+
// NOLINTNEXTLINE
38+
#endif // AUTOWARE__TRAJECTORY_MODIFIER__TRAJECTORY_MODIFIER_PLUGINS__INPUT_DATA_HPP_

‎planning/autoware_trajectory_modifier/include/autoware/trajectory_modifier/trajectory_modifier_plugins/obstacle_stop.hpp‎

Lines changed: 10 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -19,7 +19,6 @@
1919
#include "autoware/trajectory_modifier/trajectory_modifier_utils/obstacle_stop_utils.hpp"
2020
#include "autoware/trajectory_modifier/trajectory_modifier_utils/utils.hpp"
2121

22-
#include <autoware_utils_rclcpp/polling_subscriber.hpp>
2322
#include <rclcpp/rclcpp.hpp>
2423

2524
#include <visualization_msgs/msg/marker_array.hpp>
@@ -33,8 +32,10 @@ namespace autoware::trajectory_modifier::plugin
3332
{
3433
using autoware_internal_planning_msgs::msg::SafetyFactor;
3534
using autoware_internal_planning_msgs::msg::SafetyFactorArray;
35+
using autoware_perception_msgs::msg::PredictedObjects;
3636
using autoware_utils_geometry::MultiPolygon2d;
3737
using autoware_utils_geometry::Polygon2d;
38+
using sensor_msgs::msg::PointCloud2;
3839
using utils::obstacle_stop::CollisionPoint;
3940
using utils::obstacle_stop::DebugData;
4041
using visualization_msgs::msg::Marker;
@@ -45,10 +46,10 @@ class ObstacleStop : public TrajectoryModifierPluginBase
4546
public:
4647
ObstacleStop() = default;
4748

48-
bool modify_trajectory(TrajectoryPoints & traj_points) override;
49+
bool modify_trajectory(TrajectoryPoints & traj_points, const InputData & input) override;
4950

5051
[[nodiscard]] bool is_trajectory_modification_required(
51-
const TrajectoryPoints & traj_points) override;
52+
const TrajectoryPoints & traj_points, const InputData & input) override;
5253

5354
void update_params(const TrajectoryModifierParams & params) override;
5455

@@ -80,11 +81,13 @@ class ObstacleStop : public TrajectoryModifierPluginBase
8081
rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr debug_viz_pub_;
8182
rclcpp::Publisher<PointCloud2>::SharedPtr pub_clustered_pointcloud_;
8283

83-
void check_obstacles(const TrajectoryPoints & traj_points);
84-
std::optional<CollisionPoint> check_predicted_objects(const TrajectoryPoints & traj_points);
85-
std::optional<CollisionPoint> check_pointcloud(const TrajectoryPoints & traj_points);
84+
void check_obstacles(const TrajectoryPoints & traj_points, const InputData & input);
85+
std::optional<CollisionPoint> check_predicted_objects(
86+
const TrajectoryPoints & traj_points, const InputData & input);
87+
std::optional<CollisionPoint> check_pointcloud(
88+
const TrajectoryPoints & traj_points, const InputData & input);
8689

87-
bool set_stop_point(TrajectoryPoints & traj_points);
90+
bool set_stop_point(TrajectoryPoints & traj_points, const InputData & input);
8891

8992
bool apply_stopping(
9093
TrajectoryPoints & traj_points, const double target_stop_point_arc_length) const;

‎planning/autoware_trajectory_modifier/include/autoware/trajectory_modifier/trajectory_modifier_plugins/stop_point_fixer.hpp‎

Lines changed: 4 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -17,10 +17,6 @@
1717

1818
#include "autoware/trajectory_modifier/trajectory_modifier_plugins/trajectory_modifier_plugin_base.hpp"
1919

20-
#include <memory>
21-
#include <string>
22-
#include <vector>
23-
2420
namespace autoware::trajectory_modifier::plugin
2521
{
2622

@@ -29,12 +25,13 @@ class StopPointFixer : public TrajectoryModifierPluginBase
2925
public:
3026
StopPointFixer() = default;
3127

32-
bool modify_trajectory(TrajectoryPoints & traj_points) override;
28+
bool modify_trajectory(TrajectoryPoints & traj_points, const InputData & input) override;
3329

3430
bool is_long_stop_trajectory(const TrajectoryPoints & traj_points) const;
35-
bool is_stop_point_close_to_ego(const TrajectoryPoints & traj_points) const;
31+
bool is_stop_point_close_to_ego(
32+
const TrajectoryPoints & traj_points, const InputData & input) const;
3633
[[nodiscard]] bool is_trajectory_modification_required(
37-
const TrajectoryPoints & traj_points) override;
34+
const TrajectoryPoints & traj_points, const InputData & input) override;
3835

3936
void update_params(const TrajectoryModifierParams & params) override
4037
{

‎planning/autoware_trajectory_modifier/include/autoware/trajectory_modifier/trajectory_modifier_plugins/trajectory_modifier_plugin_base.hpp‎

Lines changed: 8 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -16,7 +16,8 @@
1616
#ifndef AUTOWARE__TRAJECTORY_MODIFIER__TRAJECTORY_MODIFIER_PLUGINS__TRAJECTORY_MODIFIER_PLUGIN_BASE_HPP_
1717
// NOLINTNEXTLINE
1818
#define AUTOWARE__TRAJECTORY_MODIFIER__TRAJECTORY_MODIFIER_PLUGINS__TRAJECTORY_MODIFIER_PLUGIN_BASE_HPP_
19-
#include "autoware/trajectory_modifier/trajectory_modifier_structs.hpp"
19+
#include "autoware/trajectory_modifier/trajectory_modifier_context.hpp"
20+
#include "autoware/trajectory_modifier/trajectory_modifier_plugins/input_data.hpp"
2021

2122
#include <autoware/planning_factor_interface/planning_factor_interface.hpp>
2223
#include <autoware_trajectory_modifier/trajectory_modifier_param.hpp>
@@ -26,7 +27,6 @@
2627
#include <autoware_planning_msgs/msg/trajectory.hpp>
2728
#include <autoware_planning_msgs/msg/trajectory_point.hpp>
2829

29-
#include <iostream>
3030
#include <memory>
3131
#include <string>
3232
#include <utility>
@@ -47,7 +47,7 @@ class TrajectoryModifierPluginBase
4747
void initialize(
4848
std::string name, rclcpp::Node * node_ptr,
4949
const std::shared_ptr<autoware_utils_debug::TimeKeeper> time_keeper,
50-
const std::shared_ptr<TrajectoryModifierData> & data,
50+
const std::shared_ptr<TrajectoryModifierContext> & context,
5151
[[maybe_unused]] const TrajectoryModifierParams & params)
5252
{
5353
short_name_ = std::invoke([&name]() {
@@ -57,15 +57,16 @@ class TrajectoryModifierPluginBase
5757
name_ = std::move(name);
5858
node_ptr_ = node_ptr;
5959
time_keeper_ = time_keeper;
60-
data_ = data;
60+
context_ = context;
6161
RCLCPP_DEBUG(
6262
node_ptr_->get_logger(), "instantiated TrajectoryModifierPluginBase: %s", name_.c_str());
6363
on_initialize(params);
6464
}
6565

6666
virtual ~TrajectoryModifierPluginBase() = default;
67-
virtual bool modify_trajectory(TrajectoryPoints & traj_points) = 0;
68-
virtual bool is_trajectory_modification_required(const TrajectoryPoints & traj_points) = 0;
67+
virtual bool modify_trajectory(TrajectoryPoints & traj_points, const InputData & input) = 0;
68+
virtual bool is_trajectory_modification_required(
69+
const TrajectoryPoints & traj_points, const InputData & input) = 0;
6970
std::string get_name() const { return name_; }
7071
std::string get_short_name() const { return short_name_; }
7172
rclcpp::Node * get_node_ptr() const { return node_ptr_; }
@@ -92,7 +93,7 @@ class TrajectoryModifierPluginBase
9293
virtual void on_initialize(const TrajectoryModifierParams & params) = 0;
9394
std::unique_ptr<autoware::planning_factor_interface::PlanningFactorInterface>
9495
planning_factor_interface_;
95-
std::shared_ptr<TrajectoryModifierData> data_;
96+
std::shared_ptr<TrajectoryModifierContext> context_;
9697
bool enabled_{true};
9798
double trajectory_time_step_{0.1};
9899

‎planning/autoware_trajectory_modifier/include/autoware/trajectory_modifier/trajectory_modifier_structs.hpp‎

Lines changed: 0 additions & 73 deletions
This file was deleted.

‎planning/autoware_trajectory_modifier/include/autoware/trajectory_modifier/trajectory_modifier_utils/obstacle_stop_utils.hpp‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -124,6 +124,7 @@ struct DebugData
124124
std::vector<geometry_msgs::msg::Point> target_pcd_points;
125125
geometry_msgs::msg::Point active_collision_point;
126126
std::optional<PredictedObject> colliding_object;
127+
double ego_z = 0.0; // cached for marker placement during publish_debug_data
127128
};
128129

129130
void trim_trajectory_and_remove_duplicates(TrajectoryPoints & trajectory_points);

0 commit comments

Comments
 (0)