Freespace planner partial trajectory logic may shift turning point in manoeuvers

[VRichardJP]

Checklist

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

Description

This is a similar but orthogonal issue to #5988. I think the "index logic" implemented by getReversingIndices and getPartialTrajectory causes partial trajectories to miss the actual turn point:

autoware.universe/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp

Lines 88 to 102 in 5de111f

	std::vector<size_t> getReversingIndices(const Trajectory & trajectory)
	{
	std::vector<size_t> indices;
	for (size_t i = 0; i < trajectory.points.size() - 1; ++i) {
	if (
	trajectory.points.at(i).longitudinal_velocity_mps *
	trajectory.points.at(i + 1).longitudinal_velocity_mps <
	0) {
	indices.push_back(i);
	}
	}
	return indices;
	}

autoware.universe/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp

Lines 119 to 141 in 5de111f

	Trajectory getPartialTrajectory(
	const Trajectory & trajectory, const size_t start_index, const size_t end_index)
	{
	Trajectory partial_trajectory;
	partial_trajectory.header = trajectory.header;
	partial_trajectory.header.stamp = rclcpp::Clock().now();
	partial_trajectory.points.reserve(trajectory.points.size());
	for (size_t i = start_index; i <= end_index; ++i) {
	partial_trajectory.points.push_back(trajectory.points.at(i));
	}
	// Modify velocity at start/end point
	if (partial_trajectory.points.size() >= 2) {
	partial_trajectory.points.front().longitudinal_velocity_mps =
	partial_trajectory.points.at(1).longitudinal_velocity_mps;
	}
	if (!partial_trajectory.points.empty()) {
	partial_trajectory.points.back().longitudinal_velocity_mps = 0;
	}
	return partial_trajectory;
	}

If you consider the following example trajectory, going forward then backward:

idx	0	1	2	3	4	5	6	7
t(s)	0.0	1.0	2.0	3.0	4.0	5.0	6.0	7.0
vel(m/s)	1.0	3.0	1.0	±0.0	-1.0	-3.0	-1.0	0.0
x(m)	0.0	1.0	4.0	5.0	5.0	4.0	1.0	0.0

By the logic of getPartialTrajectory, the reversing index is 2 or 3 depending on the the sign of point 3. Then getPartialTrajectory extracts point from index 0 to 2 or 3, and override the velocity of the first and last point:

• case 1: vel[3] > 0.0 (-> reversing index is 3), we get the partial trajectory:

idx	0	1	2	3
t(s)	0.0	1.0	2.0	3.0
vel(m/s)	3.0	3.0	1.0	0.0
x(m)	0.0	1.0	4.0	5.0

Changing the first point velocity is a bit strange here, but I guess it is to handle the case when we are at the turn point and avoid the first point of the partial trajectory being 0.0 (which would keep the vehicle stopped). That said, the current freespace planner implementation generate trajectories with a fixed 5km/s velocity target on all points so the velocity override has no effect in practice (velocity is later refined by motion_velocity_smoother)

• case 2: vel[3] < 0.0 (-> reversing index is 2)

idx	0	1	2
t(s)	0.0	1.0	2.0
vel(m/s)	3.0	3.0	0.0
x(m)	0.0	1.0	4.0

You can see in this case we loose the point that was reaching x=5.0. Instead, Autoware would now stop at x=4.0.

One way to avoid this is to reduce the distance between trajectory points, but that means more computation. I think default A* minimum step distance is 0.4 meters, so Autoware may be constantly off by 0.4 meters from turn point.

Another way is to set the turning point on the next index (i.e. the first < 0.0 velocity). However I am afraid this could cause some strange behavior, as the last point of the partial trajectory could end up before the previous point.

I think the proper way to handle this would be to interpolate the turn point instead.

Any suggestion?

Expected behavior

Planned trajectory should be followed precisely

Actual behavior

With the current logic, Autoware may not be able to reach the turn point.

Steps to reproduce

N/A

Versions

No response

Possible causes

No response

Additional context

No response

[4swinv]

Hello,
I've looked at this issue, but

1. I was not able to reproduce this issue on the current version. The partial trajectory is fully completed by the vehicle in autonomous mode.
2. For the example trajectory given,

idx	0	1	2	3	4	5	6	7
vel(m/s)	1.0	3.0	1.0	±0.0	-1.0	-3.0	-1.0	0.0

has velocity values that are zero, while the trajectory given to getReversingIndices would have a positive or negative non zero velocity value. Therefore the logic for finding the reversing indexes works.

createTrajectory function assigns this velocity profile before computing reversing indexes, each time it is called initially and during replanning in case of obstacles.

Trajectory createTrajectory(
  const PoseStamped & current_pose, const PlannerWaypoints & planner_waypoints,
  const double & velocity)
{
  Trajectory trajectory;
  trajectory.header = planner_waypoints.header;

  for (const auto & awp : planner_waypoints.waypoints) {
    TrajectoryPoint point;

    point.pose = awp.pose.pose;

    point.pose.position.z = current_pose.pose.position.z;  // height = const
    point.longitudinal_velocity_mps = velocity / 3.6;      // velocity = const

    // switch sign by forward/backward
    point.longitudinal_velocity_mps = (awp.is_back ? -1 : 1) * point.longitudinal_velocity_mps;

    trajectory.points.push_back(point);
  }

  return trajectory;
}

Assigning a non zero constant back/fwd velocity:

point.longitudinal_velocity_mps = velocity / 3.6; // velocity = const
// switch sign by forward/backward
point.longitudinal_velocity_mps = (awp.is_back ? -1 : 1) * point.longitudinal_velocity_mps;`

So the trajectory used as input to getReversingIndices then getPartialTrajectory would look like this:

idx	0	1	2	3	4	5	6	7
vel(m/s)	5.0	5.0	5.0	5.0	-5.0	-5.0	-5.0	-5.0

reversing index set = [3],
Partial trajectory:

idx	0	1	2	3
vel(m/s)	5.0	5.0	5.0	0.0

The output partial trajectories are given to the velocity smoother.
createStopTrajectory publishes a trajectory where the end point velocity is set to zero, but is called on replanning phase, where a new complete global trajectory is made using createTrajectory

[stale]

This pull request has been automatically marked as stale because it has not had recent activity.