feat(trajectory_modifier): add obstacle stop feature to trajectory modifier by mkquda · Pull Request #12338 · autowarefoundation/autoware_universe

mkquda · 2026-03-17T05:40:34Z

Description

This PR implements the obstacle_stop plugin within autoware_trajectory_modifier node.

Changes

Refactor trajectory_modifier to:

Use ros2 plugin framework to load submodules
Use generate_parameter_library to create parameter struct from schema on build
Use ParamListener to handle initial param loading and online param updates

Implement obstacle checking logic:

Point filtering & clustering
Predicted objects filtering
Collision check using pcd points
Collision check using predicted objects

Implement stopping behavior:

velocities are modified starting from target stop point based on jerk and acceleration limits
moving average is applied near connection point (where modified velocities meet original trajectory velocity) to smoothen acceleration
Akima spline interpolator is used to generate a constant dt trajectory based on new velocity profile

How was this PR tested?

PSIM

562676997-6e0294f3-0e4d-44ee-8858-8e8bc473d4b8.webm

Notes for reviewers

None.

Interface changes

ROS Parameter Changes

Change type	Parameter Name	Type	Default Value	Description
Added	`use_obstacle_stop`	`bool`	`true`	Flag to enable/disable obstacle stop feature
Added	`trajectory_time_step`	`double`	`0.1`	Time step for trajectory modification [s]
Added	`obstacle_stop.use_objects`	`bool`	`true`	flag to enable/disable using predicted objects for obstacle check
Added	`obstacle_stop.use_pointcloud`	`bool`	`true`	flag to enable/disable using pointcloud for obstacle check
Added	`obstacle_stop.stop_margin`	`double`	`6.0`	Distance to maintain from an obstacle when stopped [m]
Added	`obstacle_stop.nom_stopping_decel`	`double`	`1.0`	Nominal deceleration during stopping [m/s^2]
Added	`obstacle_stop.max_stopping_decel`	`double`	`4.0`	Maximum deceleration during stopping [m/s^2]
Added	`obstacle_stop.stopping_jerk`	`double`	`3.0`	Jerk during stopping [m/s^3]
Added	`obstacle_stop.lateral_margin_m`	`double`	`0.5`	Lateral margin on top of ego width from trajectory to consider for obstacle stop [m]
Added	`obstacle_stop.objects`	`N/A`	`N/A`	Parameters related to predicted objects filtering
Added	`obstacle_stop.pointcloud`	`N/A`	`N/A`	Parameters related to pointcloud filtering & clustering

Effects on system behavior

If diffusion planner trajectory causes collision with obstacle, trajectory modifier will adjust trajectory to stop ego.

* refactor trajectory_modifier node applies following refactors to the trajectory_modifier node: - Use ros2 plugin framework to load submodules - Use generate_parameter_library to create parameter struct from schema on build - Use ParamListener to handle initial param loading and online param updates Signed-off-by: Mohammad <alqudah.mohammad@tier4.jp> * remove unnecessary launch-prefix tag * modify param schema Signed-off-by: Mohammad <alqudah.mohammad@tier4.jp> * style(pre-commit): autofix --------- Signed-off-by: Mohammad <alqudah.mohammad@tier4.jp> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

* refactor trajectory_modifier node applies following refactors to the trajectory_modifier node: - Use ros2 plugin framework to load submodules - Use generate_parameter_library to create parameter struct from schema on build - Use ParamListener to handle initial param loading and online param updates Signed-off-by: Mohammad <alqudah.mohammad@tier4.jp> * remove unnecessary launch-prefix tag * add obstacle stop plugin framework > - add obstacle stop parameters > - add obstacle stop plugin > - update package.xml, plugins.xml, and CMakeLists.txt > - add obstacle stop utils > - implement pointcloud filtering and clustering logic - implement object filtering logic - refactor TrajectoryModifierData struct Signed-off-by: Mohammad <alqudah.mohammad@tier4.jp> * implement collision check and stop point logic - add function to generate trajectory polygon from footprints - add function to get nearest pcd collision point - add function to get nearest object collision point - add logic to track detected collision points for hysterisis - add logic to insert stop point Signed-off-by: Mohammad <alqudah.mohammad@tier4.jp> * fix minimum rule base planner code Signed-off-by: Mohammad <alqudah.mohammad@tier4.jp> * implement stop point logic - add function to calculate stop point index and modify trajectory - don't use hysterisis logic (on/off time buffers) - check for plugin name string instead of using isClassLoaded() Signed-off-by: Mohammad <alqudah.mohammad@tier4.jp> * implement smooth stopping logic - compute stopping trajectory from jerk and accel limits, assuming highest initial velocity - use skima spline trajectory interpolator to generate stopping trajectory - use a constant dt to compute s for each interpolated point Signed-off-by: Mohammad <alqudah.mohammad@tier4.jp> * add debug data and markers Signed-off-by: Mohammad <alqudah.mohammad@tier4.jp> * use motion_utils::calculate_stop_distance function Signed-off-by: Mohammad <alqudah.mohammad@tier4.jp> * pre-commit fix Signed-off-by: Mohammad <alqudah.mohammad@tier4.jp> * don't skip entire trajectory modification for missing pcd or predicted objects, disble stop point setting Signed-off-by: Mohammad <alqudah.mohammad@tier4.jp> --------- Signed-off-by: Mohammad <alqudah.mohammad@tier4.jp>

