apollo::planning::IterativeAnchoringSmoother类 参考

#include <iterative_anchoring_smoother.h>

apollo::planning::IterativeAnchoringSmoother 的协作图:

Public 成员函数
	IterativeAnchoringSmoother (const PlannerOpenSpaceConfig &planner_open_space_config)
	~IterativeAnchoringSmoother ()=default
bool	Smooth (const Eigen::MatrixXd &xWS, const double init_a, const double init_v, const std::vector< std::vector< common::math::Vec2d > > &obstacles_vertices_vec, DiscretizedTrajectory *discretized_trajectory)
	IterativeAnchoringSmoother (const IterativeAnchoringConfig &iterative_anchoring_config)

Public 属性
std::vector< std::vector< common::math::Vec2d > >	box_vec_
std::vector< std::vector< std::vector< common::math::Vec2d > > >	box_vec_vec_

详细描述

在文件 iterative_anchoring_smoother.h 第 43 行定义.

构造及析构函数说明

IterativeAnchoringSmoother() [1/2]

apollo::planning::IterativeAnchoringSmoother::IterativeAnchoringSmoother

(

const PlannerOpenSpaceConfig &

planner_open_space_config

)

在文件 iterative_anchoring_smoother.cc 第 47 行定义.

                                                             {
  // TODO(Jinyun, Yu): refactor after stabilized.
  const auto& vehicle_param =
      common::VehicleConfigHelper::Instance()->GetConfig().vehicle_param();
  ego_length_ = vehicle_param.length();
  ego_width_ = vehicle_param.width();
  center_shift_distance_ =
      ego_length_ / 2.0 - vehicle_param.back_edge_to_center();
  planner_open_space_config_ = planner_open_space_config;
  AINFO << "config:" << planner_open_space_config_.DebugString();
}

~IterativeAnchoringSmoother()

apollo::planning::IterativeAnchoringSmoother::~IterativeAnchoringSmoother ( )

default

IterativeAnchoringSmoother() [2/2]

apollo::planning::IterativeAnchoringSmoother::IterativeAnchoringSmoother ( const IterativeAnchoringConfig &  iterative_anchoring_config )

explicit

成员函数说明

Smooth()

bool apollo::planning::IterativeAnchoringSmoother::Smooth	(	const Eigen::MatrixXd &	xWS,
		const double	init_a,
		const double	init_v,
		const std::vector< std::vector< common::math::Vec2d > > &	obstacles_vertices_vec,
		DiscretizedTrajectory *	discretized_trajectory
	)

