From e9fbf3e572a1255d1ae99056bee3c597b74e8cbb Mon Sep 17 00:00:00 2001 From: Arjun Jagdish Ram Date: Mon, 10 Mar 2025 15:29:30 +0900 Subject: [PATCH 1/2] fix for point-cloud stop point --- .../src/obstacle_stop_module.cpp | 15 ++++++++------- .../src/obstacle_stop_module.hpp | 3 ++- 2 files changed, 10 insertions(+), 8 deletions(-) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/obstacle_stop_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/obstacle_stop_module.cpp index f739bed1f1d69..8378e21f6e4ca 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/obstacle_stop_module.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/obstacle_stop_module.cpp @@ -230,6 +230,7 @@ VelocityPlanningResult ObstacleStopModule::plan( auto stop_obstacles_for_point_cloud = filter_stop_obstacle_for_point_cloud( planner_data->current_odometry, raw_trajectory_points, decimated_traj_points, planner_data->no_ground_pointcloud, planner_data->vehicle_info_, + dist_to_bumper, planner_data->trajectory_polygon_collision_check, planner_data->find_index(raw_trajectory_points, planner_data->current_odometry.pose.pose)); @@ -268,10 +269,9 @@ std::vector ObstacleStopModule::convert_point_cloud_t std::vector stop_collision_points; - // 1. transform pointcloud pcl::PointCloud::Ptr pointcloud_ptr = std::make_shared>(pointcloud.pointcloud); - // 2. downsample & cluster pointcloud + // 1. downsample & cluster pointcloud PointCloud::Ptr filtered_points_ptr(new PointCloud); pcl::VoxelGrid filter; filter.setInputCloud(pointcloud_ptr); @@ -292,7 +292,7 @@ std::vector ObstacleStopModule::convert_point_cloud_t ec.setInputCloud(filtered_points_ptr); ec.extract(clusters); - // 3. convert clusters to obstacles + // 2. convert clusters to obstacles for (const auto & cluster_indices : clusters) { double ego_to_stop_collision_distance = std::numeric_limits::max(); double lat_dist_from_obstacle_to_traj = std::numeric_limits::max(); @@ -335,14 +335,14 @@ std::vector ObstacleStopModule::convert_point_cloud_t std::optional ObstacleStopModule::create_stop_obstacle_for_point_cloud( const std::vector & traj_points, const rclcpp::Time & stamp, - const geometry_msgs::msg::Point & stop_point) const + const geometry_msgs::msg::Point & stop_point, const double dist_to_bumper) const { if (!use_pointcloud_) { return std::nullopt; } const auto dist_to_collide_on_traj = - autoware::motion_utils::calcSignedArcLength(traj_points, 0, stop_point); + autoware::motion_utils::calcSignedArcLength(traj_points, 0, stop_point) - dist_to_bumper; const unique_identifier_msgs::msg::UUID obj_uuid; const auto & obj_uuid_str = autoware_utils::to_hex_string(obj_uuid); @@ -444,6 +444,7 @@ std::vector ObstacleStopModule::filter_stop_obstacle_for_point_clo const Odometry & odometry, const std::vector & traj_points, const std::vector & decimated_traj_points, const PlannerData::Pointcloud & point_cloud, const VehicleInfo & vehicle_info, + const double dist_to_bumper, const TrajectoryPolygonCollisionCheck & trajectory_polygon_collision_check, size_t ego_idx) { autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); @@ -467,7 +468,7 @@ std::vector ObstacleStopModule::filter_stop_obstacle_for_point_clo for (const auto & stop_point : stop_points) { // Filter obstacles for stop const auto stop_obstacle = - create_stop_obstacle_for_point_cloud(decimated_traj_points, stop_obstacle_stamp, stop_point); + create_stop_obstacle_for_point_cloud(decimated_traj_points, stop_obstacle_stamp, stop_point, dist_to_bumper); if (stop_obstacle) { stop_obstacles.push_back(*stop_obstacle); continue; @@ -494,7 +495,7 @@ std::vector ObstacleStopModule::filter_stop_obstacle_for_point_clo if (min_lat_dist_to_traj_poly < obstacle_filtering_param_.max_lat_margin_against_unknown) { auto stop_obstacle = *itr; stop_obstacle.dist_to_collide_on_decimated_traj = autoware::motion_utils::calcSignedArcLength( - decimated_traj_points, 0, stop_obstacle.collision_point); + decimated_traj_points, 0, stop_obstacle.collision_point) - dist_to_bumper; past_stop_obstacles.push_back(stop_obstacle); } diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/obstacle_stop_module.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/obstacle_stop_module.hpp index 10a29edd07990..9eb906dc3ad0d 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/obstacle_stop_module.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/obstacle_stop_module.hpp @@ -132,6 +132,7 @@ class ObstacleStopModule : public PluginModuleInterface const Odometry & odometry, const std::vector & traj_points, const std::vector & decimated_traj_points, const PlannerData::Pointcloud & point_cloud, const VehicleInfo & vehicle_info, + const double dist_to_bumper, const TrajectoryPolygonCollisionCheck & trajectory_polygon_collision_check, size_t ego_idx); std::optional plan_stop( @@ -188,7 +189,7 @@ class ObstacleStopModule : public PluginModuleInterface std::optional create_stop_obstacle_for_point_cloud( const std::vector & traj_points, const rclcpp::Time & stamp, - const geometry_msgs::msg::Point & stop_point) const; + const geometry_msgs::msg::Point & stop_point, const double dist_to_bumper) const; std::optional> create_collision_point_for_outside_stop_obstacle( From 11467ffb89533193ba6d91e7100b294a312148c1 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 10 Mar 2025 06:35:24 +0000 Subject: [PATCH 2/2] style(pre-commit): autofix --- .../src/obstacle_stop_module.cpp | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/obstacle_stop_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/obstacle_stop_module.cpp index 8378e21f6e4ca..96f13c6f30fdc 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/obstacle_stop_module.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/obstacle_stop_module.cpp @@ -229,8 +229,7 @@ VelocityPlanningResult ObstacleStopModule::plan( // 4. filter obstacles of point cloud auto stop_obstacles_for_point_cloud = filter_stop_obstacle_for_point_cloud( planner_data->current_odometry, raw_trajectory_points, decimated_traj_points, - planner_data->no_ground_pointcloud, planner_data->vehicle_info_, - dist_to_bumper, + planner_data->no_ground_pointcloud, planner_data->vehicle_info_, dist_to_bumper, planner_data->trajectory_polygon_collision_check, planner_data->find_index(raw_trajectory_points, planner_data->current_odometry.pose.pose)); @@ -467,8 +466,8 @@ std::vector ObstacleStopModule::filter_stop_obstacle_for_point_clo std::vector stop_obstacles; for (const auto & stop_point : stop_points) { // Filter obstacles for stop - const auto stop_obstacle = - create_stop_obstacle_for_point_cloud(decimated_traj_points, stop_obstacle_stamp, stop_point, dist_to_bumper); + const auto stop_obstacle = create_stop_obstacle_for_point_cloud( + decimated_traj_points, stop_obstacle_stamp, stop_point, dist_to_bumper); if (stop_obstacle) { stop_obstacles.push_back(*stop_obstacle); continue; @@ -494,8 +493,10 @@ std::vector ObstacleStopModule::filter_stop_obstacle_for_point_clo if (min_lat_dist_to_traj_poly < obstacle_filtering_param_.max_lat_margin_against_unknown) { auto stop_obstacle = *itr; - stop_obstacle.dist_to_collide_on_decimated_traj = autoware::motion_utils::calcSignedArcLength( - decimated_traj_points, 0, stop_obstacle.collision_point) - dist_to_bumper; + stop_obstacle.dist_to_collide_on_decimated_traj = + autoware::motion_utils::calcSignedArcLength( + decimated_traj_points, 0, stop_obstacle.collision_point) - + dist_to_bumper; past_stop_obstacles.push_back(stop_obstacle); }