From daf0e3a91465cf26235f42785bc5c93398901c73 Mon Sep 17 00:00:00 2001 From: Yuki TAKAGI <141538661+yuki-takagi-66@users.noreply.github.com> Date: Thu, 10 Oct 2024 21:00:07 +0900 Subject: [PATCH] feat(obstacle_cruise_planner)!: improve stop and cruise behavior for cut-in & out (#8072) (#1585) feat(obstacle_cruise_planner): improve stop and cruise behavior for cut-in & out (#8072) -------- Signed-off-by: Berkay Karaman Co-authored-by: Berkay Karaman --- .../config/obstacle_cruise_planner.param.yaml | 35 +- .../common_structs.hpp | 7 +- .../autoware/obstacle_cruise_planner/node.hpp | 32 +- .../obstacle_cruise_planner/polygon_utils.hpp | 5 + .../src/node.cpp | 545 +++++++++++++----- .../src/polygon_utils.cpp | 31 + 6 files changed, 502 insertions(+), 153 deletions(-) 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 2844ff6a54e3d..75adf228cc7ca 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 @@ -29,15 +29,26 @@ suppress_sudden_obstacle_stop: false stop_obstacle_type: - unknown: true - car: true - truck: true - bus: true - trailer: true - motorcycle: true - bicycle: true - pedestrian: true pointcloud: false + inside: + unknown: true + car: true + truck: true + bus: true + trailer: true + motorcycle: true + bicycle: true + pedestrian: true + + outside: + unknown: false + car: true + truck: true + bus: true + trailer: true + motorcycle: true + bicycle: true + pedestrian: true cruise_obstacle_type: inside: @@ -99,6 +110,12 @@ stop: max_lat_margin: 0.3 # lateral margin between the obstacles except for unknown and ego's footprint max_lat_margin_against_unknown: 0.3 # lateral margin between the unknown obstacles and ego's footprint + min_velocity_to_reach_collision_point: 2.0 # minimum velocity margin to calculate time to reach collision [m/s] + outside_obstacle: + max_lateral_time_margin: 4.0 # time threshold of lateral margin between the obstacles and ego's footprint [s] + num_of_predicted_paths: 3 # number of the highest confidence predicted path to check collision between obstacle and ego + pedestrian_deceleration_rate: 0.5 # planner perceives pedestrians will stop with this rate to avoid unnecessary stops [m/ss] + bicycle_deceleration_rate: 0.5 # planner perceives bicycles will stop with this rate to avoid unnecessary stops [m/ss] crossing_obstacle: collision_time_margin : 4.0 # time threshold of collision between obstacle adn ego for cruise or stop [s] @@ -108,6 +125,8 @@ obstacle_velocity_threshold : 3.5 # minimum velocity threshold of obstacles outside the trajectory to cruise or stop [m/s] ego_obstacle_overlap_time_threshold : 2.0 # time threshold to decide cut-in obstacle for cruise or stop [s] max_prediction_time_for_collision_check : 20.0 # prediction time to check collision between obstacle and ego + max_lateral_time_margin: 5.0 # time threshold of lateral margin between obstacle and trajectory band with ego's width [s] + num_of_predicted_paths: 3 # number of the highest confidence predicted path to check collision between obstacle and ego yield: enable_yield: true lat_distance_threshold: 5.0 # lateral margin between obstacle in neighbor lanes and trajectory band with ego's width for yielding 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 f2a475c494aba..0f35960c350a8 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,10 +58,13 @@ struct Obstacle Obstacle( const rclcpp::Time & arg_stamp, const PredictedObject & object, const geometry_msgs::msg::Pose & arg_pose, const double ego_to_obstacle_distance, - const double lat_dist_from_obstacle_to_traj) + const double lat_dist_from_obstacle_to_traj, const double longitudinal_velocity, + const double approach_velocity) : stamp(arg_stamp), ego_to_obstacle_distance(ego_to_obstacle_distance), lat_dist_from_obstacle_to_traj(lat_dist_from_obstacle_to_traj), + longitudinal_velocity(longitudinal_velocity), + approach_velocity(approach_velocity), pose(arg_pose), orientation_reliable(true), twist(object.kinematics.initial_twist_with_covariance.twist), @@ -94,6 +97,8 @@ struct Obstacle 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; + double longitudinal_velocity; + double approach_velocity; // for PredictedObject geometry_msgs::msg::Pose pose; // interpolated with the current stamp 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 35cbf31239272..629dba73e85dc 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 @@ -37,6 +37,7 @@ #include #include #include +#include #include namespace autoware::motion_planning @@ -79,24 +80,29 @@ class ObstacleCruisePlannerNode : public rclcpp::Node const Odometry & odometry, const std::vector & traj_points, const std::vector & traj_polys, const Obstacle & obstacle, const double precise_lateral_dist) const; + std::optional> + createCollisionPointForOutsideStopObstacle( + const Odometry & odometry, const std::vector & traj_points, + const std::vector & traj_polys, const Obstacle & obstacle, + const PredictedPath & resampled_predicted_path, double max_lat_margin_for_stop) const; std::optional createStopObstacleForPointCloud( const std::vector & traj_points, const Obstacle & obstacle, const double precise_lateral_dist) const; + bool isInsideStopObstacle(const uint8_t label) const; + bool isOutsideStopObstacle(const uint8_t label) const; bool isStopObstacle(const uint8_t label) const; bool isInsideCruiseObstacle(const uint8_t label) const; bool isOutsideCruiseObstacle(const uint8_t label) const; bool isCruiseObstacle(const uint8_t label) const; bool isSlowDownObstacle(const uint8_t label) const; - std::optional createCollisionPointForStopObstacle( - const std::vector & traj_points, const std::vector & traj_polys, - const Obstacle & obstacle) const; std::optional createYieldCruiseObstacle( const Obstacle & obstacle, const std::vector & traj_points); std::optional> findYieldCruiseObstacles( const std::vector & obstacles, const std::vector & traj_points); std::optional createCruiseObstacle( - const std::vector & traj_points, const std::vector & traj_polys, - const Obstacle & obstacle, const double precise_lat_dist); + const Odometry & odometry, const std::vector & traj_points, + const std::vector & traj_polys, const Obstacle & obstacle, + const double precise_lat_dist); std::optional> createCollisionPointsForInsideCruiseObstacle( const std::vector & traj_points, const std::vector & traj_polys, const Obstacle & obstacle) const; @@ -129,7 +135,9 @@ class ObstacleCruisePlannerNode : public rclcpp::Node bool isFrontCollideObstacle( const std::vector & traj_points, const Obstacle & obstacle, const size_t first_collision_idx) const; - + double calcTimeToReachCollisionPoint( + const Odometry & odometry, const geometry_msgs::msg::Point & collision_point, + const std::vector & traj_points, const double abs_ego_offset) const; bool enable_debug_info_; bool enable_calculation_time_info_; bool use_pointcloud_for_stop_; @@ -140,7 +148,8 @@ class ObstacleCruisePlannerNode : public rclcpp::Node double min_safe_distance_margin_on_curve_; bool suppress_sudden_obstacle_stop_; - std::vector stop_obstacle_types_; + std::vector inside_stop_obstacle_types_; + std::vector outside_stop_obstacle_types_; std::vector inside_cruise_obstacle_types_; std::vector outside_cruise_obstacle_types_; std::vector slow_down_obstacle_types_; @@ -226,11 +235,20 @@ class ObstacleCruisePlannerNode : public rclcpp::Node double ego_obstacle_overlap_time_threshold; double max_prediction_time_for_collision_check; double crossing_obstacle_traj_angle_threshold; + int num_of_predicted_paths_for_outside_cruise_obstacle; + int num_of_predicted_paths_for_outside_stop_obstacle; + double pedestrian_deceleration_rate; + double bicycle_deceleration_rate; // obstacle hold double stop_obstacle_hold_time_threshold; + // reach collision point + double min_velocity_to_reach_collision_point; // prediction resampling double prediction_resampling_time_interval; double prediction_resampling_time_horizon; + // max lateral time margin + double max_lat_time_margin_for_stop; + double max_lat_time_margin_for_cruise; // max lateral margin double max_lat_margin_for_stop; double max_lat_margin_for_stop_against_unknown; diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/polygon_utils.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/polygon_utils.hpp index fa6ee17d117be..f84aa26480765 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/polygon_utils.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/polygon_utils.hpp @@ -43,6 +43,11 @@ std::optional> getCollisionPoint( const Obstacle & obstacle, const bool is_driving_forward, const autoware::vehicle_info_utils::VehicleInfo & vehicle_info); +std::optional> getCollisionPoint( + const std::vector & traj_points, const size_t collision_idx, + const std::vector & collision_points, const bool is_driving_forward, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info); + std::vector getCollisionPoints( const std::vector & traj_points, const std::vector & traj_polygons, const rclcpp::Time & obstacle_stamp, const PredictedPath & predicted_path, const Shape & shape, diff --git a/planning/autoware_obstacle_cruise_planner/src/node.cpp b/planning/autoware_obstacle_cruise_planner/src/node.cpp index cbb3f98385774..f5cac52a68681 100644 --- a/planning/autoware_obstacle_cruise_planner/src/node.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/node.cpp @@ -26,8 +26,6 @@ #include #include -#include - #include #include #include @@ -72,18 +70,39 @@ std::optional calcDistanceToFrontVehicle( return ego_to_obstacle_distance; } -PredictedPath resampleHighestConfidencePredictedPath( +std::vector resampleHighestConfidencePredictedPaths( const std::vector & predicted_paths, const double time_interval, - const double time_horizon) + const double time_horizon, const size_t num_paths) { - // get highest confidence path - const auto reliable_path = std::max_element( - predicted_paths.begin(), predicted_paths.end(), - [](const PredictedPath & a, const PredictedPath & b) { return a.confidence < b.confidence; }); - - // resample - return object_recognition_utils::resamplePredictedPath( - *reliable_path, time_interval, time_horizon); + std::vector sorted_paths = predicted_paths; + + // Sort paths by descending confidence + std::sort( + sorted_paths.begin(), sorted_paths.end(), + [](const PredictedPath & a, const PredictedPath & b) { return a.confidence > b.confidence; }); + + std::vector selected_paths; + size_t path_count = 0; + + // Select paths that meet the confidence thresholds + for (const auto & path : sorted_paths) { + if (path_count < num_paths) { + selected_paths.push_back(path); + ++path_count; + } + } + + // Resample each selected path + std::vector resampled_paths; + for (const auto & path : selected_paths) { + if (path.path.size() < 2) { + continue; + } + resampled_paths.push_back( + object_recognition_utils::resamplePredictedPath(path, time_interval, time_horizon)); + } + + return resampled_paths; } double calcDiffAngleAgainstTrajectory( @@ -99,20 +118,48 @@ double calcDiffAngleAgainstTrajectory( return diff_yaw; } -std::pair projectObstacleVelocityToTrajectory( - const std::vector & traj_points, const Obstacle & obstacle) +/** + * @brief Calculates the obstacle's longitudinal and approach velocities relative to the trajectory. + * + * This function calculates the obstacle's velocity components relative to the trajectory. + * It returns the longitudinal and approach components of the obstacle's velocity + * with respect to the trajectory. Negative approach velocity indicates that the + * obstacle is getting far away from the trajectory. + * + * @param traj_points The trajectory points. + * @param obstacle_pose The current pose of the obstacle. + * @param obstacle_twist The twist (velocity) of the obstacle. + * @return A pair containing the longitudinal and approach velocity components. + */ +std::pair calculateObstacleVelocitiesRelativeToTrajectory( + const std::vector & traj_points, const geometry_msgs::msg::Pose & obstacle_pose, + const geometry_msgs::msg::Twist & obstacle_twist) { const size_t object_idx = - autoware::motion_utils::findNearestIndex(traj_points, obstacle.pose.position); - const double traj_yaw = tf2::getYaw(traj_points.at(object_idx).pose.orientation); + autoware::motion_utils::findNearestIndex(traj_points, obstacle_pose.position); + + const auto & nearest_point = traj_points.at(object_idx); + + const double traj_yaw = tf2::getYaw(nearest_point.pose.orientation); + const double obstacle_yaw = tf2::getYaw(obstacle_pose.orientation); + const Eigen::Rotation2Dd R_ego_to_obstacle( + autoware::universe_utils::normalizeRadian(obstacle_yaw - traj_yaw)); + + // Calculate the trajectory direction and the vector from the trajectory to the obstacle + const Eigen::Vector2d traj_direction(std::cos(traj_yaw), std::sin(traj_yaw)); + const Eigen::Vector2d traj_to_obstacle( + obstacle_pose.position.x - nearest_point.pose.position.x, + obstacle_pose.position.y - nearest_point.pose.position.y); - const double obstacle_yaw = tf2::getYaw(obstacle.pose.orientation); + // Determine if the obstacle is to the left or right of the trajectory using the cross product + const double cross_product = + traj_direction.x() * traj_to_obstacle.y() - traj_direction.y() * traj_to_obstacle.x(); + const int sign = (cross_product > 0) ? -1 : 1; - const Eigen::Rotation2Dd R_ego_to_obstacle(obstacle_yaw - traj_yaw); - const Eigen::Vector2d obstacle_velocity(obstacle.twist.linear.x, obstacle.twist.linear.y); + const Eigen::Vector2d obstacle_velocity(obstacle_twist.linear.x, obstacle_twist.linear.y); const Eigen::Vector2d projected_velocity = R_ego_to_obstacle * obstacle_velocity; - return std::make_pair(projected_velocity[0], projected_velocity[1]); + return std::make_pair(projected_velocity[0], sign * projected_velocity[1]); } double calcObstacleMaxLength(const Shape & shape) @@ -311,6 +358,20 @@ ObstacleCruisePlannerNode::BehaviorDeterminationParam::BehaviorDeterminationPara "behavior_determination.consider_current_pose.enable_to_consider_current_pose"); time_to_convergence = node.declare_parameter( "behavior_determination.consider_current_pose.time_to_convergence"); + min_velocity_to_reach_collision_point = node.declare_parameter( + "behavior_determination.stop.min_velocity_to_reach_collision_point"); + max_lat_time_margin_for_stop = node.declare_parameter( + "behavior_determination.stop.outside_obstacle.max_lateral_time_margin"); + max_lat_time_margin_for_cruise = node.declare_parameter( + "behavior_determination.cruise.outside_obstacle.max_lateral_time_margin"); + num_of_predicted_paths_for_outside_cruise_obstacle = node.declare_parameter( + "behavior_determination.cruise.outside_obstacle.num_of_predicted_paths"); + num_of_predicted_paths_for_outside_stop_obstacle = node.declare_parameter( + "behavior_determination.stop.outside_obstacle.num_of_predicted_paths"); + pedestrian_deceleration_rate = node.declare_parameter( + "behavior_determination.stop.outside_obstacle.pedestrian_deceleration_rate"); + bicycle_deceleration_rate = node.declare_parameter( + "behavior_determination.stop.outside_obstacle.bicycle_deceleration_rate"); } void ObstacleCruisePlannerNode::BehaviorDeterminationParam::onParam( @@ -403,6 +464,27 @@ void ObstacleCruisePlannerNode::BehaviorDeterminationParam::onParam( autoware::universe_utils::updateParam( parameters, "behavior_determination.consider_current_pose.time_to_convergence", time_to_convergence); + autoware::universe_utils::updateParam( + parameters, "behavior_determination.stop.min_velocity_to_reach_collision_point", + min_velocity_to_reach_collision_point); + autoware::universe_utils::updateParam( + parameters, "behavior_determination.stop.outside_obstacle.max_lateral_time_margin", + max_lat_time_margin_for_stop); + autoware::universe_utils::updateParam( + parameters, "behavior_determination.cruise.outside_obstacle.max_lateral_time_margin", + max_lat_time_margin_for_cruise); + autoware::universe_utils::updateParam( + parameters, "behavior_determination.cruise.outside_obstacle.num_of_predicted_paths", + num_of_predicted_paths_for_outside_cruise_obstacle); + autoware::universe_utils::updateParam( + parameters, "behavior_determination.stop.outside_obstacle.num_of_predicted_paths", + num_of_predicted_paths_for_outside_stop_obstacle); + autoware::universe_utils::updateParam( + parameters, "behavior_determination.stop.outside_obstacle.pedestrian_deceleration_rate", + pedestrian_deceleration_rate); + autoware::universe_utils::updateParam( + parameters, "behavior_determination.stop.outside_obstacle.bicycle_deceleration_rate", + bicycle_deceleration_rate); } ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & node_options) @@ -488,7 +570,8 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & } { // stop/cruise/slow down obstacle type - stop_obstacle_types_ = getTargetObjectType(*this, "common.stop_obstacle_type."); + inside_stop_obstacle_types_ = getTargetObjectType(*this, "common.stop_obstacle_type.inside."); + outside_stop_obstacle_types_ = getTargetObjectType(*this, "common.stop_obstacle_type.outside."); inside_cruise_obstacle_types_ = getTargetObjectType(*this, "common.cruise_obstacle_type.inside."); outside_cruise_obstacle_types_ = @@ -738,10 +821,21 @@ std::vector ObstacleCruisePlannerNode::convertToObstacles( const auto obj_stamp = rclcpp::Time(objects.header.stamp); const auto & p = behavior_determination_param_; + constexpr double epsilon = 1e-6; + const double max_lat_margin = std::max( + {p.max_lat_margin_for_stop, p.max_lat_margin_for_stop_against_unknown, + p.max_lat_margin_for_cruise, p.max_lat_margin_for_slow_down}); + const double max_lat_time_margin = + std::max({p.max_lat_time_margin_for_stop, p.max_lat_time_margin_for_cruise}); + const size_t ego_idx = ego_nearest_param_.findIndex(traj_points, odometry.pose.pose); + std::vector target_obstacles; for (const auto & predicted_object : objects.objects) { const auto & object_id = autoware::universe_utils::toHexString(predicted_object.object_id).substr(0, 4); + + // brkay54: When use_prediction is true, we observed wrong orientation for the object in + // scenario simulator. const auto & current_obstacle_pose = obstacle_cruise_utils::getCurrentObjectPose(predicted_object, obj_stamp, now(), true); @@ -756,8 +850,11 @@ std::vector ObstacleCruisePlannerNode::convertToObstacles( continue; } + const auto projected_vel = calculateObstacleVelocitiesRelativeToTrajectory( + traj_points, current_obstacle_pose.pose, + predicted_object.kinematics.initial_twist_with_covariance.twist); + // 2. Check if the obstacle is in front of the ego. - const size_t ego_idx = ego_nearest_param_.findIndex(traj_points, odometry.pose.pose); const auto ego_to_obstacle_distance = calcDistanceToFrontVehicle(traj_points, ego_idx, current_obstacle_pose.pose.position); if (!ego_to_obstacle_distance) { @@ -767,7 +864,8 @@ std::vector ObstacleCruisePlannerNode::convertToObstacles( continue; } - // 3. Check if rough lateral distance is smaller than the threshold + // 3. Check if rough lateral distance and time to reach trajectory are smaller than the + // threshold const double lat_dist_from_obstacle_to_traj = autoware::motion_utils::calcLateralOffset(traj_points, current_obstacle_pose.pose.position); @@ -777,19 +875,27 @@ std::vector ObstacleCruisePlannerNode::convertToObstacles( obstacle_max_length; }(); - const double max_lat_margin = std::max( - {p.max_lat_margin_for_stop, p.max_lat_margin_for_stop_against_unknown, - p.max_lat_margin_for_cruise, p.max_lat_margin_for_slow_down}); if (max_lat_margin < min_lat_dist_to_traj_poly) { - RCLCPP_INFO_EXPRESSION( - get_logger(), enable_debug_info_, - "Ignore obstacle (%s) since it is too far from the trajectory.", object_id.c_str()); - continue; + if (projected_vel.second > 0.0) { + const auto time_to_traj = + min_lat_dist_to_traj_poly / std::max(epsilon, projected_vel.second); + if (time_to_traj > max_lat_time_margin) { + RCLCPP_INFO_EXPRESSION( + get_logger(), enable_debug_info_, + "Ignore obstacle (%s) since it is too far from the trajectory.", object_id.c_str()); + continue; + } + } else { + RCLCPP_INFO_EXPRESSION( + get_logger(), enable_debug_info_, + "Ignore obstacle (%s) since it is too far from the trajectory.", object_id.c_str()); + continue; + } } const auto target_obstacle = Obstacle( - obj_stamp, predicted_object, current_obstacle_pose.pose, ego_to_obstacle_distance.value(), - lat_dist_from_obstacle_to_traj); + obj_stamp, predicted_object, current_obstacle_pose.pose, *ego_to_obstacle_distance, + lat_dist_from_obstacle_to_traj, projected_vel.first, projected_vel.second); target_obstacles.push_back(target_obstacle); } @@ -919,12 +1025,23 @@ std::vector ObstacleCruisePlannerNode::convertToObstacles( return target_obstacles; } -bool ObstacleCruisePlannerNode::isStopObstacle(const uint8_t label) const +bool ObstacleCruisePlannerNode::isInsideStopObstacle(const uint8_t label) const +{ + const auto & types = inside_stop_obstacle_types_; + return std::find(types.begin(), types.end(), label) != types.end(); +} + +bool ObstacleCruisePlannerNode::isOutsideStopObstacle(const uint8_t label) const { - const auto & types = stop_obstacle_types_; + const auto & types = outside_stop_obstacle_types_; return std::find(types.begin(), types.end(), label) != types.end(); } +bool ObstacleCruisePlannerNode::isStopObstacle(const uint8_t label) const +{ + return isInsideStopObstacle(label) || isOutsideStopObstacle(label); +} + bool ObstacleCruisePlannerNode::isInsideCruiseObstacle(const uint8_t label) const { const auto & types = inside_cruise_obstacle_types_; @@ -992,8 +1109,8 @@ ObstacleCruisePlannerNode::determineEgoBehaviorAgainstPredictedObjectObstacles( } // Filter obstacles for cruise, stop and slow down - const auto cruise_obstacle = - createCruiseObstacle(decimated_traj_points, decimated_traj_polys, obstacle, precise_lat_dist); + const auto cruise_obstacle = createCruiseObstacle( + odometry, decimated_traj_points, decimated_traj_polys, obstacle, precise_lat_dist); if (cruise_obstacle) { cruise_obstacles.push_back(*cruise_obstacle); continue; @@ -1141,8 +1258,9 @@ std::vector ObstacleCruisePlannerNode::decimateTrajectoryPoints } std::optional ObstacleCruisePlannerNode::createCruiseObstacle( - const std::vector & traj_points, const std::vector & traj_polys, - const Obstacle & obstacle, const double precise_lat_dist) + const Odometry & odometry, const std::vector & traj_points, + const std::vector & traj_polys, const Obstacle & obstacle, + const double precise_lat_dist) { const auto & object_id = obstacle.uuid.substr(0, 4); const auto & p = behavior_determination_param_; @@ -1152,12 +1270,24 @@ std::optional ObstacleCruisePlannerNode::createCruiseObstacle( if (!isCruiseObstacle(obstacle.classification.label) || !is_driving_forward_) { return std::nullopt; } - if (p.max_lat_margin_for_cruise < precise_lat_dist) { + + if (obstacle.longitudinal_velocity < 0.0) { RCLCPP_INFO_EXPRESSION( get_logger(), enable_debug_info_, - "[Cruise] Ignore obstacle (%s) since it's far from trajectory.", object_id.c_str()); + "[Cruise] Ignore obstacle (%s) since it's driving in opposite direction.", object_id.c_str()); return std::nullopt; } + + if (p.max_lat_margin_for_cruise < precise_lat_dist) { + const auto time_to_traj = precise_lat_dist / std::max(1e-6, obstacle.approach_velocity); + if (time_to_traj > p.max_lat_time_margin_for_cruise) { + RCLCPP_INFO_EXPRESSION( + get_logger(), enable_debug_info_, + "[Cruise] Ignore obstacle (%s) since it's far from trajectory.", object_id.c_str()); + return std::nullopt; + } + } + if (isObstacleCrossing(traj_points, obstacle)) { RCLCPP_INFO_EXPRESSION( get_logger(), enable_debug_info_, @@ -1173,15 +1303,23 @@ std::optional ObstacleCruisePlannerNode::createCruiseObstacle( return createCollisionPointsForInsideCruiseObstacle(traj_points, traj_polys, obstacle); } // obstacle is outside the trajectory + // If the ego is stopping, do not plan cruise for outside obstacles. Stop will be planned. + if (odometry.twist.twist.linear.x < 0.1) { + return std::nullopt; + } return createCollisionPointsForOutsideCruiseObstacle(traj_points, traj_polys, obstacle); }(); if (!collision_points) { return std::nullopt; } - const auto [tangent_vel, normal_vel] = projectObstacleVelocityToTrajectory(traj_points, obstacle); - return CruiseObstacle{obstacle.uuid, obstacle.stamp, obstacle.pose, - tangent_vel, normal_vel, *collision_points}; + return CruiseObstacle{ + obstacle.uuid, + obstacle.stamp, + obstacle.pose, + obstacle.longitudinal_velocity, + obstacle.approach_velocity, + *collision_points}; } std::optional ObstacleCruisePlannerNode::createYieldCruiseObstacle( @@ -1232,11 +1370,15 @@ std::optional ObstacleCruisePlannerNode::createYieldCruiseObstac }(); if (!collision_points) return std::nullopt; - const auto [tangent_vel, normal_vel] = projectObstacleVelocityToTrajectory(traj_points, obstacle); // check if obstacle is driving on the opposite direction - if (tangent_vel < 0.0) return std::nullopt; - return CruiseObstacle{obstacle.uuid, obstacle.stamp, obstacle.pose, - tangent_vel, normal_vel, collision_points.value()}; + if (obstacle.longitudinal_velocity < 0.0) return std::nullopt; + return CruiseObstacle{ + obstacle.uuid, + obstacle.stamp, + obstacle.pose, + obstacle.longitudinal_velocity, + obstacle.approach_velocity, + collision_points.value()}; } std::optional> ObstacleCruisePlannerNode::findYieldCruiseObstacles( @@ -1332,22 +1474,20 @@ ObstacleCruisePlannerNode::createCollisionPointsForInsideCruiseObstacle( } { // consider hysteresis - const auto [obstacle_tangent_vel, obstacle_normal_vel] = - projectObstacleVelocityToTrajectory(traj_points, obstacle); // const bool is_prev_obstacle_stop = getObstacleFromUuid(prev_stop_obstacles_, // obstacle.uuid).has_value(); const bool is_prev_obstacle_cruise = getObstacleFromUuid(prev_cruise_object_obstacles_, obstacle.uuid).has_value(); if (is_prev_obstacle_cruise) { - if (obstacle_tangent_vel < p.obstacle_velocity_threshold_from_cruise_to_stop) { + if (obstacle.longitudinal_velocity < p.obstacle_velocity_threshold_from_cruise_to_stop) { return std::nullopt; } // NOTE: else is keeping cruise } else { // if (is_prev_obstacle_stop) { // TODO(murooka) consider hysteresis for slow down // If previous obstacle is stop or does not exist. - if (obstacle_tangent_vel < p.obstacle_velocity_threshold_from_stop_to_cruise) { + if (obstacle.longitudinal_velocity < p.obstacle_velocity_threshold_from_stop_to_cruise) { return std::nullopt; } // NOTE: else is cruise from stop @@ -1355,15 +1495,19 @@ ObstacleCruisePlannerNode::createCollisionPointsForInsideCruiseObstacle( } // Get highest confidence predicted path - const auto resampled_predicted_path = resampleHighestConfidencePredictedPath( + const auto resampled_predicted_paths = resampleHighestConfidencePredictedPaths( obstacle.predicted_paths, p.prediction_resampling_time_interval, - p.prediction_resampling_time_horizon); + p.prediction_resampling_time_horizon, 1); + + if (resampled_predicted_paths.empty()) { + return std::nullopt; + } // calculate nearest collision point std::vector collision_index; const auto collision_points = polygon_utils::getCollisionPoints( - traj_points, traj_polys, obstacle.stamp, resampled_predicted_path, obstacle.shape, now(), - is_driving_forward_, collision_index, + traj_points, traj_polys, obstacle.stamp, resampled_predicted_paths.front(), obstacle.shape, + now(), is_driving_forward_, collision_index, calcObstacleMaxLength(obstacle.shape) + p.decimate_trajectory_step_length + std::hypot( vehicle_info_.vehicle_length_m, @@ -1397,21 +1541,32 @@ ObstacleCruisePlannerNode::createCollisionPointsForOutsideCruiseObstacle( return std::nullopt; } - // Get highest confidence predicted path - const auto resampled_predicted_path = resampleHighestConfidencePredictedPath( + // Get the highest confidence predicted paths + const auto resampled_predicted_paths = resampleHighestConfidencePredictedPaths( obstacle.predicted_paths, p.prediction_resampling_time_interval, - p.prediction_resampling_time_horizon); + p.prediction_resampling_time_horizon, p.num_of_predicted_paths_for_outside_cruise_obstacle); // calculate collision condition for cruise std::vector collision_index; - const auto collision_points = polygon_utils::getCollisionPoints( - traj_points, traj_polys, obstacle.stamp, resampled_predicted_path, obstacle.shape, now(), - is_driving_forward_, collision_index, - calcObstacleMaxLength(obstacle.shape) + p.decimate_trajectory_step_length + - std::hypot( - vehicle_info_.vehicle_length_m, - vehicle_info_.vehicle_width_m * 0.5 + p.max_lat_margin_for_cruise), - p.max_prediction_time_for_collision_check); + const auto getCollisionPoints = [&]() -> std::vector { + for (const auto & predicted_path : resampled_predicted_paths) { + const auto collision_points = polygon_utils::getCollisionPoints( + traj_points, traj_polys, obstacle.stamp, predicted_path, obstacle.shape, now(), + is_driving_forward_, collision_index, + calcObstacleMaxLength(obstacle.shape) + p.decimate_trajectory_step_length + + std::hypot( + vehicle_info_.vehicle_length_m, + vehicle_info_.vehicle_width_m * 0.5 + p.max_lat_margin_for_cruise), + p.max_prediction_time_for_collision_check); + if (!collision_points.empty()) { + return collision_points; + } + } + return {}; + }; + + const auto collision_points = getCollisionPoints(); + if (collision_points.empty()) { // Ignore vehicle obstacles outside the trajectory without collision RCLCPP_INFO_EXPRESSION( @@ -1446,6 +1601,49 @@ ObstacleCruisePlannerNode::createCollisionPointsForOutsideCruiseObstacle( return collision_points; } +std::optional> +ObstacleCruisePlannerNode::createCollisionPointForOutsideStopObstacle( + const Odometry & odometry, const std::vector & traj_points, + const std::vector & traj_polys, const Obstacle & obstacle, + const PredictedPath & resampled_predicted_path, double max_lat_margin_for_stop) const +{ + const auto & object_id = obstacle.uuid.substr(0, 4); + const auto & p = behavior_determination_param_; + + std::vector collision_index; + const auto collision_points = polygon_utils::getCollisionPoints( + traj_points, traj_polys, obstacle.stamp, resampled_predicted_path, obstacle.shape, now(), + is_driving_forward_, collision_index, + calcObstacleMaxLength(obstacle.shape) + p.decimate_trajectory_step_length + + std::hypot( + vehicle_info_.vehicle_length_m, + vehicle_info_.vehicle_width_m * 0.5 + max_lat_margin_for_stop)); + if (collision_points.empty()) { + RCLCPP_INFO_EXPRESSION( + get_logger(), enable_debug_info_, + "[Stop] Ignore outside obstacle (%s) since there is no collision point between the " + "predicted path " + "and the ego.", + object_id.c_str()); + debug_data_ptr_->intentionally_ignored_obstacles.push_back(obstacle); + return std::nullopt; + } + + const double collision_time_margin = + calcCollisionTimeMargin(odometry, collision_points, traj_points, is_driving_forward_); + if (p.collision_time_margin < collision_time_margin) { + RCLCPP_INFO_EXPRESSION( + get_logger(), enable_debug_info_, + "[Stop] Ignore outside obstacle (%s) since it will not collide with the ego.", + object_id.c_str()); + debug_data_ptr_->intentionally_ignored_obstacles.push_back(obstacle); + return std::nullopt; + } + + return polygon_utils::getCollisionPoint( + traj_points, collision_index.front(), collision_points, is_driving_forward_, vehicle_info_); +} + std::optional ObstacleCruisePlannerNode::createStopObstacleForPredictedObject( const Odometry & odometry, const std::vector & traj_points, const std::vector & traj_polys, const Obstacle & obstacle, @@ -1454,71 +1652,81 @@ std::optional ObstacleCruisePlannerNode::createStopObstacleForPred const auto & p = behavior_determination_param_; const auto & object_id = obstacle.uuid.substr(0, 4); - // NOTE: consider all target obstacles when driving backward if (!isStopObstacle(obstacle.classification.label)) { 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; + // Obstacle that is not inside of trajectory if (precise_lat_dist > std::max(max_lat_margin_for_stop, 1e-3)) { - return std::nullopt; - } - - // check obstacle velocity - // NOTE: If precise_lat_dist is 0, always plan stop - constexpr double epsilon = 1e-6; - if (epsilon < precise_lat_dist) { - const auto [obstacle_tangent_vel, obstacle_normal_vel] = - projectObstacleVelocityToTrajectory(traj_points, obstacle); - if (p.obstacle_velocity_threshold_from_stop_to_cruise <= obstacle_tangent_vel) { + if (!isOutsideStopObstacle(obstacle.classification.label)) { return std::nullopt; } - } - // NOTE: Dynamic obstacles for stop are crossing ego's trajectory with high speed, - // and the collision between ego and obstacles are within the margin threshold. - const bool is_obstacle_crossing = isObstacleCrossing(traj_points, obstacle); - const bool has_high_speed = p.crossing_obstacle_velocity_threshold < - std::hypot(obstacle.twist.linear.x, obstacle.twist.linear.y); - if (is_obstacle_crossing && has_high_speed) { - // Get highest confidence predicted path - const auto resampled_predicted_path = resampleHighestConfidencePredictedPath( - obstacle.predicted_paths, p.prediction_resampling_time_interval, - p.prediction_resampling_time_horizon); - - std::vector collision_index; - const auto collision_points = polygon_utils::getCollisionPoints( - traj_points, traj_polys, obstacle.stamp, resampled_predicted_path, obstacle.shape, now(), - is_driving_forward_, collision_index, - calcObstacleMaxLength(obstacle.shape) + p.decimate_trajectory_step_length + - std::hypot( - vehicle_info_.vehicle_length_m, - vehicle_info_.vehicle_width_m * 0.5 + max_lat_margin_for_stop)); - if (collision_points.empty()) { + const auto time_to_traj = precise_lat_dist / std::max(1e-6, obstacle.approach_velocity); + if (time_to_traj > p.max_lat_time_margin_for_stop) { RCLCPP_INFO_EXPRESSION( get_logger(), enable_debug_info_, - "[Stop] Ignore inside obstacle (%s) since there is no collision point between the " - "predicted path " - "and the ego.", - object_id.c_str()); - debug_data_ptr_->intentionally_ignored_obstacles.push_back(obstacle); + "[Stop] Ignore outside obstacle (%s) since it's far from trajectory.", object_id.c_str()); return std::nullopt; } - const double collision_time_margin = - calcCollisionTimeMargin(odometry, collision_points, traj_points, is_driving_forward_); - if (p.collision_time_margin < collision_time_margin) { - RCLCPP_INFO_EXPRESSION( - get_logger(), enable_debug_info_, - "[Stop] Ignore inside obstacle (%s) since it will not collide with the ego.", - object_id.c_str()); - debug_data_ptr_->intentionally_ignored_obstacles.push_back(obstacle); + // brkay54: For the pedestrians and bicycles, we need to check the collision point by thinking + // they will stop with a predefined deceleration rate to avoid unnecessary stops. + double resample_time_horizon = p.prediction_resampling_time_horizon; + if (obstacle.classification.label == ObjectClassification::PEDESTRIAN) { + resample_time_horizon = + std::sqrt(std::pow(obstacle.twist.linear.x, 2) + std::pow(obstacle.twist.linear.y, 2)) / + (2.0 * p.pedestrian_deceleration_rate); + } else if (obstacle.classification.label == ObjectClassification::BICYCLE) { + resample_time_horizon = + std::sqrt(std::pow(obstacle.twist.linear.x, 2) + std::pow(obstacle.twist.linear.y, 2)) / + (2.0 * p.bicycle_deceleration_rate); + } + + // Get the highest confidence predicted path + const auto resampled_predicted_paths = resampleHighestConfidencePredictedPaths( + obstacle.predicted_paths, p.prediction_resampling_time_interval, resample_time_horizon, + p.num_of_predicted_paths_for_outside_stop_obstacle); + if (resampled_predicted_paths.empty()) { return std::nullopt; } + + const auto getCollisionPoint = + [&]() -> std::optional> { + for (const auto & predicted_path : resampled_predicted_paths) { + const auto collision_point = createCollisionPointForOutsideStopObstacle( + odometry, traj_points, traj_polys, obstacle, predicted_path, max_lat_margin_for_stop); + if (collision_point) { + return collision_point; + } + } + return std::nullopt; + }; + + const auto collision_point = getCollisionPoint(); + + if (collision_point) { + return StopObstacle{ + obstacle.uuid, + obstacle.stamp, + obstacle.classification, + obstacle.pose, + obstacle.shape, + obstacle.longitudinal_velocity, + obstacle.approach_velocity, + collision_point->first, + collision_point->second}; + } + return std::nullopt; + } + + // Obstacle inside the trajectory + if (!isInsideStopObstacle(obstacle.classification.label)) { + return std::nullopt; } // calculate collision points with trajectory with lateral stop margin @@ -1531,10 +1739,55 @@ std::optional ObstacleCruisePlannerNode::createStopObstacleForPred return std::nullopt; } - const auto [tangent_vel, normal_vel] = projectObstacleVelocityToTrajectory(traj_points, obstacle); - return StopObstacle{obstacle.uuid, obstacle.stamp, obstacle.classification, - obstacle.pose, obstacle.shape, tangent_vel, - normal_vel, collision_point->first, collision_point->second}; + // check transient obstacle or not + const double abs_ego_offset = + min_behavior_stop_margin_ + (is_driving_forward_ + ? std::abs(vehicle_info_.max_longitudinal_offset_m) + : std::abs(vehicle_info_.min_longitudinal_offset_m)); + + const double time_to_reach_stop_point = + calcTimeToReachCollisionPoint(odometry, collision_point->first, traj_points, abs_ego_offset); + const bool is_transient_obstacle = [&]() { + if (time_to_reach_stop_point <= p.collision_time_margin) { + return false; + } + // get the predicted position of the obstacle when ego reaches the collision point + const auto resampled_predicted_paths = resampleHighestConfidencePredictedPaths( + obstacle.predicted_paths, p.prediction_resampling_time_interval, + p.prediction_resampling_time_horizon, 1); + if (resampled_predicted_paths.empty() || resampled_predicted_paths.front().path.empty()) { + return false; + } + const auto future_obj_pose = object_recognition_utils::calcInterpolatedPose( + resampled_predicted_paths.front(), time_to_reach_stop_point - p.collision_time_margin); + + Obstacle tmp_future_obs = obstacle; + tmp_future_obs.pose = + future_obj_pose ? future_obj_pose.value() : resampled_predicted_paths.front().path.back(); + const auto future_collision_point = polygon_utils::getCollisionPoint( + traj_points, traj_polys_with_lat_margin, tmp_future_obs, is_driving_forward_, vehicle_info_); + + return !future_collision_point; + }(); + + if (is_transient_obstacle) { + RCLCPP_INFO_EXPRESSION( + get_logger(), enable_debug_info_, + "[Stop] Ignore inside obstacle (%s) since it is transient obstacle.", object_id.c_str()); + debug_data_ptr_->intentionally_ignored_obstacles.push_back(obstacle); + return std::nullopt; + } + + return StopObstacle{ + obstacle.uuid, + obstacle.stamp, + obstacle.classification, + obstacle.pose, + obstacle.shape, + obstacle.longitudinal_velocity, + obstacle.approach_velocity, + collision_point->first, + collision_point->second}; } std::optional ObstacleCruisePlannerNode::createStopObstacleForPointCloud( @@ -1693,10 +1946,16 @@ std::optional ObstacleCruisePlannerNode::createSlowDownObstacl } } - const auto [tangent_vel, normal_vel] = projectObstacleVelocityToTrajectory(traj_points, obstacle); - return SlowDownObstacle{obstacle.uuid, obstacle.stamp, obstacle.classification, - obstacle.pose, tangent_vel, normal_vel, - precise_lat_dist, front_collision_point, back_collision_point}; + return SlowDownObstacle{ + obstacle.uuid, + obstacle.stamp, + obstacle.classification, + obstacle.pose, + obstacle.longitudinal_velocity, + obstacle.approach_velocity, + precise_lat_dist, + front_collision_point, + back_collision_point}; } std::optional ObstacleCruisePlannerNode::createSlowDownObstacleForPointCloud( @@ -1780,29 +2039,28 @@ double ObstacleCruisePlannerNode::calcCollisionTimeMargin( const Odometry & odometry, const std::vector & collision_points, const std::vector & traj_points, const bool is_driving_forward) const { - const auto & ego_pose = odometry.pose.pose; - const double ego_vel = odometry.twist.twist.linear.x; - - const double time_to_reach_collision_point = [&]() { - const double abs_ego_offset = is_driving_forward - ? std::abs(vehicle_info_.max_longitudinal_offset_m) - : std::abs(vehicle_info_.min_longitudinal_offset_m); - const double dist_from_ego_to_obstacle = - std::abs(autoware::motion_utils::calcSignedArcLength( - traj_points, ego_pose.position, collision_points.front().point)) - - abs_ego_offset; - return dist_from_ego_to_obstacle / std::max(1e-6, std::abs(ego_vel)); - }(); + const auto & p = behavior_determination_param_; + const double abs_ego_offset = + min_behavior_stop_margin_ + (is_driving_forward + ? std::abs(vehicle_info_.max_longitudinal_offset_m) + : std::abs(vehicle_info_.min_longitudinal_offset_m)); + const double time_to_reach_stop_point = calcTimeToReachCollisionPoint( + odometry, collision_points.front().point, traj_points, abs_ego_offset); + + const double time_to_leave_collision_point = + time_to_reach_stop_point + + abs_ego_offset / + std::max(p.min_velocity_to_reach_collision_point, odometry.twist.twist.linear.x); const double time_to_start_cross = (rclcpp::Time(collision_points.front().stamp) - now()).seconds(); const double time_to_end_cross = (rclcpp::Time(collision_points.back().stamp) - now()).seconds(); - if (time_to_reach_collision_point < time_to_start_cross) { // Ego goes first. - return time_to_start_cross - time_to_reach_collision_point; + if (time_to_leave_collision_point < time_to_start_cross) { // Ego goes first. + return time_to_start_cross - time_to_reach_stop_point; } - if (time_to_end_cross < time_to_reach_collision_point) { // Obstacle goes first. - return time_to_reach_collision_point - time_to_end_cross; + if (time_to_end_cross < time_to_reach_stop_point) { // Obstacle goes first. + return time_to_reach_stop_point - time_to_end_cross; } return 0.0; // Ego and obstacle will collide. } @@ -1982,6 +2240,19 @@ void ObstacleCruisePlannerNode::publishCalculationTime(const double calculation_ calculation_time_msg.data = calculation_time; debug_calculation_time_pub_->publish(calculation_time_msg); } + +double ObstacleCruisePlannerNode::calcTimeToReachCollisionPoint( + const Odometry & odometry, const geometry_msgs::msg::Point & collision_point, + const std::vector & traj_points, const double abs_ego_offset) const +{ + const auto & p = behavior_determination_param_; + const double dist_from_ego_to_obstacle = + std::abs(autoware::motion_utils::calcSignedArcLength( + traj_points, odometry.pose.pose.position, collision_point)) - + abs_ego_offset; + return dist_from_ego_to_obstacle / + std::max(p.min_velocity_to_reach_collision_point, std::abs(odometry.twist.twist.linear.x)); +} } // namespace autoware::motion_planning #include diff --git a/planning/autoware_obstacle_cruise_planner/src/polygon_utils.cpp b/planning/autoware_obstacle_cruise_planner/src/polygon_utils.cpp index 8c3abe58f5d59..639feec856a7e 100644 --- a/planning/autoware_obstacle_cruise_planner/src/polygon_utils.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/polygon_utils.cpp @@ -125,6 +125,37 @@ std::optional> getCollisionPoint( *max_collision_length); } +std::optional> getCollisionPoint( + const std::vector & traj_points, const size_t collision_idx, + const std::vector & collision_points, const bool is_driving_forward, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info) +{ + std::pair> collision_info; + collision_info.first = collision_idx; + collision_info.second = collision_points; + + const double x_diff_to_bumper = is_driving_forward ? vehicle_info.max_longitudinal_offset_m + : vehicle_info.min_longitudinal_offset_m; + const auto bumper_pose = autoware::universe_utils::calcOffsetPose( + traj_points.at(collision_info.first).pose, x_diff_to_bumper, 0.0, 0.0); + + std::optional max_collision_length = std::nullopt; + std::optional max_collision_point = std::nullopt; + for (const auto & poly_vertex : collision_info.second) { + const double dist_from_bumper = + std::abs(autoware::universe_utils::inverseTransformPoint(poly_vertex.point, bumper_pose).x); + + if (!max_collision_length.has_value() || dist_from_bumper > *max_collision_length) { + max_collision_length = dist_from_bumper; + max_collision_point = poly_vertex.point; + } + } + return std::make_pair( + *max_collision_point, + autoware::motion_utils::calcSignedArcLength(traj_points, 0, collision_info.first) - + *max_collision_length); +} + // NOTE: max_lat_dist is used for efficient calculation to suppress boost::geometry's polygon // calculation. std::vector getCollisionPoints(