在文件 iterative_anchoring_smoother.cc 第 60 行定义.

                                                   {
  if (xWS.cols() < 2) {
    AERROR << "reference points size smaller than two, smoother early "
              "returned";
    return false;
  }
  const auto start_timestamp = std::chrono::system_clock::now();
 
  // Set gear of the trajectory
  gear_ = CheckGear(xWS);
 
  // Set obstacle in form of linesegments
  std::vector<std::vector<LineSegment2d>> obstacles_linesegments_vec;
  for (const auto& obstacle_vertices : obstacles_vertices_vec) {
    size_t vertices_num = obstacle_vertices.size();
    std::vector<LineSegment2d> obstacle_linesegments;
    for (size_t i = 0; i + 1 < vertices_num; ++i) {
      LineSegment2d line_segment =
          LineSegment2d(obstacle_vertices[i], obstacle_vertices[i + 1]);
      obstacle_linesegments.emplace_back(line_segment);
    }
    obstacles_linesegments_vec.emplace_back(obstacle_linesegments);
  }
  obstacles_linesegments_vec_ = std::move(obstacles_linesegments_vec);
 
  // Interpolate the traj
  DiscretizedPath warm_start_path;
  size_t xWS_size = xWS.cols();
  double accumulated_s = 0.0;
  Vec2d last_path_point(xWS(0, 0), xWS(1, 0));
  for (size_t i = 0; i < xWS_size; ++i) {
    Vec2d cur_path_point(xWS(0, i), xWS(1, i));
    accumulated_s += cur_path_point.DistanceTo(last_path_point);
    PathPoint path_point;
    path_point.set_x(xWS(0, i));
    path_point.set_y(xWS(1, i));
    path_point.set_theta(xWS(2, i));
    path_point.set_s(accumulated_s);
    warm_start_path.push_back(std::move(path_point));
    last_path_point = cur_path_point;
  }
 
  const double interpolated_delta_s =
      planner_open_space_config_.iterative_anchoring_smoother_config()
          .interpolated_delta_s();
  std::vector<std::pair<double, double>> interpolated_warm_start_point2ds;
  double path_length = warm_start_path.Length();
  double delta_s = path_length / std::ceil(path_length / interpolated_delta_s);
  path_length += delta_s * 1.0e-6;
  for (double s = 0; s < path_length; s += delta_s) {
    const auto point2d = warm_start_path.Evaluate(s);
    interpolated_warm_start_point2ds.emplace_back(point2d.x(), point2d.y());
  }
 
  const size_t interpolated_size = interpolated_warm_start_point2ds.size();
  if (interpolated_size < 4) {
    AERROR << "interpolated_warm_start_path smaller than 4, can't enforce "
              "heading continuity";
    return false;
  } else if (interpolated_size < 6) {
    ADEBUG << "interpolated_warm_start_path smaller than 4, can't enforce "
              "initial zero kappa";
    enforce_initial_kappa_ = false;
  } else {
    enforce_initial_kappa_ = true;
  }
 
  // Adjust heading to ensure heading continuity
  AdjustStartEndHeading(xWS, &interpolated_warm_start_point2ds);
 
  // Reset path profile by discrete point heading and curvature estimation
  DiscretizedPath interpolated_warm_start_path;
  if (!SetPathProfile(interpolated_warm_start_point2ds,
                      &interpolated_warm_start_path)) {
    AERROR << "Set path profile fails";
    return false;
  }
 
  // Generate feasible bounds for each path point
  std::vector<double> bounds;
  if (!GenerateInitialBounds(interpolated_warm_start_path, &bounds)) {
    AERROR << "Generate initial bounds failed, path point to close to obstacle";
    return false;
  }
 
  // Check initial path collision avoidance, if it fails, smoother assumption
  // fails. Try reanchoring
  input_colliding_point_index_.clear();
  if (!CheckCollisionAvoidance(interpolated_warm_start_path,
                               &input_colliding_point_index_)) {
    AERROR << "Interpolated input path points colliding with obstacle";
    // if (!ReAnchoring(colliding_point_index, &interpolated_warm_start_path)) {
    //   AERROR << "Fail to reanchor colliding input path points";
    //   return false;
    // }
  }
 
  const auto path_smooth_start_timestamp = std::chrono::system_clock::now();
 
  // Smooth path to have smoothed x, y, phi, kappa and s
  DiscretizedPath smoothed_path_points;
  if (!SmoothPath(interpolated_warm_start_path, bounds,
                  &smoothed_path_points)) {
    return false;
  }
  // PrintCurves print_curve;
  // for (size_t i = 0; i < smoothed_path_points.size(); i++) {
  //   print_curve.AddPoint("iter_smoothed_xy", smoothed_path_points[i].x(),
  //                        smoothed_path_points[i].y());
  //   print_curve.AddPoint("iter_smoothed_s-kappa",
  //   smoothed_path_points[i].s(),
  //                        smoothed_path_points[i].kappa());
  //   print_curve.AddPoint("iter_smoothed_s-theta",
  //   smoothed_path_points[i].s(),
  //                        smoothed_path_points[i].theta());
  // }
  // for (size_t i = 0; i < interpolated_warm_start_path.size(); i++) {
  //   print_curve.AddPoint("iter_warm_xy", interpolated_warm_start_path[i].x(),
  //                        interpolated_warm_start_path[i].y());
  //   print_curve.AddPoint("iter_warm_s-kappa",
  //                        interpolated_warm_start_path[i].s(),
  //                        interpolated_warm_start_path[i].kappa());
  //   print_curve.AddPoint("iter_warm_s-theta",
  //                        interpolated_warm_start_path[i].s(),
  //                        interpolated_warm_start_path[i].theta());
  // }
  // print_curve.PrintToLog();
  const auto path_smooth_end_timestamp = std::chrono::system_clock::now();
  std::chrono::duration<double> path_smooth_diff =
      path_smooth_end_timestamp - path_smooth_start_timestamp;
  ADEBUG << "iterative anchoring path smoother time: "
         << path_smooth_diff.count() * 1000.0 << " ms.";
 
  const auto speed_smooth_start_timestamp = std::chrono::system_clock::now();
 
  // Smooth speed to have smoothed v and a
  SpeedData smoothed_speeds;
  if (!SmoothSpeed(init_a, init_v, smoothed_path_points.Length(),
                   &smoothed_speeds)) {
    return false;
  }
 
  // TODO(Jinyun): Evaluate performance
  // SpeedData smoothed_speeds;
  // if (!GenerateStopProfileFromPolynomial(
  //         init_a, init_v, smoothed_path_points.Length(), &smoothed_speeds)) {
  //   return false;
  // }
 
  const auto speed_smooth_end_timestamp = std::chrono::system_clock::now();
  std::chrono::duration<double> speed_smooth_diff =
      speed_smooth_end_timestamp - speed_smooth_start_timestamp;
  ADEBUG << "iterative anchoring speed smoother time: "
         << speed_smooth_diff.count() * 1000.0 << " ms.";
 
  // Combine path and speed
  if (!CombinePathAndSpeed(smoothed_path_points, smoothed_speeds,
                           discretized_trajectory)) {
    return false;
  }
 
  AdjustPathAndSpeedByGear(discretized_trajectory);
 
  const auto end_timestamp = std::chrono::system_clock::now();
  std::chrono::duration<double> diff = end_timestamp - start_timestamp;
  ADEBUG << "iterative anchoring smoother total time: " << diff.count() * 1000.0
         << " ms.";
 
  ADEBUG << "discretized_trajectory size " << discretized_trajectory->size();
  return true;
}

类成员变量说明

box_vec_

std::vector<std::vector<common::math::Vec2d> > apollo::planning::IterativeAnchoringSmoother::box_vec_

在文件 iterative_anchoring_smoother.h 第 120 行定义.

box_vec_vec_

std::vector<std::vector<std::vector<common::math::Vec2d> > > apollo::planning::IterativeAnchoringSmoother::box_vec_vec_

在文件 iterative_anchoring_smoother.h 第 121 行定义.

---

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

• modules/planning/planning_open_space/trajectory_smoother/iterative_anchoring_smoother.h
• modules/planning/planning_open_space/trajectory_smoother/iterative_anchoring_smoother.cc