Obstacle Cruise Planner

Overview

The obstacle_cruise_planner package has following modules.

• Stop planning
  • stop when there is a static obstacle near the trajectory.
• Cruise planning
  • cruise a dynamic obstacle in front of the ego.
• Slow down planning
  • slow down when there is a static/dynamic obstacle near the trajectory.

Interfaces

Input topics

Output topics

Design

Design for the following functions is defined here.

• Behavior determination against obstacles
• Stop planning
• Cruise planning
• Slow down planning

A data structure for cruise and stop planning is as follows. This planner data is created first, and then sent to the planning algorithm.

struct PlannerData
{
  rclcpp::Time current_time;
  autoware_auto_planning_msgs::msg::Trajectory traj;
  geometry_msgs::msg::Pose current_pose;
  double ego_vel;
  double current_acc;
  std::vector<Obstacle> target_obstacles;
};

struct Obstacle
{
  rclcpp::Time stamp;  // This is not the current stamp, but when the object was observed.
  geometry_msgs::msg::Pose pose;  // interpolated with the current stamp
  bool orientation_reliable;
  Twist twist;
  bool twist_reliable;
  ObjectClassification classification;
  std::string uuid;
  Shape shape;
  std::vector<PredictedPath> predicted_paths;
};

Behavior determination against obstacles

Obstacles for cruising, stopping and slowing down are selected in this order based on their pose and velocity. The obstacles not in front of the ego will be ignored.

Determine cruise vehicles

The obstacles meeting the following condition are determined as obstacles for cruising.

• The lateral distance from the object to the ego’s trajectory is smaller than behavior_determination.cruise.max_lat_margin.

• The object type is for cruising according to common.cruise_obstacle_type.*.
• The object is not crossing the ego’s trajectory (*1).
• If the object is inside the trajectory.
  • The object type is for inside cruising according to common.cruise_obstacle_type.inside.*.
  • The object velocity is larger than behavior_determination.obstacle_velocity_threshold_from_cruise_to_stop.
• If the object is outside the trajectory.
  • The object type is for outside cruising according to common.cruise_obstacle_type.outside.*.
  • The object velocity is larger than behavior_determination.cruise.outside_obstacle.obstacle_velocity_threshold.
  • The highest confident predicted path collides with the ego’s trajectory.
  • Its collision’s period is larger than behavior_determination.cruise.outside_obstacle.ego_obstacle_overlap_time_threshold.

File truncated at 100 lines see the full file