Apollo 11.0
自动驾驶开放平台
Public 成员函数 | 所有成员列表
apollo::planning::SmoothStopTrajectoryFallback类 参考

#include <smooth_stop_trajectory_fallback.h>

类 apollo::planning::SmoothStopTrajectoryFallback 继承关系图:
apollo::planning::SmoothStopTrajectoryFallback 的协作图:

Public 成员函数

SpeedData GenerateFallbackSpeed (const EgoInfo *ego_info, const double stop_distance=0.0) override
 
- Public 成员函数 继承自 apollo::planning::TrajectoryFallbackTask
common::Status Execute (Frame *frame, ReferenceLineInfo *reference_line_info) override
 
- Public 成员函数 继承自 apollo::planning::Task
 Task ()
 
virtual ~Task ()=default
 
const std::string & Name () const
 
virtual bool Init (const std::string &config_dir, const std::string &name, const std::shared_ptr< DependencyInjector > &injector)
 
virtual common::Status Execute (Frame *frame)
 

额外继承的成员函数

- Protected 成员函数 继承自 apollo::planning::Task
template<typename T >
bool LoadConfig (T *config)
 
- Protected 属性 继承自 apollo::planning::Task
Frame * frame_
 
ReferenceLineInfo * reference_line_info_
 
std::shared_ptr< DependencyInjector > injector_
 
std::string config_path_
 
std::string default_config_path_
 
std::string name_
 

详细描述

在文件 smooth_stop_trajectory_fallback.h 第 33 行定义.

成员函数说明

◆ GenerateFallbackSpeed()

SpeedData apollo::planning::SmoothStopTrajectoryFallback::GenerateFallbackSpeed ( const EgoInfo *  ego_info,
const double  stop_distance = 0.0 
)
overridevirtual

实现了 apollo::planning::TrajectoryFallbackTask.

在文件 smooth_stop_trajectory_fallback.cc 第 35 行定义.

