Apollo 10.0
自动驾驶开放平台
apollo::control::TrajectoryAnalyzer类 参考

process point query and conversion related to trajectory 更多...

#include <trajectory_analyzer.h>

apollo::control::TrajectoryAnalyzer 的协作图:

Public 成员函数

 TrajectoryAnalyzer ()=default
 constructor
 
 TrajectoryAnalyzer (const planning::ADCTrajectory *planning_published_trajectory)
 constructor
 
 ~TrajectoryAnalyzer ()=default
 destructor
 
unsigned int seq_num ()
 get sequence number of the trajectory
 
common::TrajectoryPoint QueryNearestPointByAbsoluteTime (const double t) const
 query a point of trajectory that its absolute time is closest to the give time.
 
common::TrajectoryPoint QueryNearestPointByRelativeTime (const double t) const
 query a point of trajectory that its relative time is closest to the give time.
 
common::TrajectoryPoint QueryNearestPointByPosition (const double x, const double y) const
 query a point of trajectory that its position is closest to the given position.
 
common::PathPoint QueryMatchedPathPoint (const double x, const double y) const
 query a point on trajectory that its position is closest to the given position.
 
void ToTrajectoryFrame (const double x, const double y, const double theta, const double v, const common::PathPoint &matched_point, double *ptr_s, double *ptr_s_dot, double *ptr_d, double *ptr_d_dot) const
 convert a position with theta and speed to trajectory frame,
 
void TrajectoryTransformToCOM (const double rear_to_com_distance)
 Transform the current trajectory points to the center of mass(COM) of the vehicle, given the distance from rear wheels to the center of mass.
 
common::math::Vec2d ComputeCOMPosition (const double rear_to_com_distance, const common::PathPoint &path_point) const
 Compute the position of center of mass(COM) of the vehicle, given the distance from rear wheels to the center of mass.
 
const std::vector< common::TrajectoryPoint > & trajectory_points () const
 get all points of the trajectory
 

Protected 属性

std::vector< common::TrajectoryPointtrajectory_points_
 
double header_time_ = 0.0
 
unsigned int seq_num_ = 0
 

详细描述

process point query and conversion related to trajectory

在文件 trajectory_analyzer.h44 行定义.

构造及析构函数说明

◆ TrajectoryAnalyzer() [1/2]

apollo::control::TrajectoryAnalyzer::TrajectoryAnalyzer ( )
default

constructor

◆ TrajectoryAnalyzer() [2/2]

apollo::control::TrajectoryAnalyzer::TrajectoryAnalyzer ( const planning::ADCTrajectory planning_published_trajectory)

constructor

参数
planning_published_trajectorytrajectory data generated by planning module

在文件 trajectory_analyzer.cc56 行定义.

57 {
58 header_time_ = planning_published_trajectory->header().timestamp_sec();
59 seq_num_ = planning_published_trajectory->header().sequence_num();
60
61 for (int i = 0; i < planning_published_trajectory->trajectory_point_size();
62 ++i) {
63 trajectory_points_.push_back(
64 planning_published_trajectory->trajectory_point(i));
65 auto *path_point = trajectory_points_.back().mutable_path_point();
66 if (!path_point->has_z()) {
67 path_point->set_z(0.0);
68 }
69 }
70}
std::vector< common::TrajectoryPoint > trajectory_points_

◆ ~TrajectoryAnalyzer()

apollo::control::TrajectoryAnalyzer::~TrajectoryAnalyzer ( )
default

destructor

成员函数说明

◆ ComputeCOMPosition()

common::math::Vec2d apollo::control::TrajectoryAnalyzer::ComputeCOMPosition ( const double  rear_to_com_distance,
const common::PathPoint path_point 
) const

Compute the position of center of mass(COM) of the vehicle, given the distance from rear wheels to the center of mass.

参数
rear_to_com_distanceDistance from rear wheels to the vehicle's center of mass.
path_pointPathPoint along the published planning trajectory.
返回
The position of the vehicle's center of mass.

在文件 trajectory_analyzer.cc253 行定义.

254 {
255 // Initialize the vector for coordinate transformation of the position
256 // reference point
257 Eigen::Vector3d v;
258 const double cos_heading = std::cos(path_point.theta());
259 const double sin_heading = std::sin(path_point.theta());
260 v << rear_to_com_distance * cos_heading, rear_to_com_distance * sin_heading,
261 0.0;
262 // Original position reference point at center of rear-axis
263 Eigen::Vector3d pos_vec(path_point.x(), path_point.y(), path_point.z());
264 // Transform original position with vector v
265 Eigen::Vector3d com_pos_3d = v + pos_vec;
266 // Return transfromed x and y
267 return common::math::Vec2d(com_pos_3d[0], com_pos_3d[1]);
268}

