Freespace planner failed to plan a trajectory for parking if there is a vehicle very close to ego car

FZ-Broky commented 2 months ago

Checklist

[X] I've read the contribution guidelines.
[X] I've searched other issues and no duplicate issues were found.
[X] I'm convinced that this is not my fault but a bug.

Description

When running the parking algorithm in the parking_lot area, there is an NPC vehicle very close behind the ego vehicle, which prevents the ego vehicle from running the freespace_planning. The NPC vehicle has not made contact with the ego vehicle and is not in the ego vehicle's driving path.

Expected behavior

Since the NPC vehicle does not affect the ego vehicle's parking, the freespace_planner should be able to plan a trajectory for parking.

Actual behavior

Unable to plan a trajectory for parking.

Steps to reproduce

Place ego vehicle in a parking_lot(should be big enough)
Spawn a NPC vehicle behind ego(should be close)
Set goal pose at a parking_space in the parking lot.

Versions

OS: ROS2
Autoware: humble-2023.08-cuda-amd64

Possible causes

Related code

// In file autoware.universe/planning/autoware_costmap_generator/nodes/autoware_costmap_generator/costmap_generator_node.cpp
// line 440 
grid_map::Matrix CostmapGenerator::generateObjectsCostmap(
  const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr in_objects)
{
  const auto object_frame = in_objects->header.frame_id;
  const auto transformed_objects =
    transformObjects(tf_buffer_, in_objects, costmap_frame_, object_frame);

  grid_map::Matrix objects_costmap = objects2costmap_.makeCostmapFromObjects(
    costmap_, expand_polygon_size_, size_of_expansion_kernel_, transformed_objects);

  return objects_costmap;
}

// In file autoware.universe/planning/autoware_costmap_generator/nodes/autoware_costmap_generator/objects_to_costmap.cpp
// line 61
Eigen::MatrixXd ObjectsToCostmap::makeRectanglePoints(
  const autoware_auto_perception_msgs::msg::PredictedObject & in_object,
  const double expand_rectangle_size)
{
  double length = in_object.shape.dimensions.x + expand_rectangle_size;
  double width = in_object.shape.dimensions.y + expand_rectangle_size;
  Eigen::MatrixXd origin_points(NUMBER_OF_DIMENSIONS, NUMBER_OF_POINTS);
  origin_points << length / 2, length / 2, -length / 2, -length / 2, width / 2, -width / 2,
    -width / 2, width / 2;

  double yaw = tf2::getYaw(in_object.kinematics.initial_pose_with_covariance.pose.orientation);
  Eigen::MatrixXd rotation_matrix(NUMBER_OF_DIMENSIONS, NUMBER_OF_DIMENSIONS);
  rotation_matrix << std::cos(yaw), -std::sin(yaw), std::sin(yaw), std::cos(yaw);
  Eigen::MatrixXd rotated_points = rotation_matrix * origin_points;

  double dx = in_object.kinematics.initial_pose_with_covariance.pose.position.x;
  double dy = in_object.kinematics.initial_pose_with_covariance.pose.position.y;
  Eigen::MatrixXd transformed_points(NUMBER_OF_DIMENSIONS, NUMBER_OF_POINTS);
  Eigen::MatrixXd ones = Eigen::MatrixXd::Ones(1, NUMBER_OF_POINTS);
  transformed_points.row(0) = rotated_points.row(0) + dx * ones;
  transformed_points.row(1) = rotated_points.row(1) + dy * ones;

  return transformed_points;
}

When generating costmap from Objects, the value of parameters expand_polygonsize and grid_resolution are too large. I think that causes autoware to misjudge whether the starting point has an obstacle.

Additional context

https://github.com/user-attachments/assets/99624811-cf77-43ce-ba58-5b8b4a35909a

I initially discovered this bug when running with the Carla simulator. Here, I am using the planning-simulator to verify it.

mkquda commented 2 months ago

@FZ-Broky

Thank you for your report!

With regard to the issue you raised, I would like to point out that several improvements have been done recently to improve the quality of free space planning, you can refer to this discussion.

I agree that the safety handling is still not ideal and needs improvement as you pointed out. But the changes applied include reducing the safety margins as you suggested. If the result with latest autoware is still not good enough you can try reducing the following parameters to achieve the desired behavior:

FZ-Broky commented 2 months ago

@mkquda Thank you for your reply! I will try on the latest version.

idorobotics commented 1 month ago

@FZ-Broky did using the latest version resolve your issues?

wbq0206 commented 2 weeks ago

I completely agree with your viewpoint. Adjusting parameters may bring some short-term effects, but it cannot fundamentally solve the problem. As the ancient Chinese saying goes: Treating symptoms without addressing the root cause，I think we should handle the coordination between speed planning and path planning well

FZ-Broky commented 5 days ago

@idorobotics I tried it on the latest version, and the issue still persists. I was wondering if we could remove the check for the start point.

autowarefoundation / autoware.universe