Obstacle Avoidance Planner

Purpose

This package generates a trajectory that is kinematically-feasible to drive and collision-free based on the input path, drivable area. Only position and orientation of trajectory are updated in this module, and velocity is just taken over from the one in the input path.

Feature

This package is able to

• make the trajectory inside the drivable area as much as possible
  • NOTE: Static obstacles to avoid can be removed from the drivable area.
• insert stop point before the planned footprint will be outside the drivable area

Note that the velocity is just taken over from the input path.

Inputs / Outputs

input

output

Flowchart

Flowchart of functions is explained here.

@startuml
title pathCallback
start

:isDataReady;

:createPlannerData;

group generateOptimizedTrajectory
  group optimizeTrajectory
    :check replan;
    if (replanning required?) then (yes)
      :getEBTrajectory;
      :getModelPredictiveTrajectory;
      if (optimization failed?) then (no)
      else (yes)
        :send previous\n trajectory;
      endif
    else (no)
      :send previous\n trajectory;
    endif
  end group

  :applyInputVelocity;
  :insertZeroVelocityOutsideDrivableArea;
  :publishDebugMarkerOfOptimization;
end group


:extendTrajectory;

:setZeroVelocityAfterStopPoint;

:publishDebugData;

stop
@enduml

createPlannerData

The following data for planning is created.

struct PlannerData
{
  // input
  Header header;
  std::vector<TrajectoryPoint> traj_points; // converted from the input path
  std::vector<geometry_msgs::msg::Point> left_bound;
  std::vector<geometry_msgs::msg::Point> right_bound;

  // ego
  geometry_msgs::msg::Pose ego_pose;
  double ego_vel;
};

check replan

When one of the following conditions are met, trajectory optimization will be executed. Otherwise, previously optimized trajectory is used with updating the velocity from the latest input path.

File truncated at 100 lines see the full file