◆ QueryMatchedPathPoint()

PathPoint apollo::control::TrajectoryAnalyzer::QueryMatchedPathPoint ( const double  x,
const double  y 
) const

query a point on trajectory that its position is closest to the given position.

参数
xvalue of x-coordination in the given position
yvalue of y-coordination in the given position
返回
a point on trajectory, the point may be a point of trajectory or interpolated by two adjacent points of trajectory

在文件 trajectory_analyzer.cc72 行定义.

73 {
74 CHECK_GT(trajectory_points_.size(), 0U);
75
76 double d_min = PointDistanceSquare(trajectory_points_.front(), x, y);
77 size_t index_min = 0;
78
79 for (size_t i = 1; i < trajectory_points_.size(); ++i) {
80 double d_temp = PointDistanceSquare(trajectory_points_[i], x, y);
81 if (d_temp < d_min) {
82 d_min = d_temp;
83 index_min = i;
84 }
85 }
86
87 size_t index_start = index_min == 0 ? index_min : index_min - 1;
88 size_t index_end =
89 index_min + 1 == trajectory_points_.size() ? index_min : index_min + 1;
90
91 const double kEpsilon = 0.001;
92 if (index_start == index_end ||
93 std::fabs(trajectory_points_[index_start].path_point().s() -
94 trajectory_points_[index_end].path_point().s()) <= kEpsilon) {
95 return TrajectoryPointToPathPoint(trajectory_points_[index_start]);
96 }
97
98 return FindMinDistancePoint(trajectory_points_[index_start],
99 trajectory_points_[index_end], x, y);
100}

◆ QueryNearestPointByAbsoluteTime()

TrajectoryPoint apollo::control::TrajectoryAnalyzer::QueryNearestPointByAbsoluteTime ( const double  t) const

query a point of trajectory that its absolute time is closest to the give time.

参数
tabsolute time for query
返回
a point of trajectory

在文件 trajectory_analyzer.cc154 行定义.

155 {
157}
common::TrajectoryPoint QueryNearestPointByRelativeTime(const double t) const
query a point of trajectory that its relative time is closest to the give time.

◆ QueryNearestPointByPosition()

TrajectoryPoint apollo::control::TrajectoryAnalyzer::QueryNearestPointByPosition ( const double  x,
const double  y 
) const

query a point of trajectory that its position is closest to the given position.

参数
xvalue of x-coordination in the given position
yvalue of y-coordination in the given position
返回
a point of trajectory

在文件 trajectory_analyzer.cc188 行定义.

189 {
190 double d_min = PointDistanceSquare(trajectory_points_.front(), x, y);
191 size_t index_min = 0;
192
193 for (size_t i = 1; i < trajectory_points_.size(); ++i) {
194 double d_temp = PointDistanceSquare(trajectory_points_[i], x, y);
195 if (d_temp < d_min) {
196 d_min = d_temp;
197 index_min = i;
198 }
199 }
200 return trajectory_points_[index_min];
201}

◆ QueryNearestPointByRelativeTime()

TrajectoryPoint apollo::control::TrajectoryAnalyzer::QueryNearestPointByRelativeTime ( const double  t) const

query a point of trajectory that its relative time is closest to the give time.

The time is relative to the first pointof trajectory

参数
trelative time for query
返回
a point of trajectory

在文件 trajectory_analyzer.cc159 行定义.

160 {
161 auto func_comp = [](const TrajectoryPoint &point,
162 const double relative_time) {
163 return point.relative_time() < relative_time;
164 };
165
166 auto it_low = std::lower_bound(trajectory_points_.begin(),
167 trajectory_points_.end(), t, func_comp);
168
169 if (it_low == trajectory_points_.begin()) {
170 return trajectory_points_.front();
171 }
172
173 if (it_low == trajectory_points_.end()) {
174 return trajectory_points_.back();
175 }
176
177 if (FLAGS_query_forward_time_point_only) {
178 return *it_low;
179 } else {
180 auto it_lower = it_low - 1;
181 if (it_low->relative_time() - t < t - it_lower->relative_time()) {
182 return *it_low;
183 }
184 return *it_lower;
185 }
186}

◆ seq_num()

unsigned int apollo::control::TrajectoryAnalyzer::seq_num ( )
inline

get sequence number of the trajectory

返回
sequence number.

在文件 trajectory_analyzer.h68 行定义.

68{ return seq_num_; }

◆ ToTrajectoryFrame()

