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;
    iterative_anchoring_config_ = planner_open_space_config_.iterative_anchoring_smoother_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

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

                                                                                                                 {
    // 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();
    iterative_anchoring_config_ = iterative_anchoring_config;
    AINFO << "config:" << iterative_anchoring_config_.DebugString();
}

成员函数说明

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 第 68 行定义.

                                                       {
    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;
    }
    if (std::fabs(accumulated_s) < 0.1) {
      AERROR << "warm_start_path smaller than 0.1 m, can't enforce "
                  "heading continuity";
      return false;
    }
 
    const double interpolated_delta_s = iterative_anchoring_config_.interpolated_delta_s();
    std::vector<std::pair<double, double>> interpolated_warm_start_point2ds;
    double path_length = warm_start_path.Length();
    int count = std::ceil(path_length / interpolated_delta_s);
    double delta_s = path_length / (count < 4 ? 4 : count);
    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 < 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 第 112 行定义.

box_vec_vec_

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

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

---

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

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