apollo::planning::OpenSpaceFallbackUtil类 参考

#include <open_space_fallback_util.h>

apollo::planning::OpenSpaceFallbackUtil 的协作图:

静态 Public 成员函数
static bool	IsCollisionFreeTrajectory (const TrajGearPair &trajectory_gear_pair, const std::vector< std::vector< common::math::Box2d > > &predicted_bounding_rectangles, const std::vector< Obstacle * > &static_obstacles, int *current_index, int *first_collision_index, bool &is_collision_with_static_obstacle, bool &is_collision_with_dynamic_obstacle, const double &collilsion_time_buffer)
static void	BuildPredictedEnvironment (const common::math::Vec2d ego_pos, const std::vector< const Obstacle * > &obstacles, std::vector< std::vector< common::math::Box2d > > &predicted_bounding_rectangles, const double &time_period, const double &build_range)
static void	BuildStaticObstacleEnvironment (const common::math::Vec2d ego_pos, const std::vector< const Obstacle * > &obstacles, std::vector< Obstacle * > &static_obstacles, const double &build_range)

详细描述

在文件 open_space_fallback_util.h 第 31 行定义.

成员函数说明

BuildPredictedEnvironment()

void apollo::planning::OpenSpaceFallbackUtil::BuildPredictedEnvironment ( const common::math::Vec2d  ego_pos, const std::vector< const Obstacle * > &  obstacles, std::vector< std::vector< common::math::Box2d > > &  predicted_bounding_rectangles, const double &  time_period, const double &  build_range  )

static

在文件 open_space_fallback_util.cc 第 98 行定义.

                               {
  predicted_bounding_rectangles.clear();
  double relative_time = 0.0;
  while (relative_time < time_period) {
    std::vector<Box2d> predicted_env;
    std::vector<Obstacle*> static_obstacles_env;
    for (const Obstacle* obstacle : obstacles) {
      if (!obstacle->IsVirtual() && !obstacle->IsStatic()) {
        TrajectoryPoint point = obstacle->GetPointAtTime(relative_time);
        if (ego_pos.DistanceTo(Vec2d(point.path_point().x(), point.path_point().y())) > build_range) {
          continue;
        }
        Box2d box = obstacle->GetBoundingBox(point);
        predicted_env.push_back(std::move(box));
      }
    }
    predicted_bounding_rectangles.emplace_back(std::move(predicted_env));
    relative_time += FLAGS_trajectory_time_resolution;
  }
}

BuildStaticObstacleEnvironment()

void apollo::planning::OpenSpaceFallbackUtil::BuildStaticObstacleEnvironment ( const common::math::Vec2d  ego_pos, const std::vector< const Obstacle * > &  obstacles, std::vector< Obstacle * > &  static_obstacles, const double &  build_range  )

static

在文件 open_space_fallback_util.cc 第 125 行定义.

                               {
  static_obstacles.clear();
  for (const Obstacle* obstacle : obstacles) {
    if (!obstacle->IsVirtual() && obstacle->IsStatic() &&
        obstacle->Perception().width() > 0.2 &&
        obstacle->Perception().length() > 0.2 &&
        obstacle->PerceptionPolygon().DistanceTo(ego_pos) <= build_range) {
      static_obstacles.push_back(const_cast<Obstacle*>(obstacle));
    }
  }
}

IsCollisionFreeTrajectory()

bool apollo::planning::OpenSpaceFallbackUtil::IsCollisionFreeTrajectory ( const TrajGearPair &  trajectory_gear_pair, const std::vector< std::vector< common::math::Box2d > > &  predicted_bounding_rectangles, const std::vector< Obstacle * > &  static_obstacles, int *  current_index, int *  first_collision_index, bool &  is_collision_with_static_obstacle, bool &  is_collision_with_dynamic_obstacle, const double &  collilsion_time_buffer  )

static

在文件 open_space_fallback_util.cc 第 31 行定义.

                                                                                    {
  // prediction time resolution: FLAGS_trajectory_time_resolution
  const auto& vehicle_config =
      common::VehicleConfigHelper::Instance()->GetConfig();
  double ego_length = vehicle_config.vehicle_param().length();
  double ego_width = vehicle_config.vehicle_param().width();
  auto trajectory_pb = trajectory_gear_pair.first;
  const int point_size = trajectory_pb.NumOfPoints();
  AINFO << "trajectory_pb.NumOfPoints: [" << point_size << "]";
  *current_index = trajectory_pb.QueryLowerBoundPoint(0.0);
 
  for (int i = *current_index; i < point_size; ++i) {
    const auto& trajectory_point = trajectory_pb.TrajectoryPointAt(i);
    double ego_theta = trajectory_point.path_point().theta();
    Box2d ego_box(
        {trajectory_point.path_point().x(), trajectory_point.path_point().y()},
        ego_theta, ego_length, ego_width);
    double shift_distance =
        ego_length / 2.0 - vehicle_config.vehicle_param().back_edge_to_center();
    Vec2d shift_vec{shift_distance * std::cos(ego_theta),
                    shift_distance * std::sin(ego_theta)};
    ego_box.Shift(shift_vec);
 
    // is collision with static obstacles
    Polygon2d ego_polygon = Polygon2d(ego_box);
    for (const Obstacle* obstacle : static_obstacles) {
      Polygon2d obstacle_polygon = obstacle->PerceptionPolygon();
      if (ego_polygon.HasOverlap(obstacle_polygon)) {
        AINFO << "HasOverlapWithStaticObstacle" << obstacle->Id()
               << "first_collision_index_of_static_obstacle: [" << i << "]";
        *first_collision_index = i;
        is_collision_with_static_obstacle = true;
        return false;
      }
    }
 
    // is collision with dynamic obstacles
    int predicted_time_horizon = predicted_bounding_rectangles.size();
    for (int j = 0; j < predicted_time_horizon; j++) {
      for (const auto& obstacle_box : predicted_bounding_rectangles[j]) {
        if (ego_box.HasOverlap(obstacle_box)) {
          ADEBUG << "HasOverlap(obstacle_box) [" << i << "]";
          // remove points in previous trajectory
          if (std::abs(trajectory_point.relative_time() -
                       static_cast<double>(j) *
                           FLAGS_trajectory_time_resolution) <
                  collilsion_time_buffer &&
              trajectory_point.relative_time() > 0.0) {
            ADEBUG << "first_collision_index: [" << i << "]";
            *first_collision_index = i;
            is_collision_with_dynamic_obstacle = true;
            return false;
          }
        }
      }
    }
  }
 
  return true;
}

---

该类的文档由以下文件生成:

• modules/planning/planning_open_space/utils/open_space_fallback_util.h
• modules/planning/planning_open_space/utils/open_space_fallback_util.cc