Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

fix(star/goal_planner): improve start/goal planner #1418

Merged
merged 3 commits into from
Jul 22, 2024
Merged
Show file tree
Hide file tree
Changes from 1 commit
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
feat(start_planner): check current_pose and estimated_stop_pose for i…
…sPreventingRearVehicleFromPassingThrough (autowarefoundation#8112)
  • Loading branch information
kosuke55 committed Jul 22, 2024
commit e553faaf8206e625b369fce04af620d4da62ef70
Original file line number Diff line number Diff line change
Expand Up @@ -90,6 +90,9 @@ struct StartPlannerDebugData
std::vector<Pose> start_pose_candidates;
size_t selected_start_pose_candidate_index;
double margin_for_start_pose_candidate;

// for isPreventingRearVehicleFromPassingThrough
std::optional<Pose> estimated_stop_pose;
};

struct StartPlannerParameters
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -229,8 +229,31 @@ class StartPlannerModule : public SceneModuleInterface

bool isModuleRunning() const;
bool isCurrentPoseOnMiddleOfTheRoad() const;

/**
* @brief Check if the ego vehicle is preventing the rear vehicle from passing through.
*
* This function just call isPreventingRearVehicleFromPassingThrough(const Pose & ego_pose) with
* two poses. If rear vehicle is obstructed by ego vehicle at either of the two poses, it returns
* true.
*
* @return true if the ego vehicle is preventing the rear vehicle from passing through with the
* current pose or the pose if it stops.
*/
bool isPreventingRearVehicleFromPassingThrough() const;

/**
* @brief Check if the ego vehicle is preventing the rear vehicle from passing through.
*
* This function measures the distance to the lane boundary from the current pose and the pose if
it stops, and determines whether there is enough space for the rear vehicle to pass through. If
* it is obstructing at either of the two poses, it returns true.
*
* @return true if the ego vehicle is preventing the rear vehicle from passing through with given
ego pose.
*/
bool isPreventingRearVehicleFromPassingThrough(const Pose & ego_pose) const;

bool isCloseToOriginalStartPose() const;
bool hasArrivedAtGoal() const;
bool isMoving() const;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -368,7 +368,35 @@ bool StartPlannerModule::isCurrentPoseOnMiddleOfTheRoad() const

bool StartPlannerModule::isPreventingRearVehicleFromPassingThrough() const
{
// prepare poses for preventing check
// - current_pose
// - estimated_stop_pose (The position assumed when stopped with the minimum stop distance,
// although it is NOT actually stopped)
const auto & current_pose = planner_data_->self_odometry->pose.pose;
std::vector<Pose> preventing_check_pose{current_pose};
const auto min_stop_distance =
autoware::behavior_path_planner::utils::parking_departure::calcFeasibleDecelDistance(
planner_data_, parameters_->maximum_deceleration_for_stop, parameters_->maximum_jerk_for_stop,
0.0);
debug_data_.estimated_stop_pose.reset(); // debug
if (min_stop_distance.has_value()) {
const auto current_path = getCurrentPath();
const auto estimated_stop_pose = calcLongitudinalOffsetPose(
current_path.points, current_pose.position, min_stop_distance.value());
if (estimated_stop_pose.has_value()) {
preventing_check_pose.push_back(estimated_stop_pose.value());
debug_data_.estimated_stop_pose = estimated_stop_pose.value(); // debug
}
}

// check if any of the preventing check poses are preventing rear vehicle from passing through
return std::any_of(
preventing_check_pose.begin(), preventing_check_pose.end(),
[&](const auto & pose) { return isPreventingRearVehicleFromPassingThrough(pose); });
}

bool StartPlannerModule::isPreventingRearVehicleFromPassingThrough(const Pose & ego_pose) const
{
const auto & dynamic_object = planner_data_->dynamic_object;
const auto & route_handler = planner_data_->route_handler;
const Pose start_pose = planner_data_->route_handler->getOriginalStartPose();
Expand Down Expand Up @@ -414,8 +442,8 @@ bool StartPlannerModule::isPreventingRearVehicleFromPassingThrough() const
geometry_msgs::msg::Pose & ego_overhang_point_as_pose,
const bool ego_is_merging_from_the_left) -> std::optional<std::pair<double, double>> {
const auto local_vehicle_footprint = vehicle_info_.createFootprint();
const auto vehicle_footprint = transformVector(
local_vehicle_footprint, autoware::universe_utils::pose2transform(current_pose));
const auto vehicle_footprint =
transformVector(local_vehicle_footprint, autoware::universe_utils::pose2transform(ego_pose));
double smallest_lateral_gap_between_ego_and_border = std::numeric_limits<double>::max();
double corresponding_lateral_gap_with_other_lane_bound = std::numeric_limits<double>::max();

Expand Down Expand Up @@ -481,7 +509,7 @@ bool StartPlannerModule::isPreventingRearVehicleFromPassingThrough() const
// Check backwards just in case the Vehicle behind ego is in a different lanelet
constexpr double backwards_length = 200.0;
const auto prev_lanes = autoware::behavior_path_planner::utils::getBackwardLanelets(
*route_handler, target_lanes, current_pose, backwards_length);
*route_handler, target_lanes, ego_pose, backwards_length);
// return all the relevant lanelets
lanelet::ConstLanelets relevant_lanelets{closest_lanelet_const};
relevant_lanelets.insert(relevant_lanelets.end(), prev_lanes.begin(), prev_lanes.end());
Expand All @@ -491,7 +519,7 @@ bool StartPlannerModule::isPreventingRearVehicleFromPassingThrough() const

// filtering objects with velocity, position and class
const auto filtered_objects = utils::path_safety_checker::filterObjects(
dynamic_object, route_handler, relevant_lanelets.value(), current_pose.position,
dynamic_object, route_handler, relevant_lanelets.value(), ego_pose.position,
objects_filtering_params_);
if (filtered_objects.objects.empty()) return false;

Expand All @@ -508,7 +536,7 @@ bool StartPlannerModule::isPreventingRearVehicleFromPassingThrough() const
target_objects_on_lane.on_current_lane.begin(), target_objects_on_lane.on_current_lane.end(),
[&](const auto & o) {
const auto arc_length = autoware::motion_utils::calcSignedArcLength(
centerline_path.points, current_pose.position, o.initial_pose.pose.position);
centerline_path.points, ego_pose.position, o.initial_pose.pose.position);
if (arc_length > 0.0) return;
if (std::abs(arc_length) >= std::abs(arc_length_to_closet_object)) return;
arc_length_to_closet_object = arc_length;
Expand Down Expand Up @@ -1723,6 +1751,16 @@ void StartPlannerModule::setDebugData()
info_marker_);
add(showPolygon(debug_data_.collision_check, "ego_and_target_polygon_relation"), info_marker_);

// visualize estimated_stop_pose for isPreventingRearVehicleFromPassingThrough()
if (debug_data_.estimated_stop_pose.has_value()) {
auto footprint_marker = createDefaultMarker(
"map", rclcpp::Clock{RCL_ROS_TIME}.now(), "estimated_stop_pose", 0, Marker::LINE_STRIP,
createMarkerScale(0.2, 0.2, 0.2), purple_color);
footprint_marker.lifetime = rclcpp::Duration::from_seconds(1.5);
addFootprintMarker(footprint_marker, debug_data_.estimated_stop_pose.value(), vehicle_info_);
debug_marker_.markers.push_back(footprint_marker);
}

// set objects of interest
for (const auto & [uuid, data] : debug_data_.collision_check) {
const auto color = data.is_safe ? ColorName::GREEN : ColorName::RED;
Expand Down
Loading