diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp index c6cb73a680e5a..a61fc7fca3ce5 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp @@ -110,14 +110,17 @@ struct StopObstacle : public TargetObstacleInterface const std::string & arg_uuid, const rclcpp::Time & arg_stamp, const geometry_msgs::msg::Pose & arg_pose, const Shape & arg_shape, const double arg_lon_velocity, const double arg_lat_velocity, - const geometry_msgs::msg::Point arg_collision_point) + const geometry_msgs::msg::Point arg_collision_point, + const double arg_traj_dist_to_stop_point) : TargetObstacleInterface(arg_uuid, arg_stamp, arg_pose, arg_lon_velocity, arg_lat_velocity), shape(arg_shape), - collision_point(arg_collision_point) + collision_point(arg_collision_point), + traj_dist_to_stop_point(arg_traj_dist_to_stop_point) { } Shape shape; geometry_msgs::msg::Point collision_point; + double traj_dist_to_stop_point; }; struct CruiseObstacle : public TargetObstacleInterface diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp index 9b2de4c8f2de0..17b9bd7749947 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp @@ -67,6 +67,9 @@ class ObstacleCruisePlannerNode : public rclcpp::Node bool isOutsideCruiseObstacle(const uint8_t label) const; bool isCruiseObstacle(const uint8_t label) const; bool isSlowDownObstacle(const uint8_t label) const; + std::optional calcTrajDistToSecurePolysDist( + const std::vector & traj_points, const std::vector & traj_polys, + const Obstacle & obstacle) const; std::optional createCollisionPointForStopObstacle( const std::vector & traj_points, const std::vector & traj_polys, const Obstacle & obstacle) const; diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp index d8c31fb35df9b..98daa78e696d2 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp @@ -102,6 +102,11 @@ class PlannerInterface slow_down_debug_multi_array_.stamp = current_time; return slow_down_debug_multi_array_; } + double getSafeDistanceMargin() + { + return longitudinal_info_.safe_distance_margin; + } + protected: // Parameters diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index c9fe9f2210423..8fc2d53de9719 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -24,10 +24,42 @@ #include "tier4_autoware_utils/ros/update_param.hpp" #include +#include +#include #include #include +#define debug(var) \ + do { \ + std::cerr << __func__ << ": " << __LINE__ << ", " << #var << " : "; \ + view(var); \ + } while (0) +template +void view(T e) +{ + std::cerr << e << std::endl; +} +template +void view(const std::vector & v) +{ + for (const auto & e : v) { + std::cerr << e << " "; + } + std::cerr << std::endl; +} +template +void view(const std::vector> & vv) +{ + for (const auto & v : vv) { + view(v); + } +} +#define line() \ + { \ + std::cerr << "(" << __FILE__ << ") " << __func__ << ": " << __LINE__ << std::endl; \ + } + namespace { VelocityLimitClearCommand createVelocityLimitClearCommandMessage( @@ -997,9 +1029,88 @@ std::optional ObstacleCruisePlannerNode::createStopObstacle( return std::nullopt; } + const auto traj_dist_to_secure_stop_point = + calcTrajDistToSecurePolysDist(traj_points, traj_polys, obstacle); + if (!traj_dist_to_secure_stop_point) { + return std::nullopt; + } + debug(traj_dist_to_secure_stop_point.value()); + const auto [tangent_vel, normal_vel] = projectObstacleVelocityToTrajectory(traj_points, obstacle); - return StopObstacle{obstacle.uuid, obstacle.stamp, obstacle.pose, obstacle.shape, - tangent_vel, normal_vel, *collision_point}; + return StopObstacle{ + obstacle.uuid, obstacle.stamp, obstacle.pose, obstacle.shape, + tangent_vel, normal_vel, *collision_point, *traj_dist_to_secure_stop_point}; +} + +std::optional ObstacleCruisePlannerNode::calcTrajDistToSecurePolysDist( + const std::vector & traj_points, const std::vector & traj_polys, + const Obstacle & obstacle) const +{ + const auto & p = behavior_determination_param_; + const auto & object_id = obstacle.uuid.substr(0, 4); + + // 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 double 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); + if (collision_points.empty()) { + 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); + return std::nullopt; + } + + const double collision_time_margin = + calcCollisionTimeMargin(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); + return std::nullopt; + } + } + + const auto traj_polys_with_lat_margin = createOneStepPolygons( + traj_points, vehicle_info_, ego_odom_ptr_->pose.pose, p.max_lat_margin_for_stop); + auto obstacle_polygon = tier4_autoware_utils::toPolygon2d(obstacle.pose, obstacle.shape); + const auto dist_to_be_secured = planner_ptr_->getSafeDistanceMargin(); + + for (size_t col_i = 0; col_i < traj_polys_with_lat_margin.size(); ++col_i) { + if (boost::geometry::intersects(traj_polys_with_lat_margin.at(col_i), obstacle_polygon)) { + double last_dist = 0.0; + for (std::int32_t sec_i = col_i - 1; sec_i >= 0; --sec_i) { + double current_dist = + boost::geometry::distance(traj_polys_with_lat_margin.at(sec_i), obstacle_polygon); + debug(current_dist); + if (current_dist > dist_to_be_secured) { + debug(sec_i); + double decimal_secure_index = + sec_i + (dist_to_be_secured - current_dist) / (last_dist - current_dist); + return decimal_secure_index * p.decimate_trajectory_step_length; + } + last_dist = current_dist; + } + return 0.0; + } + } + return std::nullopt; } std::optional diff --git a/planning/obstacle_cruise_planner/src/planner_interface.cpp b/planning/obstacle_cruise_planner/src/planner_interface.cpp index 76469364cfb39..2cd427b0f43ff 100644 --- a/planning/obstacle_cruise_planner/src/planner_interface.cpp +++ b/planning/obstacle_cruise_planner/src/planner_interface.cpp @@ -24,6 +24,15 @@ #include #include + + +#define debug(var) do{std::cerr << __func__ << ": " << __LINE__ << ", " << #var << " : ";view(var);}while(0) +template void view(T e){std::cerr << e << std::endl;} +template void view(const std::vector& v){for(const auto& e : v){ std::cerr << e << " "; } std::cerr << std::endl;} +template void view(const std::vector >& vv){ for(const auto& v : vv){ view(v); } } +#define line() {std::cerr << "(" << __FILE__ << ") " << __func__ << ": " << __LINE__ << std::endl; } + + namespace { StopSpeedExceeded createStopSpeedExceededMsg( @@ -257,8 +266,18 @@ std::vector PlannerInterface::generateStopTrajectory( "stop planning"); // Get Closest Stop Obstacle - const auto closest_stop_obstacle = - obstacle_cruise_utils::getClosestStopObstacle(planner_data.traj_points, stop_obstacles); + // const auto closest_stop_obstacle = + // obstacle_cruise_utils::getClosestStopObstacle(planner_data.traj_points, stop_obstacles); + + std::optional closest_stop_obstacle = std::nullopt; + double dist_to_closest_stop_obstacle = std::numeric_limits::max(); + for (const auto & stop_obstacle : stop_obstacles) { + if (stop_obstacle.traj_dist_to_stop_point < dist_to_closest_stop_obstacle) { + dist_to_closest_stop_obstacle = stop_obstacle.traj_dist_to_stop_point; + closest_stop_obstacle = stop_obstacle; + } + } + if (!closest_stop_obstacle) { // delete marker const auto markers = @@ -270,8 +289,9 @@ std::vector PlannerInterface::generateStopTrajectory( } // Get Closest Obstacle Stop Distance - const double closest_obstacle_dist = motion_utils::calcSignedArcLength( - planner_data.traj_points, 0, closest_stop_obstacle->collision_point); + // const double closest_obstacle_dist = motion_utils::calcSignedArcLength( + // planner_data.traj_points, 0, closest_stop_obstacle->collision_point); + const auto ego_segment_idx = ego_nearest_param_.findSegmentIndex(planner_data.traj_points, planner_data.ego_pose); @@ -279,102 +299,108 @@ std::vector PlannerInterface::generateStopTrajectory( planner_data.traj_points, planner_data.ego_pose.position, ego_segment_idx, 0); const double dist_to_ego = -negative_dist_to_ego; - const double margin_from_obstacle = - calculateMarginFromObstacleOnCurve(planner_data, *closest_stop_obstacle); - - // If behavior stop point is ahead of the closest_obstacle_stop point within a certain margin - // we set closest_obstacle_stop_distance to closest_behavior_stop_distance - const double margin_from_obstacle_considering_behavior_module = [&]() { - const size_t nearest_segment_idx = - findEgoSegmentIndex(planner_data.traj_points, planner_data.ego_pose); - const auto closest_behavior_stop_idx = - motion_utils::searchZeroVelocityIndex(planner_data.traj_points, nearest_segment_idx + 1); - - if (!closest_behavior_stop_idx) { - return margin_from_obstacle; - } - - const double closest_behavior_stop_dist_from_ego = motion_utils::calcSignedArcLength( - planner_data.traj_points, planner_data.ego_pose.position, nearest_segment_idx, - *closest_behavior_stop_idx); - - if (*closest_behavior_stop_idx == planner_data.traj_points.size() - 1) { - // Closest behavior stop point is the end point - const double closest_obstacle_stop_dist_from_ego = - closest_obstacle_dist - dist_to_ego - longitudinal_info_.terminal_safe_distance_margin - - abs_ego_offset; - const double stop_dist_diff = - closest_behavior_stop_dist_from_ego - closest_obstacle_stop_dist_from_ego; - if (stop_dist_diff < margin_from_obstacle) { - // Use terminal margin (terminal_safe_distance_margin) for obstacle stop - return longitudinal_info_.terminal_safe_distance_margin; - } - } else { - const double closest_obstacle_stop_dist_from_ego = - closest_obstacle_dist - dist_to_ego - margin_from_obstacle - abs_ego_offset; - const double stop_dist_diff = - closest_behavior_stop_dist_from_ego - closest_obstacle_stop_dist_from_ego; - if (0.0 < stop_dist_diff && stop_dist_diff < margin_from_obstacle) { - // Use shorter margin (min_behavior_stop_margin) for obstacle stop - return min_behavior_stop_margin_; - } - } - return margin_from_obstacle; - }(); - - const auto [stop_margin_from_obstacle, will_collide_with_obstacle] = [&]() { - // Check feasibility to stop - if (suppress_sudden_obstacle_stop_) { - const double closest_obstacle_stop_dist = - closest_obstacle_dist - margin_from_obstacle_considering_behavior_module - abs_ego_offset; - - // Calculate feasible stop margin (Check the feasibility) - const double feasible_stop_dist = calcMinimumDistanceToStop( - planner_data.ego_vel, longitudinal_info_.limit_max_accel, - longitudinal_info_.limit_min_accel) + - dist_to_ego; - - if (closest_obstacle_stop_dist < feasible_stop_dist) { - const auto feasible_margin_from_obstacle = - margin_from_obstacle_considering_behavior_module - - (feasible_stop_dist - closest_obstacle_stop_dist); - return std::make_pair(feasible_margin_from_obstacle, true); - } - } - return std::make_pair(margin_from_obstacle_considering_behavior_module, false); - }(); - - // Generate Output Trajectory - const double zero_vel_dist = [&]() { - const double current_zero_vel_dist = - std::max(0.0, closest_obstacle_dist - abs_ego_offset - stop_margin_from_obstacle); - - // Hold previous stop distance if necessary - if ( - std::abs(planner_data.ego_vel) < longitudinal_info_.hold_stop_velocity_threshold && - prev_stop_distance_info_) { - // NOTE: We assume that the current trajectory's front point is ahead of the previous - // trajectory's front point. - const size_t traj_front_point_prev_seg_idx = - motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - prev_stop_distance_info_->first, planner_data.traj_points.front().pose); - const double diff_dist_front_points = motion_utils::calcSignedArcLength( - prev_stop_distance_info_->first, 0, planner_data.traj_points.front().pose.position, - traj_front_point_prev_seg_idx); - - const double prev_zero_vel_dist = prev_stop_distance_info_->second - diff_dist_front_points; - if ( - std::abs(prev_zero_vel_dist - current_zero_vel_dist) < - longitudinal_info_.hold_stop_distance_threshold) { - return prev_zero_vel_dist; - } - } - - return current_zero_vel_dist; - }(); + debug(dist_to_ego); + + const double zero_vel_dist = dist_to_closest_stop_obstacle + dist_to_ego; + + // const double margin_from_obstacle = + // calculateMarginFromObstacleOnCurve(planner_data, *closest_stop_obstacle); + + // // If behavior stop point is ahead of the closest_obstacle_stop point within a certain margin + // // we set closest_obstacle_stop_distance to closest_behavior_stop_distance + // const double margin_from_obstacle_considering_behavior_module = [&]() { + // const size_t nearest_segment_idx = + // findEgoSegmentIndex(planner_data.traj_points, planner_data.ego_pose); + // const auto closest_behavior_stop_idx = + // motion_utils::searchZeroVelocityIndex(planner_data.traj_points, nearest_segment_idx + 1); + + // if (!closest_behavior_stop_idx) { + // return margin_from_obstacle; + // } + + // const double closest_behavior_stop_dist_from_ego = motion_utils::calcSignedArcLength( + // planner_data.traj_points, planner_data.ego_pose.position, nearest_segment_idx, + // *closest_behavior_stop_idx); + + // if (*closest_behavior_stop_idx == planner_data.traj_points.size() - 1) { + // // Closest behavior stop point is the end point + // const double closest_obstacle_stop_dist_from_ego = + // closest_obstacle_dist - dist_to_ego - longitudinal_info_.terminal_safe_distance_margin - + // abs_ego_offset; + // const double stop_dist_diff = + // closest_behavior_stop_dist_from_ego - closest_obstacle_stop_dist_from_ego; + // if (stop_dist_diff < margin_from_obstacle) { + // // Use terminal margin (terminal_safe_distance_margin) for obstacle stop + // return longitudinal_info_.terminal_safe_distance_margin; + // } + // } else { + // const double closest_obstacle_stop_dist_from_ego = + // closest_obstacle_dist - dist_to_ego - margin_from_obstacle - abs_ego_offset; + // const double stop_dist_diff = + // closest_behavior_stop_dist_from_ego - closest_obstacle_stop_dist_from_ego; + // if (0.0 < stop_dist_diff && stop_dist_diff < margin_from_obstacle) { + // // Use shorter margin (min_behavior_stop_margin) for obstacle stop + // return min_behavior_stop_margin_; + // } + // } + // return margin_from_obstacle; + // }(); + + // const auto [stop_margin_from_obstacle, will_collide_with_obstacle] = [&]() { + // // Check feasibility to stop + // if (suppress_sudden_obstacle_stop_) { + // const double closest_obstacle_stop_dist = + // closest_obstacle_dist - margin_from_obstacle_considering_behavior_module - + // abs_ego_offset; + + // // Calculate feasible stop margin (Check the feasibility) + // const double feasible_stop_dist = calcMinimumDistanceToStop( + // planner_data.ego_vel, + // longitudinal_info_.limit_max_accel, + // longitudinal_info_.limit_min_accel) + + // dist_to_ego; + + // if (closest_obstacle_stop_dist < feasible_stop_dist) { + // const auto feasible_margin_from_obstacle = + // margin_from_obstacle_considering_behavior_module - + // (feasible_stop_dist - closest_obstacle_stop_dist); + // return std::make_pair(feasible_margin_from_obstacle, true); + // } + // } + // return std::make_pair(margin_from_obstacle_considering_behavior_module, false); + // }(); + + // const double zero_vel_dist = [&]() { + // const double current_zero_vel_dist = + // std::max(0.0, closest_obstacle_dist - abs_ego_offset - stop_margin_from_obstacle); + + // // Hold previous stop distance if necessary + // if ( + // std::abs(planner_data.ego_vel) < longitudinal_info_.hold_stop_velocity_threshold && + // prev_stop_distance_info_) { + // // NOTE: We assume that the current trajectory's front point is ahead of the previous + // // trajectory's front point. + // const size_t traj_front_point_prev_seg_idx = + // motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + // prev_stop_distance_info_->first, planner_data.traj_points.front().pose); + // const double diff_dist_front_points = motion_utils::calcSignedArcLength( + // prev_stop_distance_info_->first, 0, planner_data.traj_points.front().pose.position, + // traj_front_point_prev_seg_idx); + + // const double prev_zero_vel_dist = prev_stop_distance_info_->second - + // diff_dist_front_points; if ( + // std::abs(prev_zero_vel_dist - current_zero_vel_dist) < + // longitudinal_info_.hold_stop_distance_threshold) { + // return prev_zero_vel_dist; + // } + // } + + // return current_zero_vel_dist; + // }(); // Insert stop point auto output_traj_points = planner_data.traj_points; + // const auto zero_vel_idx = motion_utils::insertStopPoint(0, zero_vel_dist, output_traj_points); const auto zero_vel_idx = motion_utils::insertStopPoint(0, zero_vel_dist, output_traj_points); if (zero_vel_idx) { // virtual wall marker for stop obstacle @@ -392,21 +418,21 @@ std::vector PlannerInterface::generateStopTrajectory( velocity_factors_pub_->publish(makeVelocityFactorArray(planner_data.current_time, stop_pose)); // Publish if ego vehicle collides with the obstacle with a limit acceleration - const auto stop_speed_exceeded_msg = - createStopSpeedExceededMsg(planner_data.current_time, will_collide_with_obstacle); - stop_speed_exceeded_pub_->publish(stop_speed_exceeded_msg); + // const auto stop_speed_exceeded_msg = + // createStopSpeedExceededMsg(planner_data.current_time, will_collide_with_obstacle); + // stop_speed_exceeded_pub_->publish(stop_speed_exceeded_msg); prev_stop_distance_info_ = std::make_pair(output_traj_points, zero_vel_dist); } - stop_planning_debug_info_.set( - StopPlanningDebugInfo::TYPE::STOP_CURRENT_OBSTACLE_DISTANCE, - closest_obstacle_dist - abs_ego_offset); // TODO(murooka) + // stop_planning_debug_info_.set( + // StopPlanningDebugInfo::TYPE::STOP_CURRENT_OBSTACLE_DISTANCE, + // closest_obstacle_dist - abs_ego_offset); // TODO(murooka) stop_planning_debug_info_.set( StopPlanningDebugInfo::TYPE::STOP_CURRENT_OBSTACLE_VELOCITY, closest_stop_obstacle->velocity); - stop_planning_debug_info_.set( - StopPlanningDebugInfo::TYPE::STOP_TARGET_OBSTACLE_DISTANCE, stop_margin_from_obstacle); + // stop_planning_debug_info_.set( + // StopPlanningDebugInfo::TYPE::STOP_TARGET_OBSTACLE_DISTANCE, stop_margin_from_obstacle); stop_planning_debug_info_.set(StopPlanningDebugInfo::TYPE::STOP_TARGET_VELOCITY, 0.0); stop_planning_debug_info_.set(StopPlanningDebugInfo::TYPE::STOP_TARGET_ACCELERATION, 0.0); diff --git a/planning/obstacle_cruise_planner/src/polygon_utils.cpp b/planning/obstacle_cruise_planner/src/polygon_utils.cpp index e6b4ed89ca460..c636b1201c2d9 100644 --- a/planning/obstacle_cruise_planner/src/polygon_utils.cpp +++ b/planning/obstacle_cruise_planner/src/polygon_utils.cpp @@ -98,6 +98,7 @@ std::optional>> getCollisionIndex( return std::nullopt; } + } // namespace namespace polygon_utils @@ -127,10 +128,9 @@ Polygon2d createOneStepPolygon( } bg::correct(polygon); - Polygon2d hull_polygon; bg::convex_hull(polygon, hull_polygon); - + bg::correct(hull_polygon); return hull_polygon; }