From e409c6bd8be1b6c25abfe9095573171b36200754 Mon Sep 17 00:00:00 2001 From: Tomohito ANDO Date: Fri, 19 Aug 2022 09:56:51 +0900 Subject: [PATCH] feat(run_out): add lateral nearest points filter (#1527) * feat(run_out): add lateral nearest points filter Signed-off-by: Tomohito Ando * chore: update documents Signed-off-by: Tomohito Ando * chore: pre-commit fix Signed-off-by: Tomohito Ando * chore: fix typo Signed-off-by: Tomohito Ando Signed-off-by: Tomohito Ando --- .../run_out.param.yaml | 1 + .../scene_module/run_out/dynamic_obstacle.hpp | 12 +- .../run-out-design.md | 17 +-- .../scene_module/run_out/dynamic_obstacle.cpp | 116 +++++++++++++++++- .../src/scene_module/run_out/manager.cpp | 1 + 5 files changed, 131 insertions(+), 16 deletions(-) diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/run_out.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/run_out.param.yaml index 527c00ccf3867..7bd363ac60feb 100644 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/run_out.param.yaml +++ b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/run_out.param.yaml @@ -24,6 +24,7 @@ height: 2.0 # [m] height of obstacles. used for creating dynamic obstacles from points max_prediction_time: 10.0 # [sec] create predicted path until this time time_step: 0.5 # [sec] time step for each path step. used for creating dynamic obstacles from points or objects without path + points_interval: 0.1 # [m] divide obstacle points into groups with this interval, and detect only lateral nearest point. used only for Points method # approach if ego has stopped in the front of the obstacle for a certain amount of time approaching: diff --git a/planning/behavior_velocity_planner/include/scene_module/run_out/dynamic_obstacle.hpp b/planning/behavior_velocity_planner/include/scene_module/run_out/dynamic_obstacle.hpp index 10e2fd86f61c0..4fba6baf5ab14 100644 --- a/planning/behavior_velocity_planner/include/scene_module/run_out/dynamic_obstacle.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/run_out/dynamic_obstacle.hpp @@ -16,6 +16,7 @@ #define SCENE_MODULE__RUN_OUT__DYNAMIC_OBSTACLE_HPP_ #include "behavior_velocity_planner/planner_data.hpp" +#include "utilization/path_utilization.hpp" #include "utilization/util.hpp" #include @@ -52,6 +53,7 @@ struct DynamicObstacleParam float height{2.0}; // [m] float max_prediction_time{10.0}; // [sec] float time_step{0.5}; // [sec] + float points_interval{0.1}; // [m] }; struct PoseWithRange @@ -84,7 +86,7 @@ struct DynamicObstacle struct DynamicObstacleData { PredictedObjects predicted_objects; - pcl::PointCloud compare_map_filtered_pointcloud; + pcl::PointCloud obstacle_points; PathWithLaneId path; Polygons2d detection_area_polygon; }; @@ -102,6 +104,8 @@ class DynamicObstacleCreator void setData( const PlannerData & planner_data, const PathWithLaneId & path, const Polygons2d & poly) { + std::lock_guard lock(mutex_); + // compare map filtered points are subscribed in derived class that needs points dynamic_obstacle_data_.predicted_objects = *planner_data.predicted_objects; dynamic_obstacle_data_.path = path; @@ -112,6 +116,9 @@ class DynamicObstacleCreator DynamicObstacleParam param_; rclcpp::Node & node_; DynamicObstacleData dynamic_obstacle_data_; + + // mutex for dynamic_obstacle_data_ + std::mutex mutex_; }; /** @@ -153,9 +160,6 @@ class DynamicObstacleCreatorForPoints : public DynamicObstacleCreator // tf tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; - - // mutex for compare_map_filtered_pointcloud - std::mutex mutex_; }; } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/run-out-design.md b/planning/behavior_velocity_planner/run-out-design.md index 0ced511d8aff7..9b07c204d4728 100644 --- a/planning/behavior_velocity_planner/run-out-design.md +++ b/planning/behavior_velocity_planner/run-out-design.md @@ -163,14 +163,15 @@ You can choose whether to use this feature by parameter of `slow_down_limit.enab | `margin_ahead` | double | [m] ahead margin for detection area polygon | | `margin_behind` | double | [m] behind margin for detection area polygon | -| Parameter /dynamic_obstacle | Type | Description | -| --------------------------- | ------ | ----------------------------------------------------------------------------------------------------------- | -| `min_vel_kmph` | double | [km/h] minimum velocity for dynamic obstacles | -| `max_vel_kmph` | double | [km/h] maximum velocity for dynamic obstacles | -| `diameter` | double | [m] diameter of obstacles. used for creating dynamic obstacles from points | -| `height` | double | [m] height of obstacles. used for creating dynamic obstacles from points | -| `max_prediction_time` | double | [sec] create predicted path until this time | -| `time_step` | double | [sec] time step for each path step. used for creating dynamic obstacles from points or objects without path | +| Parameter /dynamic_obstacle | Type | Description | +| --------------------------- | ------ | ----------------------------------------------------------------------------------------------------------------------------- | +| `min_vel_kmph` | double | [km/h] minimum velocity for dynamic obstacles | +| `max_vel_kmph` | double | [km/h] maximum velocity for dynamic obstacles | +| `diameter` | double | [m] diameter of obstacles. used for creating dynamic obstacles from points | +| `height` | double | [m] height of obstacles. used for creating dynamic obstacles from points | +| `max_prediction_time` | double | [sec] create predicted path until this time | +| `time_step` | double | [sec] time step for each path step. used for creating dynamic obstacles from points or objects without path | +| `points_interval` | double | [m] divide obstacle points into groups with this interval, and detect only lateral nearest point. used only for Points method | | Parameter /approaching | Type | Description | | ---------------------- | ------ | -------------------------------------------------------------------------------------------------- | diff --git a/planning/behavior_velocity_planner/src/scene_module/run_out/dynamic_obstacle.cpp b/planning/behavior_velocity_planner/src/scene_module/run_out/dynamic_obstacle.cpp index 1763ef3c66445..e7dc061fde2b1 100644 --- a/planning/behavior_velocity_planner/src/scene_module/run_out/dynamic_obstacle.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/run_out/dynamic_obstacle.cpp @@ -71,6 +71,15 @@ pcl::PointCloud applyVoxelGridFilter( return output_points; } +bool isAheadOf( + const geometry_msgs::msg::Point & target_point, const geometry_msgs::msg::Pose & base_pose) +{ + const auto longitudinal_deviation = + tier4_autoware_utils::calcLongitudinalDeviation(base_pose, target_point); + const bool is_ahead = longitudinal_deviation > 0; + return is_ahead; +} + pcl::PointCloud extractObstaclePointsWithinPolygon( const pcl::PointCloud & input_points, const Polygons2d & polys) { @@ -105,6 +114,90 @@ pcl::PointCloud extractObstaclePointsWithinPolygon( return output_points; } + +// group points with its nearest segment of path points +std::vector> groupPointsWithNearestSegmentIndex( + const pcl::PointCloud & input_points, const PathPointsWithLaneId & path_points) +{ + // assign nearest segment index to each point + std::vector> points_with_index; + points_with_index.resize(path_points.size()); + + for (const auto & p : input_points.points) { + const auto ros_point = tier4_autoware_utils::createPoint(p.x, p.y, p.z); + const size_t nearest_seg_idx = motion_utils::findNearestSegmentIndex(path_points, ros_point); + + // if the point is ahead of end of the path, index should be path.size() - 1 + if ( + nearest_seg_idx == path_points.size() - 2 && + isAheadOf(ros_point, path_points.back().point.pose)) { + points_with_index.back().push_back(p); + continue; + } + + points_with_index.at(nearest_seg_idx).push_back(p); + } + + return points_with_index; +} + +// calculate lateral nearest point from base_pose +pcl::PointXYZ calculateLateralNearestPoint( + const pcl::PointCloud & input_points, const geometry_msgs::msg::Pose & base_pose) +{ + const auto lateral_nearest_point = std::min_element( + input_points.points.begin(), input_points.points.end(), [&](const auto & p1, const auto & p2) { + const auto lateral_deviation_p1 = std::abs(tier4_autoware_utils::calcLateralDeviation( + base_pose, tier4_autoware_utils::createPoint(p1.x, p1.y, 0))); + const auto lateral_deviation_p2 = std::abs(tier4_autoware_utils::calcLateralDeviation( + base_pose, tier4_autoware_utils::createPoint(p2.x, p2.y, 0))); + + return lateral_deviation_p1 < lateral_deviation_p2; + }); + + return *lateral_nearest_point; +} + +pcl::PointCloud selectLateralNearestPoints( + const std::vector> & points_with_index, + const PathPointsWithLaneId & path_points) +{ + pcl::PointCloud lateral_nearest_points; + for (size_t idx = 0; idx < points_with_index.size(); idx++) { + if (points_with_index.at(idx).points.empty()) { + continue; + } + + lateral_nearest_points.push_back( + calculateLateralNearestPoint(points_with_index.at(idx), path_points.at(idx).point.pose)); + } + + return lateral_nearest_points; +} + +// extract lateral nearest points for nearest segment of the path +// path is interpolated with given interval +pcl::PointCloud extractLateralNearestPoints( + const pcl::PointCloud & input_points, const PathWithLaneId & path, + const float interval) +{ + // interpolate path points with given interval + PathWithLaneId interpolated_path; + if (!splineInterpolate( + path, interval, interpolated_path, rclcpp::get_logger("dynamic_obstacle_creator"))) { + return input_points; + } + + // divide points into groups according to nearest segment index + const auto points_with_index = + groupPointsWithNearestSegmentIndex(input_points, interpolated_path.points); + + // select the lateral nearest point for each group + const auto lateral_nearest_points = + selectLateralNearestPoints(points_with_index, interpolated_path.points); + + return lateral_nearest_points; +} } // namespace DynamicObstacleCreatorForObject::DynamicObstacleCreatorForObject(rclcpp::Node & node) @@ -191,7 +284,7 @@ std::vector DynamicObstacleCreatorForPoints::createDynamicObsta { std::lock_guard lock(mutex_); std::vector dynamic_obstacles; - for (const auto & point : dynamic_obstacle_data_.compare_map_filtered_pointcloud) { + for (const auto & point : dynamic_obstacle_data_.obstacle_points) { DynamicObstacle dynamic_obstacle; // create pose facing the direction of the lane @@ -233,7 +326,7 @@ void DynamicObstacleCreatorForPoints::onCompareMapFilteredPointCloud( { if (msg->data.empty()) { std::lock_guard lock(mutex_); - dynamic_obstacle_data_.compare_map_filtered_pointcloud.clear(); + dynamic_obstacle_data_.obstacle_points.clear(); return; } @@ -253,9 +346,24 @@ void DynamicObstacleCreatorForPoints::onCompareMapFilteredPointCloud( pcl::PointCloud::Ptr pc_transformed(new pcl::PointCloud); pcl::transformPointCloud(pc, *pc_transformed, affine); + // apply voxel grid filter to reduce calculation cost const auto voxel_grid_filtered_points = applyVoxelGridFilter(pc_transformed); + + // these variables are written in another callback + mutex_.lock(); + const auto detection_area_polygon = dynamic_obstacle_data_.detection_area_polygon; + const auto path = dynamic_obstacle_data_.path; + mutex_.unlock(); + + // filter obstacle points within detection area polygon + const auto detection_area_filtered_points = + extractObstaclePointsWithinPolygon(voxel_grid_filtered_points, detection_area_polygon); + + // filter points that have lateral nearest distance + const auto lateral_nearest_points = + extractLateralNearestPoints(detection_area_filtered_points, path, param_.points_interval); + std::lock_guard lock(mutex_); - dynamic_obstacle_data_.compare_map_filtered_pointcloud = extractObstaclePointsWithinPolygon( - voxel_grid_filtered_points, dynamic_obstacle_data_.detection_area_polygon); + dynamic_obstacle_data_.obstacle_points = lateral_nearest_points; } } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/scene_module/run_out/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/run_out/manager.cpp index a0b41ed5e35af..018569dab60dd 100644 --- a/planning/behavior_velocity_planner/src/scene_module/run_out/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/run_out/manager.cpp @@ -78,6 +78,7 @@ RunOutModuleManager::RunOutModuleManager(rclcpp::Node & node) p.height = node.declare_parameter(ns_do + ".height", 2.0); p.max_prediction_time = node.declare_parameter(ns_do + ".max_prediction_time", 10.0); p.time_step = node.declare_parameter(ns_do + ".time_step", 0.5); + p.points_interval = node.declare_parameter(ns_do + ".points_interval", 0.1); } {