Misc backward driving issues in `motion_velocity_smoother`

[VRichardJP]

Checklist

• [X] I've read the contribution guidelines.
• [X] I've searched other issues and no duplicate issues were found.
• [X] I've agreed with the maintainers that I can plan this task.

Description

As I investigate freespace planners, I observe motion_velocity_smoother has many issues handling trajectories going backwards. The trajectories are assumed to go forward in many places but calculation are plain wrong if the path goes backward.

I would like to list all issues here.

• [ ] isEngageStatus() on backward engage
• [ ] MotionVelocitySmootherNode::calcExternalVelocityLimit() assumes forward drive
• [ ] applyExternalVelocityLimit
• [ ] reversed front_wheel_angle_rad on backward path

Purpose

Find and fix logic when backward trajectories are used.

Possible approaches

N/A

Definition of done

N/A

[VRichardJP]

isEngageStatus does not return true when engaging backward (i.e. target_vel < node_param_.engage_velocity)

https://github.com/autowarefoundation/autoware.universe/blob/a04f825bc542618fb86a347b45ae90162c1ff17e/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp#L1038-L1043

[VRichardJP]

MotionVelocitySmootherNode::calcExternalVelocityLimit() assumes forward drive:

https://github.com/autowarefoundation/autoware.universe/blob/a04f825bc542618fb86a347b45ae90162c1ff17e/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp#L362-L363

https://github.com/autowarefoundation/autoware.universe/blob/a04f825bc542618fb86a347b45ae90162c1ff17e/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp#L377-L384

when v0 < 0.0, max_velocity_with_deceleration_ is set to negative value, but it is supposedly absolute velocity.

[VRichardJP]

applyExternalVelocityLimit() logic looks strange on backward path:

• The function is called by calcTrajectoryVelocity() after input velocity has been "flipped" in onCurrentTrajectory:

https://github.com/autowarefoundation/autoware.universe/blob/a04f825bc542618fb86a347b45ae90162c1ff17e/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp#L468-L475

• After maximum velocity limit has been applied by trajectory_utils::applyMaximumVelocityLimit(), motion_utils::insertTargetPoint() is used to insert a point further on the path, from which the external speed limit will be applied. However, the functions will always fail on reversed path because of this check:

https://github.com/autowarefoundation/autoware.universe/blob/bf559863831b3b0bcaece8dcc1a6b15521ec6369/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp#L1370-L1373

And the external velocity limit is not applied on the path:

https://github.com/autowarefoundation/autoware.universe/blob/a04f825bc542618fb86a347b45ae90162c1ff17e/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp#L890-L894

I feel that if we wanted the same logic to apply to both forward and backward trajectories, the flipVelocity should rather flip every thing: orientation, velocity, acceleration, etc

[VRichardJP]

With the freespace planner in the planning simulator:

Turn left while going forward:

One of the middle point of /planning/scenario_planning/motion_velocity_smoother/trajectory trajectory shows front_wheel_angle_rad = 0.300...

Same but with vehicle and goal pose swapped:

One of the middle point shows front_wheel_angle_rad = -0.300...

We should get the same front_wheel_angle_rad in both cases, but in the reverse case it is as if the vehicle was turning right.

[VRichardJP]

So far, I have been able to fix all the issues above by refactoring the "velocity flipping" logic a bit and by replacing the flipVelocity function by this one:

void MotionVelocitySmootherNode::flipTrajectory(TrajectoryPoints & points) const
{
  for (auto & pt : points) {
    pt.pose = flipPose(pt.pose);
    pt.longitudinal_velocity_mps = -pt.longitudinal_velocity_mps;
    pt.lateral_velocity_mps = -pt.lateral_velocity_mps;
    // NOTE(VRichardJP) this is actually a directed acceleration so it should not be flipped
    // pt.acceleration_mps2 = -pt.acceleration_mps2;
    pt.heading_rate_rps = -pt.heading_rate_rps;
    pt.front_wheel_angle_rad = -pt.front_wheel_angle_rad;
    pt.rear_wheel_angle_rad = -pt.rear_wheel_angle_rad;
  }
}

which flips everything in the trajectory, not just velocity. I also do the same for all ego pose/twist while driving in reverse:

geometry_msgs::msg::Pose MotionVelocitySmootherNode::flipPose(const geometry_msgs::msg::Pose & pose) const
{
  geometry_msgs::msg::Pose output = pose;

  // rotation by 180 degrees
  tf2::Quaternion q_rot;
  q_rot.setRPY(0, 0, M_PI);

  tf2::Quaternion q;
  tf2::fromMsg(pose.orientation, q);
  q *= q_rot;
  output.orientation = tf2::toMsg(q);
  return output;
}

This way, flipped backward trajectories just looks like forward trajectories, which just need to be flipped at the end of processing.

[stale[bot]]

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