void apollo::control::TrajectoryAnalyzer::ToTrajectoryFrame ( const double  x,
const double  y,
const double  theta,
const double  v,
const common::PathPoint matched_point,
double *  ptr_s,
double *  ptr_s_dot,
double *  ptr_d,
double *  ptr_d_dot 
) const

convert a position with theta and speed to trajectory frame,

  • longitudinal and lateral direction to the trajectory
    参数
    xx-value of the position
    yy-value of the position
    thetaheading angle on the position
    vspeed on the position
    matched_pointmatched point on trajectory for the given position
    ptr_slongitudinal distance
    ptr_s_dotlongitudinal speed
    ptr_dlateral distance
    ptr_d_dotlateral speed

在文件 trajectory_analyzer.cc109 行定义.

114 {
115 double dx = x - ref_point.x();
116 double dy = y - ref_point.y();
117
118 double cos_ref_theta = std::cos(ref_point.theta());
119 double sin_ref_theta = std::sin(ref_point.theta());
120
121 // the sin of diff angle between vector (cos_ref_theta, sin_ref_theta) and
122 // (dx, dy)
123 double cross_rd_nd = cos_ref_theta * dy - sin_ref_theta * dx;
124 *ptr_d = cross_rd_nd;
125
126 // the cos of diff angle between vector (cos_ref_theta, sin_ref_theta) and
127 // (dx, dy)
128 double dot_rd_nd = dx * cos_ref_theta + dy * sin_ref_theta;
129 *ptr_s = ref_point.s() + dot_rd_nd;
130
131 double delta_theta = theta - ref_point.theta();
132 double cos_delta_theta = std::cos(delta_theta);
133 double sin_delta_theta = std::sin(delta_theta);
134
135 *ptr_d_dot = v * sin_delta_theta;
136
137 double one_minus_kappa_r_d = 1 - ref_point.kappa() * (*ptr_d);
138 if (one_minus_kappa_r_d <= 0.0) {
139 AERROR << "TrajectoryAnalyzer::ToTrajectoryFrame "
140 "found fatal reference and actual difference. "
141 "Control output might be unstable:"
142 << " ref_point.kappa:" << ref_point.kappa()
143 << " ref_point.x:" << ref_point.x()
144 << " ref_point.y:" << ref_point.y() << " car x:" << x
145 << " car y:" << y << " *ptr_d:" << *ptr_d
146 << " one_minus_kappa_r_d:" << one_minus_kappa_r_d;
147 // currently set to a small value to avoid control crash.
148 one_minus_kappa_r_d = 0.01;
149 }
150
151 *ptr_s_dot = v * cos_delta_theta / one_minus_kappa_r_d;
152}
#define AERROR
Definition log.h:44

◆ trajectory_points()

const std::vector< TrajectoryPoint > & apollo::control::TrajectoryAnalyzer::trajectory_points ( ) const

get all points of the trajectory

返回
a vector of trajectory points

在文件 trajectory_analyzer.cc203 行定义.

204 {
205 return trajectory_points_;
206}

◆ TrajectoryTransformToCOM()

void apollo::control::TrajectoryAnalyzer::TrajectoryTransformToCOM ( const double  rear_to_com_distance)

Transform the current trajectory points to the center of mass(COM) of the vehicle, given the distance from rear wheels to the center of mass.

参数
rear_to_com_distanceDistance from rear wheels to the vehicle's center of mass.

在文件 trajectory_analyzer.cc242 行定义.

243 {
244 CHECK_GT(trajectory_points_.size(), 0U);
245 for (size_t i = 0; i < trajectory_points_.size(); ++i) {
246 auto com = ComputeCOMPosition(rear_to_com_distance,
247 trajectory_points_[i].path_point());
248 trajectory_points_[i].mutable_path_point()->set_x(com.x());
249 trajectory_points_[i].mutable_path_point()->set_y(com.y());
250 }
251}
common::math::Vec2d ComputeCOMPosition(const double rear_to_com_distance, const common::PathPoint &path_point) const
Compute the position of center of mass(COM) of the vehicle, given the distance from rear wheels to th...

类成员变量说明

◆ header_time_

double apollo::control::TrajectoryAnalyzer::header_time_ = 0.0
protected

在文件 trajectory_analyzer.h153 行定义.

◆ seq_num_

unsigned int apollo::control::TrajectoryAnalyzer::seq_num_ = 0
protected

在文件 trajectory_analyzer.h154 行定义.

◆ trajectory_points_

std::vector<common::TrajectoryPoint> apollo::control::TrajectoryAnalyzer::trajectory_points_
protected

在文件 trajectory_analyzer.h151 行定义.


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