check for null ptr Signed-off-by: Mohammad Alqudah <alqudah.mohammad@tier4.jp>

…ior (#2770) * improve logic for generating stopping trajectory Signed-off-by: Mohammad <alqudah.mohammad@tier4.jp> * log name of plugins that modified trajectory Signed-off-by: Mohammad <alqudah.mohammad@tier4.jp> --------- Signed-off-by: Mohammad <alqudah.mohammad@tier4.jp>

Signed-off-by: Mohammad Alqudah <alqudah.mohammad@tier4.jp>

github-actions · 2026-03-17T05:41:23Z

Thank you for contributing to the Autoware project!

🚧 If your pull request is in progress, switch it to draft mode.

Please ensure:

You've checked our contribution guidelines.
Your PR follows our pull request guidelines.
All required CI checks pass before marking the PR ready for review.

codecov · 2026-03-17T05:47:13Z

Codecov Report

❌ Patch coverage is 27.38302% with 419 lines in your changes missing coverage. Please review.
✅ Project coverage is 18.60%. Comparing base (45a3d8f) to head (8ac69cf).
⚠️ Report is 2 commits behind head on main.

Files with missing lines	Patch %	Lines
.../src/trajectory_modifier_plugins/obstacle_stop.cpp	0.46%	213 Missing ⚠️
.../trajectory_modifier_utils/obstacle_stop_utils.cpp	0.00%	113 Missing ⚠️
...re_trajectory_modifier/src/trajectory_modifier.cpp	0.00%	56 Missing ⚠️
...difier/tests/test_stop_point_fixer_integration.cpp	87.94%	3 Missing and 14 partials ⚠️
...difier_plugins/trajectory_modifier_plugin_base.hpp	41.66%	6 Missing and 1 partial ⚠️
...fier/trajectory_modifier_plugins/obstacle_stop.hpp	0.00%	5 Missing ⚠️
...rajectory_modifier/trajectory_modifier_structs.hpp	44.44%	5 Missing ⚠️
.../trajectory_modifier_utils/obstacle_stop_utils.hpp	0.00%	2 Missing ⚠️
...toware/trajectory_modifier/trajectory_modifier.hpp	0.00%	1 Missing ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##             main   #12338      +/-   ##
==========================================
+ Coverage   18.54%   18.60%   +0.06%     
==========================================
  Files        1851     1858       +7     
  Lines      127991   128587     +596     
  Branches    45588    45780     +192     
==========================================
+ Hits        23738    23928     +190     
- Misses      84559    85128     +569     
+ Partials    19694    19531     -163

Flag	Coverage Δ		*Carryforward flag
daily	`20.99% <ø> (-0.01%)`	⬇️	Carriedforward from 41c9509
daily-cuda	`18.53% <ø> (-0.02%)`	⬇️	Carriedforward from 41c9509
differential	`8.61% <27.38%> (?)`
total-cuda	`18.53% <ø> (-0.02%)`	⬇️	Carriedforward from 41c9509

*This pull request uses carry forward flags. Click here to find out more.

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

🚀 New features to boost your workflow:

❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

Copilot

Pull request overview

This PR refactors autoware_trajectory_modifier into a plugin-based architecture (pluginlib) with generated parameter handling, and adds a new ObstacleStop modifier plugin that detects collisions (pointcloud + predicted objects) and modifies trajectories to stop before obstacles.

Changes:

Refactor trajectory modifier node to load modifier submodules via pluginlib and handle parameters via generate_parameter_library (ParamListener).
Add ObstacleStop plugin + supporting utilities for pointcloud/object filtering, collision detection, and stop-trajectory generation.
Update configuration, launch, and tests to match new plugin + parameter structure and new utils header paths.

Reviewed changes

Copilot reviewed 21 out of 21 changed files in this pull request and generated 16 comments.

Show a summary per file

File	Description
planning/autoware_trajectory_modifier/src/trajectory_modifier.cpp	Switch node to ParamListener + pluginlib loading; update callback flow
planning/autoware_trajectory_modifier/include/autoware/trajectory_modifier/trajectory_modifier.hpp	Update node interface, plugin loader, and subscribers for new inputs
planning/autoware_trajectory_modifier/include/autoware/trajectory_modifier/trajectory_modifier_plugins/trajectory_modifier_plugin_base.hpp	Redefine plugin base API for pluginlib + shared data + param updates
planning/autoware_trajectory_modifier/src/trajectory_modifier_plugins/obstacle_stop.cpp	Implement obstacle collision checks and stop/velocity modification
planning/autoware_trajectory_modifier/include/autoware/trajectory_modifier/trajectory_modifier_plugins/obstacle_stop.hpp	Declare ObstacleStop plugin interface and debug publishing
planning/autoware_trajectory_modifier/src/trajectory_modifier_utils/obstacle_stop_utils.cpp	Add geometry/pointcloud/object utility functions for obstacle stop
planning/autoware_trajectory_modifier/include/autoware/trajectory_modifier/trajectory_modifier_utils/obstacle_stop_utils.hpp	Add public utility declarations and shared structs for obstacle stop
planning/autoware_trajectory_modifier/src/trajectory_modifier_plugins/stop_point_fixer.cpp	Adapt StopPointFixer to new plugin base API and planning factor publication
planning/autoware_trajectory_modifier/include/autoware/trajectory_modifier/trajectory_modifier_plugins/stop_point_fixer.hpp	Update StopPointFixer interface to new plugin base API
planning/autoware_trajectory_modifier/include/autoware/trajectory_modifier/trajectory_modifier_structs.hpp	Expand shared data to include vehicle info, TF, objects, pointcloud
planning/autoware_trajectory_modifier/param/trajectory_modifier_parameter_struct.yaml	Define generate_parameter_library schema for node + plugins
planning/autoware_trajectory_modifier/schema/trajectory_modifier.schema.json	Extend JSON schema to include obstacle_stop parameters
planning/autoware_trajectory_modifier/config/trajectory_modifier.param.yaml	Add obstacle_stop parameters and top-level flags
planning/autoware_trajectory_modifier/launch/trajectory_modifier.launch.xml	Add module selection + new input remaps (objects/pointcloud)
planning/autoware_trajectory_modifier/plugins.xml	Register pluginlib classes for StopPointFixer and ObstacleStop
planning/autoware_trajectory_modifier/CMakeLists.txt	Split libs (core/utils/plugins), export pluginlib XML, generate params
planning/autoware_trajectory_modifier/package.xml	Add dependencies needed for plugins/params/trajectory tools
planning/autoware_trajectory_modifier/src/trajectory_modifier_utils/utils.cpp	Update include path for utils header
planning/autoware_trajectory_modifier/include/autoware/trajectory_modifier/trajectory_modifier_utils/utils.hpp	Move/rename include guard for new utils header location
planning/autoware_trajectory_modifier/tests/test_utils.cpp	Update include path for utils header
planning/autoware_trajectory_modifier/tests/test_stop_point_fixer_integration.cpp	Update tests to new plugin init + generated params pattern

💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.

planning/autoware_trajectory_modifier/src/trajectory_modifier_utils/obstacle_stop_utils.cpp

+#include <pcl/registration/gicp.h>
+#include <pcl/segmentation/extract_clusters.h>
+#include <pcl/surface/convex_hull.h>
+#include <pcl_conversions/pcl_conversions.h>


planning/autoware_trajectory_modifier/src/trajectory_modifier_plugins/obstacle_stop.cpp

+bool ObstacleStop::modify_trajectory(TrajectoryPoints & traj_points)
+{
+  if (!enabled_ || !is_trajectory_modification_required(traj_points)) return false;
+
+  if (!nearest_collision_point_) return false;
+
+  RCLCPP_WARN_THROTTLE(
+    get_node_ptr()->get_logger(), *get_clock(), 500,
+    "[TM ObstacleStop] Detected collision point at arc length %f m",
+    nearest_collision_point_->arc_length);
+
+  return set_stop_point(traj_points);
+}


planning/autoware_trajectory_modifier/src/trajectory_modifier_plugins/obstacle_stop.cpp

+    geometry_msgs::msg::TransformStamped transform_stamped;
+    try {
+      transform_stamped = data_->tf_buffer.lookupTransform(
+        "map", pointcloud->header.frame_id, pointcloud->header.stamp,
+        rclcpp::Duration::from_seconds(0.1));
+    } catch (tf2::TransformException & e) {
+      RCLCPP_WARN(get_node_ptr()->get_logger(), "no transform found for pointcloud: %s", e.what());
+    }
+
+    Eigen::Affine3f isometry = tf2::transformToEigen(transform_stamped.transform).cast<float>();
+    autoware_utils::transform_pointcloud(*filtered_pointcloud, *filtered_pointcloud, isometry);
+  }


...ifier/include/autoware/trajectory_modifier/trajectory_modifier_utils/obstacle_stop_utils.hpp

+struct CollisionPoint
+{
+  geometry_msgs::msg::Point point;
+  double arc_length;
+  rclcpp::Time start_time;
+  bool is_active{false};


...autoware/trajectory_modifier/trajectory_modifier_plugins/trajectory_modifier_plugin_base.hpp

+    name_ = std::invoke([&name]() {
+      const auto npos = name.find_last_of(':');
+      return npos != std::string::npos ? name.substr(npos + 1) : name;
+    });


planning/autoware_trajectory_modifier/src/trajectory_modifier_plugins/obstacle_stop.cpp

+
+bool ObstacleStop::is_trajectory_modification_required(const TrajectoryPoints & traj_points)
+{
+  debug_data_ = DebugData();


planning/autoware_trajectory_modifier/schema/trajectory_modifier.schema.json

+              "description": "Nominal deceleration during stopping [m/s^2]",
+              "default": -2.0
+            },
+            "nom_stopping_jerk": {
+              "type": "number",
+              "description": "Nominal jerk during stopping [m/s^3]",
+              "default": -3.0
+            },
+            "max_stopping_decel": {
+              "type": "number",
+              "description": "Maximum deceleration during stopping [m/s^2]",
+              "default": -4.0
+            },
+            "max_stopping_jerk": {
+              "type": "number",
+              "description": "Maximum jerk during stopping [m/s^3]",
+              "default": -8.0
+            },
+            "on_time_buffer": {


planning/autoware_trajectory_modifier/src/trajectory_modifier_plugins/obstacle_stop.cpp

+  const auto collision_point_pcd = check_pointcloud(traj_points, debug_data_.trajectory_polygon);
+  const auto collision_point_objects =
+    check_predicted_objects(traj_points, debug_data_.trajectory_polygon);


planning/autoware_trajectory_modifier/src/trajectory_modifier_utils/obstacle_stop_utils.cpp

+autoware_utils_geometry::MultiPolygon2d get_trajectory_polygon(
+  const TrajectoryPoints & trajectory_points, const geometry_msgs::msg::Pose & ego_pose,
+  const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const double lateral_margin,
+  const double longitudinal_margin)
+{
+  const auto offset_pose =
+    autoware_utils::calc_offset_pose(ego_pose, vehicle_info.max_longitudinal_offset_m, 0, 0);
+  auto start_idx = motion_utils::findNearestSegmentIndex(trajectory_points, offset_pose.position);
+  const auto forward_traj_points =
+    TrajectoryPoints(trajectory_points.begin() + start_idx, trajectory_points.end());
+


planning/autoware_trajectory_modifier/src/trajectory_modifier_utils/obstacle_stop_utils.cpp

+  const std::vector<ObjectType> object_types = std::invoke([&]() {
+    std::vector<ObjectType> object_types;
+    object_types.reserve(object_type_strings.size());
+    for (const auto & object_type_string : object_type_strings) {
+      object_types.push_back(string_to_object_type.at(object_type_string));
+    }
+    return object_types;


…ehavior (#2783) add parameter to enable/disable stopping Signed-off-by: Mohammad <alqudah.mohammad@tier4.jp>

…2785) add check for use_objects and use_pointcloud flags Signed-off-by: Mohammad <alqudah.mohammad@tier4.jp>

maxime-clem · 2026-03-19T00:57:42Z

planning/autoware_trajectory_modifier/src/trajectory_modifier.cpp

@@ -39,10 +44,13 @@ TrajectoryModifier::TrajectoryModifier(const rclcpp::NodeOptions & options)

  time_keeper_ = std::make_shared<autoware_utils_debug::TimeKeeper>();


this needs to be initialized with a publisher, otherwise the runtime topic is not published.

danielsanchezaran

@mkquda there seems to be a strange issue with parameter loading, this is how rqt reconfigure looks:

Also it seems that all options to reconfigure the stop point fixer disappeared? OSS trajectory_modifier can be reconfigured online, but this PR seems to remove that option?

Also I think Copilot and @maxime-clem made some good remarks, can you please check?

danielsanchezaran · 2026-03-19T02:08:31Z

@mkquda can you please add some video examples of your feature working? The new plugin should also be reflected in the documentation (README) or please make a new docs/ directory with .md files for the different plugins

danielsanchezaran

The stop walls are great additions, but I noticed a flickering for the obstacle stop wall:

cap-.2026-03-19-11-10-15.mp4

could you please investigate? @mkquda

danielsanchezaran

The plugin loading method used by this PR does not really align with how the autoware_trajectory_optimizer loads its plugins, as the optimizer receives the plugin names from the param file and not directly in the launch file. I think it is better to do it how the optimizer does and have a separate enable flag to enable /disable plugins online

danielsanchezaran

Please add some unit tests for the ObstableStop modifier plugin and / or its utils

Copilot

Pull request overview

This PR refactors autoware_trajectory_modifier into a pluginlib-based architecture with generated parameter structs, and adds a new ObstacleStop plugin that detects collisions (pointcloud + predicted objects) and modifies trajectories to stop before obstacles.

Changes:

Refactor TrajectoryModifier to load modifier plugins via pluginlib and update parameters via generate_parameter_library + ParamListener.
Add ObstacleStop plugin + supporting utilities for collision detection and stop/velocity-profile generation.
Update configs/launch/tests/includes to match the new module + utils structure.

Reviewed changes

Copilot reviewed 21 out of 21 changed files in this pull request and generated 10 comments.

Show a summary per file

File	Description
planning/autoware_trajectory_modifier/src/trajectory_modifier.cpp	Loads plugins from `launch_modules`, updates params via `ParamListener`, and runs plugins over candidate trajectories.
planning/autoware_trajectory_modifier/include/autoware/trajectory_modifier/trajectory_modifier.hpp	Updates node members for pluginlib + generated params + extra subscriptions.
planning/autoware_trajectory_modifier/include/autoware/trajectory_modifier/trajectory_modifier_plugins/trajectory_modifier_plugin_base.hpp	Changes plugin API to `initialize()/update_params()/modify_trajectory()` and adds planning factor + debug hooks.
planning/autoware_trajectory_modifier/src/trajectory_modifier_plugins/stop_point_fixer.cpp	Migrates StopPointFixer to the new plugin base interface and planning factor publishing.
planning/autoware_trajectory_modifier/include/autoware/trajectory_modifier/trajectory_modifier_plugins/stop_point_fixer.hpp	Updates StopPointFixer interface for the new plugin API and generated params.
planning/autoware_trajectory_modifier/src/trajectory_modifier_plugins/obstacle_stop.cpp	Implements obstacle collision checks and trajectory stopping behavior with debug outputs.
planning/autoware_trajectory_modifier/include/autoware/trajectory_modifier/trajectory_modifier_plugins/obstacle_stop.hpp	Declares ObstacleStop plugin interface and debug publishers.
planning/autoware_trajectory_modifier/src/trajectory_modifier_utils/obstacle_stop_utils.cpp	Adds pointcloud filtering/clustering and object filtering + collision point selection helpers.
planning/autoware_trajectory_modifier/include/autoware/trajectory_modifier/trajectory_modifier_utils/obstacle_stop_utils.hpp	Declares ObstacleStop helper types/APIs.
planning/autoware_trajectory_modifier/src/trajectory_modifier_utils/utils.cpp	Updates include path for shared trajectory modifier utilities.
planning/autoware_trajectory_modifier/include/autoware/trajectory_modifier/trajectory_modifier_utils/utils.hpp	Renames include guard to match new header path.
planning/autoware_trajectory_modifier/include/autoware/trajectory_modifier/trajectory_modifier_structs.hpp	Extends shared data container with vehicle info, TF, predicted objects, and pointcloud.
planning/autoware_trajectory_modifier/plugins.xml	Registers StopPointFixer and ObstacleStop as pluginlib classes.
planning/autoware_trajectory_modifier/CMakeLists.txt	Adds generated param library, splits libs into core/utils/plugins, exports plugin description.
planning/autoware_trajectory_modifier/package.xml	Adds dependencies for pluginlib + parameter generation + obstacle stop implementation.
planning/autoware_trajectory_modifier/launch/trajectory_modifier.launch.xml	Adds module selection + object/pointcloud remaps and constructs `launch_modules` list.
planning/autoware_trajectory_modifier/config/trajectory_modifier.param.yaml	Adds obstacle_stop parameters and new top-level parameters.
planning/autoware_trajectory_modifier/schema/trajectory_modifier.schema.json	Extends parameter schema for obstacle stop and new top-level flags.
planning/autoware_trajectory_modifier/param/trajectory_modifier_parameter_struct.yaml	New parameter-struct definition used by `generate_parameter_library`.
planning/autoware_trajectory_modifier/tests/test_stop_point_fixer_integration.cpp	Updates integration test to new plugin init + generated params and node options.
planning/autoware_trajectory_modifier/tests/test_utils.cpp	Updates utils header include path.

💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.

...ifier/include/autoware/trajectory_modifier/trajectory_modifier_utils/obstacle_stop_utils.hpp

+#include <autoware_utils_geometry/boost_geometry.hpp>
+#include <autoware_vehicle_info_utils/vehicle_info.hpp>
+
+#include <autoware_perception_msgs/msg/predicted_objects.hpp>
+#include <autoware_planning_msgs/msg/trajectory.hpp>
+#include <autoware_planning_msgs/msg/trajectory_point.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl_conversions/pcl_conversions.h>
+
+#include <string>
+#include <unordered_map>
+#include <vector>
+
+namespace autoware::trajectory_modifier::utils::obstacle_stop
+{
+using sensor_msgs::msg::PointCloud2;
+using PointCloud = pcl::PointCloud<pcl::PointXYZ>;
+using autoware_perception_msgs::msg::ObjectClassification;
+using autoware_perception_msgs::msg::PredictedObjects;
+using autoware_planning_msgs::msg::TrajectoryPoint;
+using TrajectoryPoints = std::vector<TrajectoryPoint>;
+
+enum class ObjectType : uint8_t {
+  UNKNOWN = 0,
+  CAR,
+  TRUCK,
+  BUS,
+  TRAILER,
+  MOTORCYCLE,
+  BICYCLE,
+  PEDESTRIAN
+};
+
+inline static const std::unordered_map<std::string, ObjectType> string_to_object_type = {
+  {"unknown", ObjectType::UNKNOWN}, {"car", ObjectType::CAR},
+  {"truck", ObjectType::TRUCK},     {"bus", ObjectType::BUS},
+  {"trailer", ObjectType::TRAILER}, {"motorcycle", ObjectType::MOTORCYCLE},
+  {"bicycle", ObjectType::BICYCLE}, {"pedestrian", ObjectType::PEDESTRIAN}};
+
+inline static const std::unordered_map<uint8_t, ObjectType> classification_to_object_type = {
+  {ObjectClassification::UNKNOWN, ObjectType::UNKNOWN},
+  {ObjectClassification::CAR, ObjectType::CAR},
+  {ObjectClassification::TRUCK, ObjectType::TRUCK},
+  {ObjectClassification::BUS, ObjectType::BUS},
+  {ObjectClassification::TRAILER, ObjectType::TRAILER},
+  {ObjectClassification::MOTORCYCLE, ObjectType::MOTORCYCLE},
+  {ObjectClassification::BICYCLE, ObjectType::BICYCLE},
+  {ObjectClassification::PEDESTRIAN, ObjectType::PEDESTRIAN}};
+
+struct CollisionPoint
+{
+  geometry_msgs::msg::Point point;
+  double arc_length;
+  rclcpp::Time start_time;
+  bool is_active{false};
+
+  CollisionPoint(const geometry_msgs::msg::Point & point, const double arc_length)
+  : point(point), arc_length(arc_length)
+  {
+  }
+
+  CollisionPoint(
+    const CollisionPoint & collision_point, const rclcpp::Time & start_time, const bool active)
+  : point(collision_point.point),
+    arc_length(collision_point.arc_length),
+    start_time(start_time),
+    is_active(active)
+  {
+  }
+};
+
+struct DebugData
+{
+  PointCloud2::SharedPtr cluster_points;
+  PointCloud2::SharedPtr voxel_points;
+  PointCloud2::SharedPtr target_pcd_points;
+  std::vector<autoware_utils_geometry::Polygon2d> target_polygons;
+  autoware_utils_geometry::MultiPolygon2d trajectory_polygon;
+  std::vector<geometry_msgs::msg::Point> target_collision_points;
+  geometry_msgs::msg::Point active_collision_point;
+};
+
+autoware_utils_geometry::MultiPolygon2d get_trajectory_polygon(
+  const TrajectoryPoints & trajectory_points, const geometry_msgs::msg::Pose & ego_pose,
+  const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const double lateral_margin = 0.0,
+  const double longitudinal_margin = 0.0);


planning/autoware_trajectory_modifier/schema/trajectory_modifier.schema.json

+            "nom_stopping_decel": {
+              "type": "number",
+              "description": "Nominal deceleration during stopping [m/s^2]",
+              "default": -2.0
+            },
+            "nom_stopping_jerk": {
+              "type": "number",
+              "description": "Nominal jerk during stopping [m/s^3]",
+              "default": -3.0
+            },
+            "max_stopping_decel": {
+              "type": "number",
+              "description": "Maximum deceleration during stopping [m/s^2]",
+              "default": -4.0
+            },
+            "max_stopping_jerk": {
+              "type": "number",
+              "description": "Maximum jerk during stopping [m/s^3]",
+              "default": -8.0
+            },
+            "on_time_buffer": {
+              "type": "number",
+              "description": "Duration an obstacle must be detected before stopping [s]",
+              "default": 0.5,
+              "minimum": 0.0
+            },
+            "off_time_buffer": {
+              "type": "number",
+              "description": "Duration an obstacle must be detected before stopping [s]",
+              "default": 1.0,
+              "minimum": 0.0
+            },


planning/autoware_trajectory_modifier/src/trajectory_modifier_plugins/obstacle_stop.cpp

+bool ObstacleStop::modify_trajectory(TrajectoryPoints & traj_points)
+{
+  if (!enabled_ || !is_trajectory_modification_required(traj_points)) return false;
+
+  if (!nearest_collision_point_) return false;
+
+  RCLCPP_WARN_THROTTLE(
+    get_node_ptr()->get_logger(), *get_clock(), 500,
+    "[TM ObstacleStop] Detected collision point at arc length %f m",
+    nearest_collision_point_->arc_length);
+
+  if (!params_.enable_stop) return false;
+
+  return set_stop_point(traj_points);
+}


planning/autoware_trajectory_modifier/src/trajectory_modifier_plugins/obstacle_stop.cpp

+    geometry_msgs::msg::TransformStamped transform_stamped;
+    try {
+      transform_stamped = data_->tf_buffer.lookupTransform(
+        "map", pointcloud->header.frame_id, pointcloud->header.stamp,
+        rclcpp::Duration::from_seconds(0.1));
+    } catch (tf2::TransformException & e) {
+      RCLCPP_WARN(get_node_ptr()->get_logger(), "no transform found for pointcloud: %s", e.what());
+    }
+
+    Eigen::Affine3f isometry = tf2::transformToEigen(transform_stamped.transform).cast<float>();
+    autoware_utils::transform_pointcloud(*filtered_pointcloud, *filtered_pointcloud, isometry);


planning/autoware_trajectory_modifier/src/trajectory_modifier_plugins/obstacle_stop.cpp

+    Marker marker = autoware_utils::create_default_marker(
+      "map", get_clock()->now(), ns, id, Marker::LINE_LIST,
+      autoware_utils::create_marker_scale(0.1, 0.1, 0.1), color);
+    marker.lifetime = rclcpp::Duration::from_seconds(0.2);
+
+    for (const auto & p : polygon.outer()) {
+      marker.points.push_back(autoware_utils_geometry::create_point(p.x(), p.y(), ego_z));
+    }
+    if (!marker.points.empty()) {
+      marker.points.push_back(marker.points.front());
+    }
+    marker_array.markers.push_back(marker);


planning/autoware_trajectory_modifier/src/trajectory_modifier.cpp

+  // Check if the plugin is already instantiated
+  auto it = std::find_if(
+    plugins_.begin(), plugins_.end(), [&](const auto & p) { return p->get_name() == name; });
+  if (it != plugins_.end()) {
+    RCLCPP_WARN(
+      this->get_logger(), "The plugin '%s' is already in the plugins list.", name.c_str());
+    return;


planning/autoware_trajectory_modifier/src/trajectory_modifier_utils/obstacle_stop_utils.cpp

+    std::vector<ObjectType> object_types;
+    object_types.reserve(object_type_strings.size());
+    for (const auto & object_type_string : object_type_strings) {
+      object_types.push_back(string_to_object_type.at(object_type_string));


planning/autoware_trajectory_modifier/param/trajectory_modifier_parameter_struct.yaml

+    default_value: false
+    description: Enable the use of stop point fixer
+    read_only: false
+  use_obstacle_stop:
+    type: bool
+    default_value: false


planning/autoware_trajectory_modifier/src/trajectory_modifier_plugins/obstacle_stop.cpp

+
+bool ObstacleStop::is_trajectory_modification_required(const TrajectoryPoints & traj_points)
+{
+  debug_data_ = DebugData();


...ng/autoware_trajectory_modifier/include/autoware/trajectory_modifier/trajectory_modifier.hpp

 #include "autoware/trajectory_modifier/trajectory_modifier_structs.hpp"

+#include <autoware_trajectory_modifier/trajectory_modifier_param.hpp>
+#include <autoware_utils/ros/polling_subscriber.hpp>


mkquda and others added 5 commits March 17, 2026 11:14

fix(trajectory_modifier): fix node crash (#2764)

4fd408f

check for null ptr Signed-off-by: Mohammad Alqudah <alqudah.mohammad@tier4.jp>

fix stop_point_fixer_integration_test

41c9509

Signed-off-by: Mohammad Alqudah <alqudah.mohammad@tier4.jp>

mkquda requested review from SakodaShintaro, danielsanchezaran, go-sakayori and yukkysaito as code owners March 17, 2026 05:40

github-project-automation bot moved this to To Triage in Software Working Group Mar 17, 2026

github-project-automation bot added this to Software Working Group Mar 17, 2026

github-actions bot added the component:planning Route planning, decision-making, and navigation. (auto-assigned) label Mar 17, 2026

mkquda added the run:build-and-test-differential Mark to enable build-and-test-differential workflow. (used-by-ci) label Mar 17, 2026

mkquda assigned danielsanchezaran Mar 17, 2026

danielsanchezaran requested a review from Copilot March 17, 2026 08:32

Copilot started reviewing on behalf of danielsanchezaran March 17, 2026 08:32 View session

Copilot AI reviewed Mar 17, 2026

View reviewed changes

mkquda added 2 commits March 18, 2026 08:54

feat(trajectory_modifier): add parameter to enable/disable stopping b…

2f9d5a7

…ehavior (#2783) add parameter to enable/disable stopping Signed-off-by: Mohammad <alqudah.mohammad@tier4.jp>

fix(trajectory_modifier): add missing checks for obstacle stop flags (#…

8ac69cf

…2785) add check for use_objects and use_pointcloud flags Signed-off-by: Mohammad <alqudah.mohammad@tier4.jp>

maxime-clem reviewed Mar 19, 2026

View reviewed changes

danielsanchezaran requested changes Mar 19, 2026

View reviewed changes

danielsanchezaran reviewed Mar 19, 2026

View reviewed changes

danielsanchezaran requested changes Mar 19, 2026

View reviewed changes

danielsanchezaran reviewed Mar 19, 2026

View reviewed changes

danielsanchezaran requested a review from Copilot March 19, 2026 02:51

Copilot AI reviewed Mar 19, 2026

View reviewed changes

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

feat(trajectory_modifier): add obstacle stop feature to trajectory modifier#12338

feat(trajectory_modifier): add obstacle stop feature to trajectory modifier#12338
mkquda wants to merge 7 commits intoautowarefoundation:mainfrom
tier4:T4DEV-49131-add-obstacle-stop-to-trajectory-modifier

mkquda commented Mar 17, 2026

Uh oh!

github-actions bot commented Mar 17, 2026 •

edited

Loading

Uh oh!

codecov bot commented Mar 17, 2026 •

edited

Loading

Uh oh!

Copilot AI left a comment

Uh oh!

maxime-clem Mar 19, 2026

Uh oh!

danielsanchezaran left a comment

Uh oh!

danielsanchezaran commented Mar 19, 2026

Uh oh!

danielsanchezaran left a comment

Uh oh!

danielsanchezaran left a comment

Uh oh!

danielsanchezaran left a comment

Uh oh!

Copilot AI left a comment

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

4 participants

		@@ -39,10 +44,13 @@ TrajectoryModifier::TrajectoryModifier(const rclcpp::NodeOptions & options)

		time_keeper_ = std::make_shared<autoware_utils_debug::TimeKeeper>();

Conversation

mkquda commented Mar 17, 2026

Description

Changes

Related links

How was this PR tested?

Notes for reviewers

Interface changes

ROS Parameter Changes

Effects on system behavior

Uh oh!

github-actions bot commented Mar 17, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

codecov bot commented Mar 17, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Codecov Report

Uh oh!

Copilot AI left a comment

Choose a reason for hiding this comment

Pull request overview

Reviewed changes

Uh oh!

maxime-clem Mar 19, 2026

Choose a reason for hiding this comment

Uh oh!

danielsanchezaran left a comment

Choose a reason for hiding this comment

Uh oh!

danielsanchezaran commented Mar 19, 2026

Uh oh!

danielsanchezaran left a comment

Choose a reason for hiding this comment

Uh oh!

danielsanchezaran left a comment

Choose a reason for hiding this comment

Uh oh!

danielsanchezaran left a comment

Choose a reason for hiding this comment

Uh oh!

Copilot AI left a comment

Choose a reason for hiding this comment

Pull request overview

Reviewed changes

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

4 participants

github-actions bot commented Mar 17, 2026 •

edited

Loading

codecov bot commented Mar 17, 2026 •

edited

Loading