diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml index ae43c2d81efb..d0c34ea01b2c 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml @@ -160,6 +160,7 @@ + diff --git a/planning/autoware_obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml b/planning/autoware_obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml index 8b338e9d0cd9..e797faaff9aa 100644 --- a/planning/autoware_obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml +++ b/planning/autoware_obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml @@ -37,6 +37,7 @@ motorcycle: true bicycle: true pedestrian: true + pointcloud: false cruise_obstacle_type: inside: @@ -68,8 +69,17 @@ motorcycle: true bicycle: true pedestrian: true + pointcloud: false behavior_determination: + pointcloud_search_radius: 5.0 + pointcloud_voxel_grid_x: 0.05 + pointcloud_voxel_grid_y: 0.05 + pointcloud_voxel_grid_z: 100000.0 + pointcloud_cluster_tolerance: 1.0 + pointcloud_min_cluster_size: 1 + pointcloud_max_cluster_size: 100000 + decimate_trajectory_step_length : 2.0 # longitudinal step length to calculate trajectory polygon for collision checking prediction_resampling_time_interval: 0.1 diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/common_structs.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/common_structs.hpp index e4ae670ef6b9..fc8a0a96ade4 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/common_structs.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/common_structs.hpp @@ -58,15 +58,15 @@ struct Obstacle const geometry_msgs::msg::Pose & arg_pose, const double ego_to_obstacle_distance, const double lat_dist_from_obstacle_to_traj) : stamp(arg_stamp), + ego_to_obstacle_distance(ego_to_obstacle_distance), + lat_dist_from_obstacle_to_traj(lat_dist_from_obstacle_to_traj), pose(arg_pose), orientation_reliable(true), twist(object.kinematics.initial_twist_with_covariance.twist), twist_reliable(true), classification(object.classification.at(0)), uuid(autoware::universe_utils::toHexString(object.object_id)), - shape(object.shape), - ego_to_obstacle_distance(ego_to_obstacle_distance), - lat_dist_from_obstacle_to_traj(lat_dist_from_obstacle_to_traj) + shape(object.shape) { predicted_paths.clear(); for (const auto & path : object.kinematics.predicted_paths) { @@ -74,9 +74,26 @@ struct Obstacle } } - Polygon2d toPolygon() const { return autoware::universe_utils::toPolygon2d(pose, shape); } + Obstacle( + const rclcpp::Time & arg_stamp, + const std::optional & stop_collision_point, + const std::optional & slow_down_front_collision_point, + const std::optional & slow_down_back_collision_point, + const double ego_to_obstacle_distance, const double lat_dist_from_obstacle_to_traj) + : stamp(arg_stamp), + ego_to_obstacle_distance(ego_to_obstacle_distance), + lat_dist_from_obstacle_to_traj(lat_dist_from_obstacle_to_traj), + stop_collision_point(stop_collision_point), + slow_down_front_collision_point(slow_down_front_collision_point), + slow_down_back_collision_point(slow_down_back_collision_point) + { + } rclcpp::Time stamp; // This is not the current stamp, but when the object was observed. + double ego_to_obstacle_distance; + double lat_dist_from_obstacle_to_traj; + + // for PredictedObject geometry_msgs::msg::Pose pose; // interpolated with the current stamp bool orientation_reliable; Twist twist; @@ -85,8 +102,11 @@ struct Obstacle std::string uuid; Shape shape; std::vector predicted_paths; - double ego_to_obstacle_distance; - double lat_dist_from_obstacle_to_traj; + + // for PointCloud + std::optional stop_collision_point; + std::optional slow_down_front_collision_point; + std::optional slow_down_back_collision_point; }; struct TargetObstacleInterface diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/node.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/node.hpp index dd9a6c388067..b96605f57b94 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/node.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/node.hpp @@ -27,6 +27,9 @@ #include #include +#include +#include + #include #include #include @@ -58,16 +61,27 @@ class ObstacleCruisePlannerNode : public rclcpp::Node std::vector convertToObstacles( const Odometry & odometry, const PredictedObjects & objects, const std::vector & traj_points) const; + std::vector convertToObstacles( + const Odometry & odometry, const PointCloud2 & pointcloud, + const std::vector & traj_points, + const std_msgs::msg::Header & traj_header) const; std::tuple, std::vector, std::vector> - determineEgoBehaviorAgainstObstacles( + determineEgoBehaviorAgainstPredictedObjectObstacles( const Odometry & odometry, const PredictedObjects & objects, const std::vector & traj_points, const std::vector & obstacles); + std::tuple, std::vector, std::vector> + determineEgoBehaviorAgainstPointCloudObstacles( + const Odometry & odometry, const std::vector & traj_points, + const std::vector & obstacles); std::vector decimateTrajectoryPoints( const Odometry & odometry, const std::vector & traj_points) const; - std::optional createStopObstacle( + std::optional createStopObstacleForPredictedObject( const Odometry & odometry, const std::vector & traj_points, const std::vector & traj_polys, const Obstacle & obstacle, const double precise_lateral_dist) const; + std::optional createStopObstacleForPointCloud( + const std::vector & traj_points, const Obstacle & obstacle, + const double precise_lateral_dist) const; bool isStopObstacle(const uint8_t label) const; bool isInsideCruiseObstacle(const uint8_t label) const; bool isOutsideCruiseObstacle(const uint8_t label) const; @@ -94,9 +108,11 @@ class ObstacleCruisePlannerNode : public rclcpp::Node double calcCollisionTimeMargin( const Odometry & odometry, const std::vector & collision_points, const std::vector & traj_points, const bool is_driving_forward) const; - std::optional createSlowDownObstacle( + std::optional createSlowDownObstacleForPredictedObject( const Odometry & odometry, const std::vector & traj_points, const Obstacle & obstacle, const double precise_lat_dist); + std::optional createSlowDownObstacleForPointCloud( + const Obstacle & obstacle, const double precise_lat_dist); PlannerData createPlannerData( const Odometry & odometry, const AccelWithCovarianceStamped & acc, const std::vector & traj_points) const; @@ -116,6 +132,8 @@ class ObstacleCruisePlannerNode : public rclcpp::Node bool enable_debug_info_; bool enable_calculation_time_info_; + bool use_pointcloud_for_stop_; + bool use_pointcloud_for_slow_down_; double min_behavior_stop_margin_; bool enable_approaching_on_curve_; double additional_safe_distance_margin_on_curve_; @@ -149,9 +167,14 @@ class ObstacleCruisePlannerNode : public rclcpp::Node this, "~/input/odometry"}; autoware::universe_utils::InterProcessPollingSubscriber objects_sub_{ this, "~/input/objects"}; + autoware::universe_utils::InterProcessPollingSubscriber pointcloud_sub_{ + this, "~/input/pointcloud"}; autoware::universe_utils::InterProcessPollingSubscriber acc_sub_{ this, "~/input/acceleration"}; + std::unique_ptr tf_buffer_; + std::shared_ptr tf_listener_{nullptr}; + // Vehicle Parameters VehicleInfo vehicle_info_; @@ -182,6 +205,13 @@ class ObstacleCruisePlannerNode : public rclcpp::Node void onParam(const std::vector & parameters); double decimate_trajectory_step_length; + double pointcloud_search_radius; + double pointcloud_voxel_grid_x; + double pointcloud_voxel_grid_y; + double pointcloud_voxel_grid_z; + double pointcloud_cluster_tolerance; + int pointcloud_min_cluster_size; + int pointcloud_max_cluster_size; // hysteresis for stop and cruise double obstacle_velocity_threshold_from_cruise_to_stop; double obstacle_velocity_threshold_from_stop_to_cruise; @@ -271,6 +301,7 @@ class ObstacleCruisePlannerNode : public rclcpp::Node bool is_driving_forward_{true}; bool enable_slow_down_planning_{false}; + bool use_pointcloud_{false}; std::vector prev_closest_stop_obstacles_{}; diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp index 0641d076bb3f..3679c253e2bb 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp @@ -63,12 +63,13 @@ class PlannerInterface void setParam( const bool enable_debug_info, const bool enable_calculation_time_info, - const double min_behavior_stop_margin, const double enable_approaching_on_curve, - const double additional_safe_distance_margin_on_curve, + const bool use_pointcloud, const double min_behavior_stop_margin, + const double enable_approaching_on_curve, const double additional_safe_distance_margin_on_curve, const double min_safe_distance_margin_on_curve, const bool suppress_sudden_obstacle_stop) { enable_debug_info_ = enable_debug_info; enable_calculation_time_info_ = enable_calculation_time_info; + use_pointcloud_ = use_pointcloud; min_behavior_stop_margin_ = min_behavior_stop_margin; enable_approaching_on_curve_ = enable_approaching_on_curve; additional_safe_distance_margin_on_curve_ = additional_safe_distance_margin_on_curve; @@ -117,6 +118,7 @@ class PlannerInterface // Parameters bool enable_debug_info_{false}; bool enable_calculation_time_info_{false}; + bool use_pointcloud_{false}; LongitudinalInfo longitudinal_info_; double min_behavior_stop_margin_; bool enable_approaching_on_curve_; diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/type_alias.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/type_alias.hpp index 5721efd051f6..03e536ce238c 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/type_alias.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/type_alias.hpp @@ -27,6 +27,7 @@ #include "geometry_msgs/msg/transform_stamped.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" +#include "sensor_msgs/msg/point_cloud2.hpp" #include "tier4_debug_msgs/msg/float32_stamped.hpp" #include "tier4_planning_msgs/msg/stop_factor.hpp" #include "tier4_planning_msgs/msg/stop_reason_array.hpp" @@ -35,6 +36,9 @@ #include "tier4_planning_msgs/msg/velocity_limit_clear_command.hpp" #include "visualization_msgs/msg/marker_array.hpp" +#include +#include + using autoware::vehicle_info_utils::VehicleInfo; using autoware_adapi_v1_msgs::msg::PlanningBehavior; using autoware_adapi_v1_msgs::msg::VelocityFactor; @@ -50,6 +54,7 @@ using geometry_msgs::msg::AccelStamped; using geometry_msgs::msg::AccelWithCovarianceStamped; using geometry_msgs::msg::Twist; using nav_msgs::msg::Odometry; +using sensor_msgs::msg::PointCloud2; using tier4_debug_msgs::msg::Float32Stamped; using tier4_planning_msgs::msg::StopFactor; using tier4_planning_msgs::msg::StopReason; @@ -63,4 +68,6 @@ namespace bg = boost::geometry; using autoware::universe_utils::Point2d; using autoware::universe_utils::Polygon2d; +using PointCloud = pcl::PointCloud; + #endif // AUTOWARE__OBSTACLE_CRUISE_PLANNER__TYPE_ALIAS_HPP_ diff --git a/planning/autoware_obstacle_cruise_planner/src/node.cpp b/planning/autoware_obstacle_cruise_planner/src/node.cpp index f6e7c2bb43b3..c8f962c3ad71 100644 --- a/planning/autoware_obstacle_cruise_planner/src/node.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/node.cpp @@ -23,8 +23,15 @@ #include "autoware/universe_utils/ros/update_param.hpp" #include "object_recognition_utils/predicted_path_utils.hpp" +#include +#include + #include +#include +#include +#include + #include #include @@ -194,6 +201,15 @@ std::vector resampleTrajectoryPoints( return autoware::motion_utils::convertToTrajectoryPointArray(resampled_traj); } +geometry_msgs::msg::Point toGeomPoint(const pcl::PointXYZ & point) +{ + geometry_msgs::msg::Point geom_point; + geom_point.x = point.x; + geom_point.y = point.y; + geom_point.z = point.z; + return geom_point; +} + geometry_msgs::msg::Point toGeomPoint(const autoware::universe_utils::Point2d & point) { geometry_msgs::msg::Point geom_point; @@ -216,6 +232,12 @@ bool isLowerConsideringHysteresis( } return false; } + +template +void concatenate(std::vector & first, const std::vector & last) +{ + first.insert(first.end(), last.begin(), last.end()); +} } // namespace namespace autoware::motion_planning @@ -225,6 +247,20 @@ ObstacleCruisePlannerNode::BehaviorDeterminationParam::BehaviorDeterminationPara { // behavior determination decimate_trajectory_step_length = node.declare_parameter("behavior_determination.decimate_trajectory_step_length"); + pointcloud_search_radius = + node.declare_parameter("behavior_determination.pointcloud_search_radius"); + pointcloud_voxel_grid_x = + node.declare_parameter("behavior_determination.pointcloud_voxel_grid_x"); + pointcloud_voxel_grid_y = + node.declare_parameter("behavior_determination.pointcloud_voxel_grid_y"); + pointcloud_voxel_grid_z = + node.declare_parameter("behavior_determination.pointcloud_voxel_grid_z"); + pointcloud_cluster_tolerance = + node.declare_parameter("behavior_determination.pointcloud_cluster_tolerance"); + pointcloud_min_cluster_size = + node.declare_parameter("behavior_determination.pointcloud_min_cluster_size"); + pointcloud_max_cluster_size = + node.declare_parameter("behavior_determination.pointcloud_max_cluster_size"); obstacle_velocity_threshold_from_cruise_to_stop = node.declare_parameter( "behavior_determination.obstacle_velocity_threshold_from_cruise_to_stop"); obstacle_velocity_threshold_from_stop_to_cruise = node.declare_parameter( @@ -284,6 +320,21 @@ void ObstacleCruisePlannerNode::BehaviorDeterminationParam::onParam( autoware::universe_utils::updateParam( parameters, "behavior_determination.decimate_trajectory_step_length", decimate_trajectory_step_length); + autoware::universe_utils::updateParam( + parameters, "behavior_determination.pointcloud_search_radius", pointcloud_search_radius); + autoware::universe_utils::updateParam( + parameters, "behavior_determination.pointcloud_voxel_grid_x", pointcloud_voxel_grid_x); + autoware::universe_utils::updateParam( + parameters, "behavior_determination.pointcloud_voxel_grid_y", pointcloud_voxel_grid_y); + autoware::universe_utils::updateParam( + parameters, "behavior_determination.pointcloud_voxel_grid_z", pointcloud_voxel_grid_z); + autoware::universe_utils::updateParam( + parameters, "behavior_determination.pointcloud_cluster_tolerance", + pointcloud_cluster_tolerance); + autoware::universe_utils::updateParam( + parameters, "behavior_determination.pointcloud_min_cluster_size", pointcloud_min_cluster_size); + autoware::universe_utils::updateParam( + parameters, "behavior_determination.pointcloud_max_cluster_size", pointcloud_max_cluster_size); autoware::universe_utils::updateParam( parameters, "behavior_determination.crossing_obstacle.obstacle_velocity_threshold", crossing_obstacle_velocity_threshold); @@ -387,6 +438,10 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & debug_slow_down_planning_info_pub_ = create_publisher("~/debug/slow_down_planning_info", 1); + // tf listener + tf_buffer_ = std::make_unique(get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); + const auto longitudinal_info = LongitudinalInfo(*this); ego_nearest_param_ = EgoNearestParam(*this); @@ -395,6 +450,11 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & enable_calculation_time_info_ = declare_parameter("common.enable_calculation_time_info"); enable_slow_down_planning_ = declare_parameter("common.enable_slow_down_planning"); + use_pointcloud_for_stop_ = declare_parameter("common.stop_obstacle_type.pointcloud"); + use_pointcloud_for_slow_down_ = + declare_parameter("common.slow_down_obstacle_type.pointcloud"); + use_pointcloud_ = use_pointcloud_for_stop_ || use_pointcloud_for_slow_down_; + behavior_determination_param_ = BehaviorDeterminationParam(*this); { // planning algorithm @@ -422,7 +482,7 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & suppress_sudden_obstacle_stop_ = declare_parameter("common.suppress_sudden_obstacle_stop"); planner_ptr_->setParam( - enable_debug_info_, enable_calculation_time_info_, min_behavior_stop_margin_, + enable_debug_info_, enable_calculation_time_info_, use_pointcloud_, min_behavior_stop_margin_, enable_approaching_on_curve_, additional_safe_distance_margin_on_curve_, min_safe_distance_margin_on_curve_, suppress_sudden_obstacle_stop_); } @@ -476,7 +536,7 @@ rcl_interfaces::msg::SetParametersResult ObstacleCruisePlannerNode::onParam( min_safe_distance_margin_on_curve_); planner_ptr_->setParam( - enable_debug_info_, enable_calculation_time_info_, min_behavior_stop_margin_, + enable_debug_info_, enable_calculation_time_info_, use_pointcloud_, min_behavior_stop_margin_, enable_approaching_on_curve_, additional_safe_distance_margin_on_curve_, min_safe_distance_margin_on_curve_, suppress_sudden_obstacle_stop_); @@ -495,13 +555,13 @@ void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr ms { const auto ego_odom_ptr = ego_odom_sub_.takeData(); const auto objects_ptr = objects_sub_.takeData(); + const auto pointcloud_ptr = use_pointcloud_ ? pointcloud_sub_.takeData() : nullptr; const auto acc_ptr = acc_sub_.takeData(); - if (!ego_odom_ptr || !objects_ptr || !acc_ptr) { + if (!ego_odom_ptr || (!objects_ptr && (!use_pointcloud_ || !pointcloud_ptr)) || !acc_ptr) { return; } const auto & ego_odom = *ego_odom_ptr; - const auto & objects = *objects_ptr; const auto & acc = *acc_ptr; const auto traj_points = autoware::motion_utils::convertToTrajectoryPointArray(*msg); @@ -516,15 +576,39 @@ void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr ms const auto is_driving_forward = autoware::motion_utils::isDrivingForwardWithTwist(traj_points); is_driving_forward_ = is_driving_forward ? is_driving_forward.value() : is_driving_forward_; - // 1. Convert predicted objects to obstacles which are - // (1) with a proper label - // (2) in front of ego - // (3) not too far from trajectory - const auto target_obstacles = convertToObstacles(ego_odom, objects, traj_points); + const auto & [stop_obstacles, cruise_obstacles, slow_down_obstacles] = [&]() { + std::vector stop_obstacles; + std::vector cruise_obstacles; + std::vector slow_down_obstacles; + if (objects_ptr) { + // 1. Convert predicted objects to obstacles which are + // (1) with a proper label + // (2) in front of ego + // (3) not too far from trajectory + const auto target_obstacles = convertToObstacles(ego_odom, *objects_ptr, traj_points); + + // 2. Determine ego's behavior against each obstacle from stop, cruise and slow down. + const auto & [stop_object_obstacles, cruise_object_obstacles, slow_down_object_obstacles] = + determineEgoBehaviorAgainstPredictedObjectObstacles( + ego_odom, *objects_ptr, traj_points, target_obstacles); + + concatenate(stop_obstacles, stop_object_obstacles); + concatenate(cruise_obstacles, cruise_object_obstacles); + concatenate(slow_down_obstacles, slow_down_object_obstacles); + } + if (pointcloud_ptr && !pointcloud_ptr->data.empty()) { + const auto target_obstacles = + convertToObstacles(ego_odom, *pointcloud_ptr, traj_points, msg->header); + + const auto & [stop_pc_obstacles, cruise_pc_obstacles, slow_down_pc_obstacles] = + determineEgoBehaviorAgainstPointCloudObstacles(ego_odom, traj_points, target_obstacles); - // 2. Determine ego's behavior against each obstacle from stop, cruise and slow down. - const auto & [stop_obstacles, cruise_obstacles, slow_down_obstacles] = - determineEgoBehaviorAgainstObstacles(ego_odom, objects, traj_points, target_obstacles); + concatenate(stop_obstacles, stop_pc_obstacles); + concatenate(cruise_obstacles, cruise_pc_obstacles); + concatenate(slow_down_obstacles, slow_down_pc_obstacles); + } + return std::make_tuple(stop_obstacles, cruise_obstacles, slow_down_obstacles); + }(); // 3. Create data for planning const auto planner_data = createPlannerData(ego_odom, acc, traj_points); @@ -715,6 +799,124 @@ std::vector ObstacleCruisePlannerNode::convertToObstacles( return target_obstacles; } +std::vector ObstacleCruisePlannerNode::convertToObstacles( + const Odometry & odometry, const PointCloud2 & pointcloud, + const std::vector & traj_points, const std_msgs::msg::Header & traj_header) const +{ + stop_watch_.tic(__func__); + + const auto & p = behavior_determination_param_; + + std::vector target_obstacles; + + std::optional transform_stamped{}; + try { + transform_stamped = tf_buffer_->lookupTransform( + traj_header.frame_id, pointcloud.header.frame_id, pointcloud.header.stamp, + rclcpp::Duration::from_seconds(0.5)); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR_STREAM( + get_logger(), "Failed to look up transform from " << traj_header.frame_id << " to " + << pointcloud.header.frame_id); + transform_stamped = std::nullopt; + } + + if (transform_stamped) { + // 1. transform pointcloud + PointCloud::Ptr pointcloud_ptr(new PointCloud); + pcl::fromROSMsg(pointcloud, *pointcloud_ptr); + const Eigen::Matrix4f transform = + tf2::transformToEigen(transform_stamped.value().transform).matrix().cast(); + pcl::transformPointCloud(*pointcloud_ptr, *pointcloud_ptr, transform); + + // 2. downsample & cluster pointcloud + PointCloud::Ptr filtered_points_ptr(new PointCloud); + pcl::VoxelGrid filter; + filter.setInputCloud(pointcloud_ptr); + filter.setLeafSize( + p.pointcloud_voxel_grid_x, p.pointcloud_voxel_grid_y, p.pointcloud_voxel_grid_z); + filter.filter(*filtered_points_ptr); + + pcl::search::KdTree::Ptr tree(new pcl::search::KdTree); + tree->setInputCloud(filtered_points_ptr); + std::vector clusters; + pcl::EuclideanClusterExtraction ec; + ec.setClusterTolerance(p.pointcloud_cluster_tolerance); + ec.setMinClusterSize(p.pointcloud_min_cluster_size); + ec.setMaxClusterSize(p.pointcloud_max_cluster_size); + ec.setSearchMethod(tree); + ec.setInputCloud(filtered_points_ptr); + ec.extract(clusters); + + const auto max_lat_margin = + std::max(p.max_lat_margin_for_stop_against_unknown, p.max_lat_margin_for_slow_down); + const size_t ego_idx = ego_nearest_param_.findIndex(traj_points, odometry.pose.pose); + + // 3. convert clusters to obstacles + for (const auto & cluster_indices : clusters) { + double ego_to_stop_collision_distance = std::numeric_limits::max(); + double ego_to_slow_down_front_collision_distance = std::numeric_limits::max(); + double ego_to_slow_down_back_collision_distance = std::numeric_limits::min(); + double lat_dist_from_obstacle_to_traj = std::numeric_limits::max(); + double ego_to_obstacle_distance = std::numeric_limits::max(); + std::optional stop_collision_point = std::nullopt; + std::optional slow_down_front_collision_point = std::nullopt; + std::optional slow_down_back_collision_point = std::nullopt; + + for (const auto & index : cluster_indices.indices) { + const auto obstacle_point = toGeomPoint(filtered_points_ptr->points[index]); + const auto current_lat_dist_from_obstacle_to_traj = + autoware::motion_utils::calcLateralOffset(traj_points, obstacle_point); + const auto min_lat_dist_to_traj_poly = + std::abs(current_lat_dist_from_obstacle_to_traj) - vehicle_info_.vehicle_width_m; + + if (min_lat_dist_to_traj_poly < max_lat_margin) { + const auto current_ego_to_obstacle_distance = + calcDistanceToFrontVehicle(traj_points, ego_idx, obstacle_point); + if (current_ego_to_obstacle_distance) { + ego_to_obstacle_distance = + std::min(ego_to_obstacle_distance, *current_ego_to_obstacle_distance); + } else { + continue; + } + + lat_dist_from_obstacle_to_traj = + std::min(lat_dist_from_obstacle_to_traj, current_lat_dist_from_obstacle_to_traj); + + if (min_lat_dist_to_traj_poly < p.max_lat_margin_for_stop_against_unknown) { + if (*current_ego_to_obstacle_distance < ego_to_stop_collision_distance) { + stop_collision_point = obstacle_point; + ego_to_stop_collision_distance = *current_ego_to_obstacle_distance; + } + } else if (min_lat_dist_to_traj_poly < p.max_lat_margin_for_slow_down) { + if (*current_ego_to_obstacle_distance < ego_to_slow_down_front_collision_distance) { + slow_down_front_collision_point = obstacle_point; + ego_to_slow_down_front_collision_distance = *current_ego_to_obstacle_distance; + } else if ( + *current_ego_to_obstacle_distance > ego_to_slow_down_back_collision_distance) { + slow_down_back_collision_point = obstacle_point; + ego_to_slow_down_back_collision_distance = *current_ego_to_obstacle_distance; + } + } + } + } + + if (stop_collision_point || slow_down_front_collision_point) { + target_obstacles.emplace_back( + pointcloud.header.stamp, stop_collision_point, slow_down_front_collision_point, + slow_down_back_collision_point, ego_to_obstacle_distance, lat_dist_from_obstacle_to_traj); + } + } + } + + const double calculation_time = stop_watch_.toc(__func__); + RCLCPP_INFO_EXPRESSION( + rclcpp::get_logger("ObstacleCruisePlanner"), enable_debug_info_, " %s := %f [ms]", __func__, + calculation_time); + + return target_obstacles; +} + bool ObstacleCruisePlannerNode::isStopObstacle(const uint8_t label) const { const auto & types = stop_obstacle_types_; @@ -760,7 +962,7 @@ bool ObstacleCruisePlannerNode::isFrontCollideObstacle( } std::tuple, std::vector, std::vector> -ObstacleCruisePlannerNode::determineEgoBehaviorAgainstObstacles( +ObstacleCruisePlannerNode::determineEgoBehaviorAgainstPredictedObjectObstacles( const Odometry & odometry, const PredictedObjects & objects, const std::vector & traj_points, const std::vector & obstacles) { @@ -778,7 +980,7 @@ ObstacleCruisePlannerNode::determineEgoBehaviorAgainstObstacles( std::vector slow_down_obstacles; slow_down_condition_counter_.resetCurrentUuids(); for (const auto & obstacle : obstacles) { - const auto obstacle_poly = obstacle.toPolygon(); + const auto obstacle_poly = autoware::universe_utils::toPolygon2d(obstacle.pose, obstacle.shape); // Calculate distance between trajectory and obstacle first double precise_lat_dist = std::numeric_limits::max(); @@ -794,14 +996,14 @@ ObstacleCruisePlannerNode::determineEgoBehaviorAgainstObstacles( cruise_obstacles.push_back(*cruise_obstacle); continue; } - const auto stop_obstacle = createStopObstacle( + const auto stop_obstacle = createStopObstacleForPredictedObject( odometry, decimated_traj_points, decimated_traj_polys, obstacle, precise_lat_dist); if (stop_obstacle) { stop_obstacles.push_back(*stop_obstacle); continue; } - const auto slow_down_obstacle = - createSlowDownObstacle(odometry, decimated_traj_points, obstacle, precise_lat_dist); + const auto slow_down_obstacle = createSlowDownObstacleForPredictedObject( + odometry, decimated_traj_points, obstacle, precise_lat_dist); if (slow_down_obstacle) { slow_down_obstacles.push_back(*slow_down_obstacle); continue; @@ -842,6 +1044,47 @@ ObstacleCruisePlannerNode::determineEgoBehaviorAgainstObstacles( return {stop_obstacles, cruise_obstacles, slow_down_obstacles}; } +std::tuple, std::vector, std::vector> +ObstacleCruisePlannerNode::determineEgoBehaviorAgainstPointCloudObstacles( + const Odometry & odometry, const std::vector & traj_points, + const std::vector & obstacles) +{ + stop_watch_.tic(__func__); + + // calculated decimated trajectory points and trajectory polygon + const auto decimated_traj_points = decimateTrajectoryPoints(odometry, traj_points); + const auto decimated_traj_polys = + createOneStepPolygons(decimated_traj_points, vehicle_info_, odometry.pose.pose); + debug_data_ptr_->detection_polygons = decimated_traj_polys; + + // determine ego's behavior from stop and slow down + std::vector stop_obstacles; + std::vector slow_down_obstacles; + for (const auto & obstacle : obstacles) { + const auto & precise_lat_dist = obstacle.lat_dist_from_obstacle_to_traj; + + // Filter obstacles for stop and slow down + const auto stop_obstacle = + createStopObstacleForPointCloud(decimated_traj_points, obstacle, precise_lat_dist); + if (stop_obstacle) { + stop_obstacles.push_back(*stop_obstacle); + continue; + } + const auto slow_down_obstacle = createSlowDownObstacleForPointCloud(obstacle, precise_lat_dist); + if (slow_down_obstacle) { + slow_down_obstacles.push_back(*slow_down_obstacle); + continue; + } + } + + const double calculation_time = stop_watch_.toc(__func__); + RCLCPP_INFO_EXPRESSION( + rclcpp::get_logger("ObstacleCruisePlanner"), enable_calculation_time_info_, " %s := %f [ms]", + __func__, calculation_time); + + return {stop_obstacles, {}, slow_down_obstacles}; +} + std::vector ObstacleCruisePlannerNode::decimateTrajectoryPoints( const Odometry & odometry, const std::vector & traj_points) const { @@ -1173,7 +1416,7 @@ ObstacleCruisePlannerNode::createCollisionPointsForOutsideCruiseObstacle( return collision_points; } -std::optional ObstacleCruisePlannerNode::createStopObstacle( +std::optional ObstacleCruisePlannerNode::createStopObstacleForPredictedObject( const Odometry & odometry, const std::vector & traj_points, const std::vector & traj_polys, const Obstacle & obstacle, const double precise_lat_dist) const @@ -1264,7 +1507,45 @@ std::optional ObstacleCruisePlannerNode::createStopObstacle( normal_vel, collision_point->first, collision_point->second}; } -std::optional ObstacleCruisePlannerNode::createSlowDownObstacle( +std::optional ObstacleCruisePlannerNode::createStopObstacleForPointCloud( + const std::vector & traj_points, const Obstacle & obstacle, + const double precise_lat_dist) const +{ + const auto & p = behavior_determination_param_; + + if (!use_pointcloud_for_stop_) { + return std::nullopt; + } + + if (!obstacle.stop_collision_point) { + return std::nullopt; + } + + const double max_lat_margin_for_stop = + (obstacle.classification.label == ObjectClassification::UNKNOWN) + ? p.max_lat_margin_for_stop_against_unknown + : p.max_lat_margin_for_stop; + + if (precise_lat_dist > std::max(max_lat_margin_for_stop, 1e-3)) { + return std::nullopt; + } + + const auto dist_to_collide_on_traj = + autoware::motion_utils::calcSignedArcLength(traj_points, 0, *obstacle.stop_collision_point); + + return StopObstacle{ + obstacle.uuid, + obstacle.stamp, + obstacle.classification, + obstacle.pose, + obstacle.shape, + {}, + {}, + *obstacle.stop_collision_point, + dist_to_collide_on_traj}; +} + +std::optional ObstacleCruisePlannerNode::createSlowDownObstacleForPredictedObject( const Odometry & odometry, const std::vector & traj_points, const Obstacle & obstacle, const double precise_lat_dist) { @@ -1315,7 +1596,7 @@ std::optional ObstacleCruisePlannerNode::createSlowDownObstacl return std::nullopt; } - const auto obstacle_poly = obstacle.toPolygon(); + const auto obstacle_poly = autoware::universe_utils::toPolygon2d(obstacle.pose, obstacle.shape); // calculate collision points with trajectory with lateral stop margin // NOTE: For additional margin, hysteresis is not divided by two. @@ -1388,6 +1669,26 @@ std::optional ObstacleCruisePlannerNode::createSlowDownObstacl precise_lat_dist, front_collision_point, back_collision_point}; } +std::optional ObstacleCruisePlannerNode::createSlowDownObstacleForPointCloud( + const Obstacle & obstacle, const double precise_lat_dist) +{ + if (!enable_slow_down_planning_ || !use_pointcloud_for_slow_down_) { + return std::nullopt; + } + + if (!obstacle.slow_down_front_collision_point) { + return std::nullopt; + } + + auto front_collision_point = *obstacle.slow_down_front_collision_point; + auto back_collision_point = + obstacle.slow_down_back_collision_point.value_or(front_collision_point); + + return SlowDownObstacle{ + obstacle.uuid, obstacle.stamp, obstacle.classification, obstacle.pose, {}, {}, + precise_lat_dist, front_collision_point, back_collision_point}; +} + void ObstacleCruisePlannerNode::checkConsistency( const rclcpp::Time & current_time, const PredictedObjects & predicted_objects, std::vector & stop_obstacles) diff --git a/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp b/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp index 516bf5579b82..0413d4bbf352 100644 --- a/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp @@ -439,7 +439,7 @@ std::vector PlannerInterface::generateStopTrajectory( double PlannerInterface::calculateMarginFromObstacleOnCurve( const PlannerData & planner_data, const StopObstacle & stop_obstacle) const { - if (!enable_approaching_on_curve_) { + if (!enable_approaching_on_curve_ || use_pointcloud_) { return longitudinal_info_.safe_distance_margin; }