35 {
36 const double v0 = ego_info->start_point().v();
37 const double a0 = ego_info->start_point().a();
38 // Use the max value of jerk bound as max jerk value for fallback speed
39 // planning for convenience.
40 const double max_deceleration = FLAGS_slowdown_profile_deceleration;
41 // Get the lower bound to check collision.
42 std::vector<std::vector<Vec2d>> lower_bounds;
43 GetLowerSTBound(lower_bounds);
44 // Check the initial motion state to make sure the vehicle not move backward.
45 if (a0 <= 0.0) {
46 SpeedData speed_data;
47 if (v0 <= 0.0) {
48 // Make vehicle stop directly to get status: (v0 = 0, a0 = 0)
49 speed_data.AppendSpeedPoint(0.0, 0.0, 0.0, 0.0, 0.0);
50 AINFO << "Fallback speed: current speed is already 0! a0: " << a0 << " v0: " << v0;
51 SpeedProfileGenerator::FillEnoughSpeedPoints(&speed_data);
52 return speed_data;
53 } else {
54 double square_a0 = a0 * a0;
55 double min_jerk_required = square_a0 / (2.0 * v0);
56 if (FLAGS_longitudinal_jerk_upper_bound < min_jerk_required) {
57 double slow_down_time = -a0 / min_jerk_required;
58 // Make vehicle stop directly to get status: (v0 = 0, a0 = 0)
59 double s = 0.0;
60 double v = 0.0;
61 double a = 0.0;
62 double square_t = 0.0;
63 for (double t = 0.0; t < slow_down_time; t += FLAGS_fallback_time_unit) {
64 square_t = t * t;
65 a = a0 + t * min_jerk_required;
66 v = v0 + a0 * t + 0.5 * min_jerk_required * square_t;
67 s = v0 * t + 0.5 * a0 * square_t + min_jerk_required * square_t * t / 6.0;
68 speed_data.AppendSpeedPoint(s, t, v, a, min_jerk_required);
69 }
70 if (!IsCollisionWithSpeedBoundaries(speed_data)) {
71 AINFO << "Fallback speed: Make vehicle just stop to zero speed. a0: " << a0 << " v0: " << v0
72 << " new_jerk: " << min_jerk_required;
73 SpeedProfileGenerator::FillEnoughSpeedPoints(&speed_data);
74 return speed_data;
75 }
76 // Fast stop because of collision.
77 SpeedData fast_stop_speed_data;
78 SpeedPlanner::FastStop(v0, a0, max_deceleration, &fast_stop_speed_data);
79 SpeedProfileGenerator::FillEnoughSpeedPoints(&fast_stop_speed_data);
80 AINFO << "Fallback speed: Fast stop because of collision. a0: " << a0 << " v0: " << v0
81 << " dec: " << max_deceleration;
82 return fast_stop_speed_data;
83 }
84 }
85 }
86 // 1. Stop with max deceleration and check collision.
87 SpeedData fast_stop_speed_data;
88 SpeedPlanner::FastStop(v0, a0, max_deceleration, &fast_stop_speed_data);
89 if (IsCollisionWithSpeedBoundaries(fast_stop_speed_data)) {
90 SpeedProfileGenerator::FillEnoughSpeedPoints(&fast_stop_speed_data);
91 AINFO << "Fallback speed: Fast stop the vehicle. a0: " << a0 << " v0: " << v0 << " dec: " << max_deceleration;
92 return fast_stop_speed_data;
93 }
94
95 // 2. Stop smoothly and check collision.
96 SpeedData smooth_stop_speed_data;
97 SpeedPlanner::SmoothStop(
98 v0,
99 a0,
100 max_deceleration,
101 FLAGS_longitudinal_jerk_upper_bound,
102 FLAGS_longitudinal_jerk_lower_bound,
103 &smooth_stop_speed_data);
104 if (IsCollisionWithSpeedBoundaries(smooth_stop_speed_data)) {
105 // Need stop more quickly. Try to enlarge max jerk.
106 double max_jerk = fabs(max_deceleration / FLAGS_fallback_time_unit);
107 smooth_stop_speed_data.clear();
108 SpeedPlanner::SmoothStop(v0, a0, max_deceleration, max_jerk, max_jerk, &smooth_stop_speed_data);
109 if (IsCollisionWithSpeedBoundaries(smooth_stop_speed_data)) {
110 AINFO << "Fallback speed: Hard to stop smoothly and use still fast stop. "
111 "a0: "
112 << a0 << " v0: " << v0 << " dec: " << max_deceleration;
113 SpeedProfileGenerator::FillEnoughSpeedPoints(&fast_stop_speed_data);
114 return fast_stop_speed_data;
115 }
116 SpeedData last_no_collision_speed_data = smooth_stop_speed_data;
117 // Find the proper max_jerk with binary search.
118 double lower_jerk
119 = std::min(fabs(FLAGS_longitudinal_jerk_upper_bound), fabs(FLAGS_longitudinal_jerk_lower_bound));
120 double upper_jerk = max_jerk;
121 double mid_jerk = 0.0;
122 const double jerk_error = 1.0;
123 while (mid_jerk - lower_jerk > jerk_error) {
124 mid_jerk = (lower_jerk + upper_jerk) * 0.5;
125 SpeedPlanner::SmoothStop(v0, a0, max_deceleration, mid_jerk, mid_jerk, &smooth_stop_speed_data);
126 if (IsCollisionWithSpeedBoundaries(smooth_stop_speed_data)) {
127 lower_jerk = mid_jerk;
128 } else {
129 upper_jerk = mid_jerk;
130 last_no_collision_speed_data = smooth_stop_speed_data;
131 }
132 }
133 SpeedProfileGenerator::FillEnoughSpeedPoints(&last_no_collision_speed_data);
134 AINFO << "Fallback speed: Find new jerk to stop the vehilce. a0: " << a0 << " v0: " << v0
135 << " new jerk: " << mid_jerk;
136 return last_no_collision_speed_data;
137 }
138 AINFO << "Fallback speed: stop smoothly the vehilce. a0: " << a0 << " v0: " << v0 << " dec: " << max_deceleration;
139 SpeedProfileGenerator::FillEnoughSpeedPoints(&smooth_stop_speed_data);
140 return smooth_stop_speed_data;
141}
apollo::planning::SpeedPlanner::SmoothStop
static void SmoothStop(double v0, double a0, double max_deceleration, double max_pos_jerk, double max_neg_jerk, SpeedData *speed_data)
Definition speed_planner.cc:45
apollo::planning::SpeedPlanner::FastStop
static void FastStop(double v0, double a0, double max_deceleration, SpeedData *speed_data)
Definition speed_planner.cc:28
apollo::planning::SpeedProfileGenerator::FillEnoughSpeedPoints
static void FillEnoughSpeedPoints(SpeedData *const speed_data)
Definition speed_profile_generator.cc:36
AINFO
#define AINFO
Definition log.h:42
该类的文档由以下文件生成: