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

#include <reeds_shepp_path.h>

apollo::planning::ReedShepp 的协作图:

Public 成员函数

 ReedShepp (const common::VehicleParam &vehicle_param, const PlannerOpenSpaceConfig &open_space_conf)
 
virtual ~ReedShepp ()=default
 
bool ShortestRSP (const std::shared_ptr< Node3d > start_node, const std::shared_ptr< Node3d > end_node, std::shared_ptr< ReedSheppPath > optimal_path)
 
 ReedShepp (const common::VehicleParam &vehicle_param, const WarmStartConfig &warm_start_conf)
 

Public 属性

bool reeds_sheep_last_straight_ = false
 

Protected 成员函数

bool GenerateRSPs (const std::shared_ptr< Node3d > start_node, const std::shared_ptr< Node3d > end_node, std::vector< ReedSheppPath > *all_possible_paths)
 
bool GenerateRSP (const std::shared_ptr< Node3d > start_node, const std::shared_ptr< Node3d > end_node, std::vector< ReedSheppPath > *all_possible_paths)
 
bool GenerateRSPPar (const std::shared_ptr< Node3d > start_node, const std::shared_ptr< Node3d > end_node, std::vector< ReedSheppPath > *all_possible_paths)
 
bool GenerateLocalConfigurations (const std::shared_ptr< Node3d > start_node, const std::shared_ptr< Node3d > end_node, ReedSheppPath *shortest_path)
 
void Interpolation (const int index, const double pd, const char m, const double ox, const double oy, const double ophi, const double radius, std::vector< double > *px, std::vector< double > *py, std::vector< double > *pphi, std::vector< bool > *pgear)
 
bool SetRSP (const int size, const double *lengths, const char *types, std::vector< ReedSheppPath > *all_possible_paths, double radius=1.0)
 
bool SetRSPPar (const int size, const double *lengths, const std::string &types, std::vector< ReedSheppPath > *all_possible_paths, const int idx)
 
bool CS (const double x, const double y, const double phi, std::vector< ReedSheppPath > *all_possible_paths)
 
bool SCS (const double x, const double y, const double phi, std::vector< ReedSheppPath > *all_possible_paths)
 
bool CSC (const double x, const double y, const double phi, std::vector< ReedSheppPath > *all_possible_paths)
 
bool CCC (const double x, const double y, const double phi, std::vector< ReedSheppPath > *all_possible_paths)
 
bool CCCC (const double x, const double y, const double phi, std::vector< ReedSheppPath > *all_possible_paths)
 
bool CCSC (const double x, const double y, const double phi, std::vector< ReedSheppPath > *all_possible_paths)
 
bool CCSCC (const double x, const double y, const double phi, std::vector< ReedSheppPath > *all_possible_paths)
 
void LSL (const double x, const double y, const double phi, RSPParam *param)
 
void LSR (const double x, const double y, const double phi, RSPParam *param)
 
void LRL (const double x, const double y, const double phi, RSPParam *param)
 
void SLS (const double x, const double y, const double phi, RSPParam *param)
 
void LRLRn (const double x, const double y, const double phi, RSPParam *param)
 
void LRLRp (const double x, const double y, const double phi, RSPParam *param)
 
void LRSR (const double x, const double y, const double phi, RSPParam *param)
 
void LRSL (const double x, const double y, const double phi, RSPParam *param)
 
void LRSLR (const double x, const double y, const double phi, RSPParam *param)
 
void LS (const double x, const double y, const double phi, RSPParam *param)
 
std::pair< double, double > calc_tau_omega (const double u, const double v, const double xi, const double eta, const double phi)
 

Protected 属性

common::VehicleParam vehicle_param_
 
PlannerOpenSpaceConfig planner_open_space_config_
 
double max_kappa_
 
double traj_forward_penalty_
 
double traj_back_penalty_
 
double traj_gear_switch_penalty_
 
double traj_steer_penalty_
 
double traj_steer_change_penalty_
 
double traj_short_length_penalty_
 
double traj_expected_shortest_length_
 
WarmStartConfig planner_warm_start_config_
 

详细描述

在文件 reeds_shepp_path.h 第 63 行定义.

构造及析构函数说明

◆ ReedShepp() [1/2]

apollo::planning::ReedShepp::ReedShepp ( const common::VehicleParam &  vehicle_param,
const PlannerOpenSpaceConfig &  open_space_conf 
)

在文件 reeds_shepp_path.cc 第 26 行定义.

26 :
27 vehicle_param_(vehicle_param), planner_open_space_config_(open_space_conf) {
28 max_kappa_ = std::tan(
29 vehicle_param_.max_steer_angle()
30 * planner_open_space_config_.warm_start_config().traj_kappa_contraint_ratio()
31 / vehicle_param_.steer_ratio())
32 / vehicle_param_.wheel_base();
33 traj_forward_penalty_ = planner_open_space_config_.warm_start_config().traj_forward_penalty();
34 traj_back_penalty_ = planner_open_space_config_.warm_start_config().traj_back_penalty();
35 traj_gear_switch_penalty_ = planner_open_space_config_.warm_start_config().traj_gear_switch_penalty();
36 traj_steer_penalty_ = planner_open_space_config_.warm_start_config().traj_steer_penalty();
37 traj_steer_change_penalty_ = planner_open_space_config_.warm_start_config().traj_steer_change_penalty();
38 traj_short_length_penalty_ = planner_open_space_config_.warm_start_config().traj_short_length_penalty();
39 traj_expected_shortest_length_ = planner_open_space_config_.warm_start_config().traj_expected_shortest_length();
40 AINFO << "max kappa: " << max_kappa_;
41 AINFO << "traj_short_length_penalty_: " << traj_short_length_penalty_;
42 AINFO_IF(FLAGS_enable_parallel_hybrid_a) << "parallel REEDShepp";
43}
apollo::planning::ReedShepp::planner_open_space_config_
PlannerOpenSpaceConfig planner_open_space_config_
Definition reeds_shepp_path.h:156
apollo::planning::ReedShepp::vehicle_param_
common::VehicleParam vehicle_param_
Definition reeds_shepp_path.h:155
AINFO_IF
#define AINFO_IF(cond)
Definition log.h:72
AINFO
#define AINFO
Definition log.h:42

◆ ~ReedShepp()

virtual apollo::planning::ReedShepp::~ReedShepp ( )
virtualdefault

◆ ReedShepp() [2/2]

apollo::planning::ReedShepp::ReedShepp ( const common::VehicleParam &  vehicle_param,
const WarmStartConfig &  warm_start_conf 
)

在文件 reeds_shepp_path.cc 第 45 行定义.

45 :
46 vehicle_param_(vehicle_param), planner_warm_start_config_(warm_start_conf) {
47 max_kappa_ = std::tan(
48 vehicle_param_.max_steer_angle() * planner_warm_start_config_.traj_kappa_contraint_ratio()
49 / vehicle_param_.steer_ratio())
50 / vehicle_param_.wheel_base();
51 traj_forward_penalty_ = planner_warm_start_config_.traj_forward_penalty();
52 traj_back_penalty_ = planner_warm_start_config_.traj_back_penalty();
53 traj_gear_switch_penalty_ = planner_warm_start_config_.traj_gear_switch_penalty();
54 traj_steer_penalty_ = planner_warm_start_config_.traj_steer_penalty();
55 traj_steer_change_penalty_ = planner_warm_start_config_.traj_steer_change_penalty();
56 traj_short_length_penalty_ = planner_warm_start_config_.traj_short_length_penalty();
57 traj_expected_shortest_length_ = planner_warm_start_config_.traj_expected_shortest_length();
58 AINFO << "max kappa: " << max_kappa_;
59 AINFO << "traj_short_length_penalty_: " << traj_short_length_penalty_;
60 AINFO_IF(FLAGS_enable_parallel_hybrid_a) << "parallel REEDShepp";
61}
apollo::planning::ReedShepp::planner_warm_start_config_
WarmStartConfig planner_warm_start_config_
Definition reeds_shepp_path.h:173

成员函数说明

◆ calc_tau_omega()

std::pair< double, double > apollo::planning::ReedShepp::calc_tau_omega ( const double  u,
const double  v,
const double  xi,
const double  eta,
const double  phi 
)
protected

在文件 reeds_shepp_path.cc 第 63 行定义.

68 {
69 double delta = common::math::NormalizeAngle(u - v);
70 double A = std::sin(u) - std::sin(delta);
71 double B = std::cos(u) - std::cos(delta) - 1.0;
72
73 double t1 = std::atan2(eta * A - xi * B, xi * A + eta * B);
74 double t2 = 2.0 * (std::cos(delta) - std::cos(v) - std::cos(u)) + 3.0;
75 double tau = 0.0;
76 if (t2 < 0) {
77 tau = common::math::NormalizeAngle(t1 + M_PI);
78 } else {
79 tau = common::math::NormalizeAngle(t1);
80 }
81 double omega = common::math::NormalizeAngle(tau - u + v - phi);
82 return std::make_pair(tau, omega);
83}
apollo::common::math::NormalizeAngle
double NormalizeAngle(const double angle)
Normalize angle to [-PI, PI).
Definition math_utils.cc:53
apollo::drivers::camera::A
const size_t A
Definition util.h:178

◆ CCC()

bool apollo::planning::ReedShepp::CCC ( const double  x,
const double  y,
const double  phi,
std::vector< ReedSheppPath > *  all_possible_paths 
)
protected

在文件 reeds_shepp_path.cc 第 381 行定义.

381 {
382 RSPParam LRL1_param;
383 LRL(x, y, phi, &LRL1_param);
384 double LRL1_lengths[3] = {LRL1_param.t, LRL1_param.u, LRL1_param.v};
385 char LRL1_types[] = "LRL";
386 if (LRL1_param.flag && !SetRSP(3, LRL1_lengths, LRL1_types, all_possible_paths)) {
387 ADEBUG << "Fail at SetRSP with LRL_param";
388 return false;
389 }
390
391 RSPParam LRL2_param;
392 LRL(-x, y, -phi, &LRL2_param);
393 double LRL2_lengths[3] = {-LRL2_param.t, -LRL2_param.u, -LRL2_param.v};
394 char LRL2_types[] = "LRL";
395 if (LRL2_param.flag && !SetRSP(3, LRL2_lengths, LRL2_types, all_possible_paths)) {
396 ADEBUG << "Fail at SetRSP with LRL2_param";
397 return false;
398 }
399
400 RSPParam LRL3_param;
401 LRL(x, -y, -phi, &LRL3_param);
402 double LRL3_lengths[3] = {LRL3_param.t, LRL3_param.u, LRL3_param.v};
403 char LRL3_types[] = "RLR";
404 if (LRL3_param.flag && !SetRSP(3, LRL3_lengths, LRL3_types, all_possible_paths)) {
405 ADEBUG << "Fail at SetRSP with LRL3_param";
406 return false;
407 }
408
409 RSPParam LRL4_param;
410 LRL(-x, -y, phi, &LRL4_param);
411 double LRL4_lengths[3] = {-LRL4_param.t, -LRL4_param.u, -LRL4_param.v};
412 char LRL4_types[] = "RLR";
413 if (LRL4_param.flag && !SetRSP(3, LRL4_lengths, LRL4_types, all_possible_paths)) {
414 ADEBUG << "Fail at SetRSP with LRL4_param";
415 return false;
416 }
417
418 // backward
419 double xb = x * std::cos(phi) + y * std::sin(phi);
420 double yb = x * std::sin(phi) - y * std::cos(phi);
421
422 RSPParam LRL5_param;
423 LRL(xb, yb, phi, &LRL5_param);
424 double LRL5_lengths[3] = {LRL5_param.v, LRL5_param.u, LRL5_param.t};
425 char LRL5_types[] = "LRL";
426 if (LRL5_param.flag && !SetRSP(3, LRL5_lengths, LRL5_types, all_possible_paths)) {
427 ADEBUG << "Fail at SetRSP with LRL5_param";
428 return false;
429 }
430
431 RSPParam LRL6_param;
432 LRL(-xb, yb, -phi, &LRL6_param);
433 double LRL6_lengths[3] = {-LRL6_param.v, -LRL6_param.u, -LRL6_param.t};
434 char LRL6_types[] = "LRL";
435 if (LRL6_param.flag && !SetRSP(3, LRL6_lengths, LRL6_types, all_possible_paths)) {
436 ADEBUG << "Fail at SetRSP with LRL6_param";
437 return false;
438 }
439
440 RSPParam LRL7_param;
441 LRL(xb, -yb, -phi, &LRL7_param);
442 double LRL7_lengths[3] = {LRL7_param.v, LRL7_param.u, LRL7_param.t};
443 char LRL7_types[] = "RLR";
444 if (LRL7_param.flag && !SetRSP(3, LRL7_lengths, LRL7_types, all_possible_paths)) {
445 ADEBUG << "Fail at SetRSP with LRL7_param";
446 return false;
447 }
448
449 RSPParam LRL8_param;
450 LRL(-xb, -yb, phi, &LRL8_param);
451 double LRL8_lengths[3] = {-LRL8_param.v, -LRL8_param.u, -LRL8_param.t};
452 char LRL8_types[] = "RLR";
453 if (LRL8_param.flag && !SetRSP(3, LRL8_lengths, LRL8_types, all_possible_paths)) {
454 ADEBUG << "Fail at SetRSP with LRL8_param";
455 return false;
456 }
457 return true;
458}
apollo::planning::ReedShepp::LRL
void LRL(const double x, const double y, const double phi, RSPParam *param)
Definition reeds_shepp_path.cc:767
apollo::planning::ReedShepp::SetRSP
bool SetRSP(const int size, const double *lengths, const char *types, std::vector< ReedSheppPath > *all_possible_paths, double radius=1.0)
Definition reeds_shepp_path.cc:947
ADEBUG
#define ADEBUG
Definition log.h:41

◆ CCCC()

bool apollo::planning::ReedShepp::CCCC ( const double  x,
const double  y,
const double  phi,
std::vector< ReedSheppPath > *  all_possible_paths 
)
protected

在文件 reeds_shepp_path.cc 第 460 行定义.

460 {
461 RSPParam LRLRn1_param;
462 LRLRn(x, y, phi, &LRLRn1_param);
463 double LRLRn1_lengths[4] = {LRLRn1_param.t, LRLRn1_param.u, -LRLRn1_param.u, LRLRn1_param.v};
464 char LRLRn1_types[] = "LRLR";
465 if (LRLRn1_param.flag && !SetRSP(4, LRLRn1_lengths, LRLRn1_types, all_possible_paths)) {
466 ADEBUG << "Fail at SetRSP with LRLRn_param";
467 return false;
468 }
469
470 RSPParam LRLRn2_param;
471 LRLRn(-x, y, -phi, &LRLRn2_param);
472 double LRLRn2_lengths[4] = {-LRLRn2_param.t, -LRLRn2_param.u, LRLRn2_param.u, -LRLRn2_param.v};
473 char LRLRn2_types[] = "LRLR";
474 if (LRLRn2_param.flag && !SetRSP(4, LRLRn2_lengths, LRLRn2_types, all_possible_paths)) {
475 ADEBUG << "Fail at SetRSP with LRLRn2_param";
476 return false;
477 }
478
479 RSPParam LRLRn3_param;
480 LRLRn(x, -y, -phi, &LRLRn3_param);
481 double LRLRn3_lengths[4] = {LRLRn3_param.t, LRLRn3_param.u, -LRLRn3_param.u, LRLRn3_param.v};
482 char LRLRn3_types[] = "RLRL";
483 if (LRLRn3_param.flag && !SetRSP(4, LRLRn3_lengths, LRLRn3_types, all_possible_paths)) {
484 ADEBUG << "Fail at SetRSP with LRLRn3_param";
485 return false;
486 }
487
488 RSPParam LRLRn4_param;
489 LRLRn(-x, -y, phi, &LRLRn4_param);
490 double LRLRn4_lengths[4] = {-LRLRn4_param.t, -LRLRn4_param.u, LRLRn4_param.u, -LRLRn4_param.v};
491 char LRLRn4_types[] = "RLRL";
492 if (LRLRn4_param.flag && !SetRSP(4, LRLRn4_lengths, LRLRn4_types, all_possible_paths)) {
493 ADEBUG << "Fail at SetRSP with LRLRn4_param";
494 return false;
495 }
496
497 RSPParam LRLRp1_param;
498 LRLRp(x, y, phi, &LRLRp1_param);
499 double LRLRp1_lengths[4] = {LRLRp1_param.t, LRLRp1_param.u, LRLRp1_param.u, LRLRp1_param.v};
500 char LRLRp1_types[] = "LRLR";
501 if (LRLRp1_param.flag && !SetRSP(4, LRLRp1_lengths, LRLRp1_types, all_possible_paths)) {
502 ADEBUG << "Fail at SetRSP with LRLRp1_param";
503 return false;
504 }
505
506 RSPParam LRLRp2_param;
507 LRLRp(-x, y, -phi, &LRLRp2_param);
508 double LRLRp2_lengths[4] = {-LRLRp2_param.t, -LRLRp2_param.u, -LRLRp2_param.u, -LRLRp2_param.v};
509 char LRLRp2_types[] = "LRLR";
510 if (LRLRp2_param.flag && !SetRSP(4, LRLRp2_lengths, LRLRp2_types, all_possible_paths)) {
511 ADEBUG << "Fail at SetRSP with LRLRp2_param";
512 return false;
513 }
514
515 RSPParam LRLRp3_param;
516 LRLRp(x, -y, -phi, &LRLRp3_param);
517 double LRLRp3_lengths[4] = {LRLRp3_param.t, LRLRp3_param.u, LRLRp3_param.u, LRLRp3_param.v};
518 char LRLRp3_types[] = "RLRL";
519 if (LRLRp3_param.flag && !SetRSP(4, LRLRp3_lengths, LRLRp3_types, all_possible_paths)) {
520 ADEBUG << "Fail at SetRSP with LRLRp3_param";
521 return false;
522 }
523
524 RSPParam LRLRp4_param;
525 LRLRp(-x, -y, phi, &LRLRp4_param);
526 double LRLRp4_lengths[4] = {-LRLRp4_param.t, -LRLRp4_param.u, -LRLRp4_param.u, -LRLRp4_param.v};
527 char LRLRp4_types[] = "RLRL";
528 if (LRLRp4_param.flag && !SetRSP(4, LRLRp4_lengths, LRLRp4_types, all_possible_paths)) {
529 ADEBUG << "Fail at SetRSP with LRLRp4_param";
530 return false;
531 }
532 return true;
533}
apollo::planning::ReedShepp::LRLRn
void LRLRn(const double x, const double y, const double phi, RSPParam *param)
Definition reeds_shepp_path.cc:838
apollo::planning::ReedShepp::LRLRp
void LRLRp(const double x, const double y, const double phi, RSPParam *param)
Definition reeds_shepp_path.cc:857

◆ CCSC()

bool apollo::planning::ReedShepp::CCSC ( const double  x,
const double  y,
const double  phi,
std::vector< ReedSheppPath > *  all_possible_paths 
)
protected

在文件 reeds_shepp_path.cc 第 535 行定义.

535 {
536 RSPParam LRSL1_param;
537 LRSL(x, y, phi, &LRSL1_param);
538 double LRSL1_lengths[4] = {LRSL1_param.t, -0.5 * M_PI, LRSL1_param.u, LRSL1_param.v};
539 char LRSL1_types[] = "LRSL";
540 if (LRSL1_param.flag && !SetRSP(4, LRSL1_lengths, LRSL1_types, all_possible_paths)) {
541 ADEBUG << "Fail at SetRSP with LRSL1_param";
542 return false;
543 }
544
545 RSPParam LRSL2_param;
546 LRSL(-x, y, -phi, &LRSL2_param);
547 double LRSL2_lengths[4] = {-LRSL2_param.t, 0.5 * M_PI, -LRSL2_param.u, -LRSL2_param.v};
548 char LRSL2_types[] = "LRSL";
549 if (LRSL2_param.flag && !SetRSP(4, LRSL2_lengths, LRSL2_types, all_possible_paths)) {
550 ADEBUG << "Fail at SetRSP with LRSL2_param";
551 return false;
552 }
553
554 RSPParam LRSL3_param;
555 LRSL(x, -y, -phi, &LRSL3_param);
556 double LRSL3_lengths[4] = {LRSL3_param.t, -0.5 * M_PI, LRSL3_param.u, LRSL3_param.v};
557 char LRSL3_types[] = "RLSR";
558 if (LRSL3_param.flag && !SetRSP(4, LRSL3_lengths, LRSL3_types, all_possible_paths)) {
559 ADEBUG << "Fail at SetRSP with LRSL3_param";
560 return false;
561 }
562
563 RSPParam LRSL4_param;
564 LRSL(-x, -y, phi, &LRSL4_param);
565 double LRSL4_lengths[4] = {-LRSL4_param.t, 0.5 * M_PI, -LRSL4_param.u, -LRSL4_param.v};
566 char LRSL4_types[] = "RLSR";
567 if (LRSL4_param.flag && !SetRSP(4, LRSL4_lengths, LRSL4_types, all_possible_paths)) {
568 ADEBUG << "Fail at SetRSP with LRSL4_param";
569 return false;
570 }
571
572 RSPParam LRSR1_param;
573 LRSR(x, y, phi, &LRSR1_param);
574 double LRSR1_lengths[4] = {LRSR1_param.t, -0.5 * M_PI, LRSR1_param.u, LRSR1_param.v};
575 char LRSR1_types[] = "LRSR";
576 if (LRSR1_param.flag && !SetRSP(4, LRSR1_lengths, LRSR1_types, all_possible_paths)) {
577 ADEBUG << "Fail at SetRSP with LRSR1_param";
578 return false;
579 }
580
581 RSPParam LRSR2_param;
582 LRSR(-x, y, -phi, &LRSR2_param);
583 double LRSR2_lengths[4] = {-LRSR2_param.t, 0.5 * M_PI, -LRSR2_param.u, -LRSR2_param.v};
584 char LRSR2_types[] = "LRSR";
585 if (LRSR2_param.flag && !SetRSP(4, LRSR2_lengths, LRSR2_types, all_possible_paths)) {
586 ADEBUG << "Fail at SetRSP with LRSR2_param";
587 return false;
588 }
589
590 RSPParam LRSR3_param;
591 LRSR(x, -y, -phi, &LRSR3_param);
592 double LRSR3_lengths[4] = {LRSR3_param.t, -0.5 * M_PI, LRSR3_param.u, LRSR3_param.v};
593 char LRSR3_types[] = "RLSL";
594 if (LRSR3_param.flag && !SetRSP(4, LRSR3_lengths, LRSR3_types, all_possible_paths)) {
595 ADEBUG << "Fail at SetRSP with LRSR3_param";
596 return false;
597 }
598
599 RSPParam LRSR4_param;
600 LRSR(-x, -y, phi, &LRSR4_param);
601 double LRSR4_lengths[4] = {-LRSR4_param.t, 0.5 * M_PI, -LRSR4_param.u, -LRSR4_param.v};
602 char LRSR4_types[] = "RLSL";
603 if (LRSR4_param.flag && !SetRSP(4, LRSR4_lengths, LRSR4_types, all_possible_paths)) {
604 ADEBUG << "Fail at SetRSP with LRSR4_param";
605 return false;
606 }
607
608 // backward
609 double xb = x * std::cos(phi) + y * std::sin(phi);
610 double yb = x * std::sin(phi) - y * std::cos(phi);
611
612 RSPParam LRSL5_param;
613 LRSL(xb, yb, phi, &LRSL5_param);
614 double LRSL5_lengths[4] = {LRSL5_param.v, LRSL5_param.u, -0.5 * M_PI, LRSL5_param.t};
615 char LRSL5_types[] = "LSRL";
616 if (LRSL5_param.flag && !SetRSP(4, LRSL5_lengths, LRSL5_types, all_possible_paths)) {
617 ADEBUG << "Fail at SetRSP with LRLRn_param";
618 return false;
619 }
620
621 RSPParam LRSL6_param;
622 LRSL(-xb, yb, -phi, &LRSL6_param);
623 double LRSL6_lengths[4] = {-LRSL6_param.v, -LRSL6_param.u, 0.5 * M_PI, -LRSL6_param.t};
624 char LRSL6_types[] = "LSRL";
625 if (LRSL6_param.flag && !SetRSP(4, LRSL6_lengths, LRSL6_types, all_possible_paths)) {
626 ADEBUG << "Fail at SetRSP with LRSL6_param";
627 return false;
628 }
629
630 RSPParam LRSL7_param;
631 LRSL(xb, -yb, -phi, &LRSL7_param);
632 double LRSL7_lengths[4] = {LRSL7_param.v, LRSL7_param.u, -0.5 * M_PI, LRSL7_param.t};
633 char LRSL7_types[] = "RSLR";
634 if (LRSL7_param.flag && !SetRSP(4, LRSL7_lengths, LRSL7_types, all_possible_paths)) {
635 ADEBUG << "Fail at SetRSP with LRSL7_param";
636 return false;
637 }
638
639 RSPParam LRSL8_param;
640 LRSL(-xb, -yb, phi, &LRSL8_param);
641 double LRSL8_lengths[4] = {-LRSL8_param.v, -LRSL8_param.u, 0.5 * M_PI, -LRSL8_param.t};
642 char LRSL8_types[] = "RSLR";
643 if (LRSL8_param.flag && !SetRSP(4, LRSL8_lengths, LRSL8_types, all_possible_paths)) {
644 ADEBUG << "Fail at SetRSP with LRSL8_param";
645 return false;
646 }
647
648 RSPParam LRSR5_param;
649 LRSR(xb, yb, phi, &LRSR5_param);
650 double LRSR5_lengths[4] = {LRSR5_param.v, LRSR5_param.u, -0.5 * M_PI, LRSR5_param.t};
651 char LRSR5_types[] = "RSRL";
652 if (LRSR5_param.flag && !SetRSP(4, LRSR5_lengths, LRSR5_types, all_possible_paths)) {
653 ADEBUG << "Fail at SetRSP with LRSR5_param";
654 return false;
655 }
656
657 RSPParam LRSR6_param;
658 LRSR(-xb, yb, -phi, &LRSR6_param);
659 double LRSR6_lengths[4] = {-LRSR6_param.v, -LRSR6_param.u, 0.5 * M_PI, -LRSR6_param.t};
660 char LRSR6_types[] = "RSRL";
661 if (LRSR6_param.flag && !SetRSP(4, LRSR6_lengths, LRSR6_types, all_possible_paths)) {
662 ADEBUG << "Fail at SetRSP with LRSR6_param";
663 return false;
664 }
665
666 RSPParam LRSR7_param;
667 LRSR(xb, -yb, -phi, &LRSR7_param);
668 double LRSR7_lengths[4] = {LRSR7_param.v, LRSR7_param.u, -0.5 * M_PI, LRSR7_param.t};
669 char LRSR7_types[] = "LSLR";
670 if (LRSR7_param.flag && !SetRSP(4, LRSR7_lengths, LRSR7_types, all_possible_paths)) {
671 ADEBUG << "Fail at SetRSP with LRSR7_param";
672 return false;
673 }
674
675 RSPParam LRSR8_param;
676 LRSR(-xb, -yb, phi, &LRSR8_param);
677 double LRSR8_lengths[4] = {-LRSR8_param.v, -LRSR8_param.u, 0.5 * M_PI, -LRSR8_param.t};
678 char LRSR8_types[] = "LSLR";
679 if (LRSR8_param.flag && !SetRSP(4, LRSR8_lengths, LRSR8_types, all_possible_paths)) {
680 ADEBUG << "Fail at SetRSP with LRSR8_param";
681 return false;
682 }
683 return true;
684}
apollo::planning::ReedShepp::LRSL
void LRSL(const double x, const double y, const double phi, RSPParam *param)
Definition reeds_shepp_path.cc:898
apollo::planning::ReedShepp::LRSR
void LRSR(const double x, const double y, const double phi, RSPParam *param)
Definition reeds_shepp_path.cc:876

◆ CCSCC()

bool apollo::planning::ReedShepp::CCSCC ( const double  x,
const double  y,
const double  phi,
std::vector< ReedSheppPath > *  all_possible_paths 
)
protected

在文件 reeds_shepp_path.cc 第 686 行定义.

690 {
691 RSPParam LRSLR1_param;
692 LRSLR(x, y, phi, &LRSLR1_param);
693 double LRSLR1_lengths[5] = {LRSLR1_param.t, -0.5 * M_PI, LRSLR1_param.u, -0.5 * M_PI, LRSLR1_param.v};
694 char LRSLR1_types[] = "LRSLR";
695 if (LRSLR1_param.flag && !SetRSP(5, LRSLR1_lengths, LRSLR1_types, all_possible_paths)) {
696 ADEBUG << "Fail at SetRSP with LRSLR1_param";
697 return false;
698 }
699
700 RSPParam LRSLR2_param;
701 LRSLR(-x, y, -phi, &LRSLR2_param);
702 double LRSLR2_lengths[5] = {-LRSLR2_param.t, 0.5 * M_PI, -LRSLR2_param.u, 0.5 * M_PI, -LRSLR2_param.v};
703 char LRSLR2_types[] = "LRSLR";
704 if (LRSLR2_param.flag && !SetRSP(5, LRSLR2_lengths, LRSLR2_types, all_possible_paths)) {
705 ADEBUG << "Fail at SetRSP with LRSLR2_param";
706 return false;
707 }
708
709 RSPParam LRSLR3_param;
710 LRSLR(x, -y, -phi, &LRSLR3_param);
711 double LRSLR3_lengths[5] = {LRSLR3_param.t, -0.5 * M_PI, LRSLR3_param.u, -0.5 * M_PI, LRSLR3_param.v};
712 char LRSLR3_types[] = "RLSRL";
713 if (LRSLR3_param.flag && !SetRSP(5, LRSLR3_lengths, LRSLR3_types, all_possible_paths)) {
714 ADEBUG << "Fail at SetRSP with LRSLR3_param";
715 return false;
716 }
717
718 RSPParam LRSLR4_param;
719 LRSLR(-x, -y, phi, &LRSLR4_param);
720 double LRSLR4_lengths[5] = {-LRSLR4_param.t, 0.5 * M_PI, -LRSLR4_param.u, 0.5 * M_PI, -LRSLR4_param.v};
721 char LRSLR4_types[] = "RLSRL";
722 if (LRSLR4_param.flag && !SetRSP(5, LRSLR4_lengths, LRSLR4_types, all_possible_paths)) {
723 ADEBUG << "Fail at SetRSP with LRSLR4_param";
724 return false;
725 }
726 return true;
727}
apollo::planning::ReedShepp::LRSLR
void LRSLR(const double x, const double y, const double phi, RSPParam *param)
Definition reeds_shepp_path.cc:923

◆ CS()

bool apollo::planning::ReedShepp::CS ( const double  x,
const double  y,
const double  phi,
std::vector< ReedSheppPath > *  all_possible_paths 
)
protected

在文件 reeds_shepp_path.cc 第 269 行定义.

269 {
270 RSPParam LS1_param;
271 LS(x, y, phi, &LS1_param); // L+S+ L+S-
272 double LS1_lengths[2] = {LS1_param.t, LS1_param.u};
273 char LS1_types[] = "LS";
274 if (LS1_param.flag && !SetRSP(2, LS1_lengths, LS1_types, all_possible_paths, LS1_param.v)) {
275 ADEBUG << "Fail at SetRSP with LS_param";
276 return false;
277 }
278 RSPParam LS2_param;
279 LS(-x, y, -phi, &LS2_param); // L-S+ L-S-
280 double LS2_lengths[2] = {-LS2_param.t, -LS2_param.u};
281 char LS2_types[] = "LS";
282 if (LS2_param.flag && !SetRSP(2, LS2_lengths, LS2_types, all_possible_paths, LS2_param.v)) {
283 ADEBUG << "Fail at SetRSP with LS_param";
284 return false;
285 }
286 RSPParam RS1_param;
287 LS(x, -y, -phi, &RS1_param); // R+S+ R+S-
288 double RS1_lengths[2] = {RS1_param.t, RS1_param.u};
289 char RS1_types[] = "RS";
290 if (RS1_param.flag && !SetRSP(2, RS1_lengths, RS1_types, all_possible_paths, RS1_param.v)) {
291 ADEBUG << "Fail at SetRSP with SLS_param";
292 return false;
293 }
294
295 RSPParam RS2_param;
296 LS(-x, -y, phi, &RS2_param); // R-S+ R-S-
297 double RS2_lengths[2] = {-RS2_param.t, -RS2_param.u};
298 char RS2_types[] = "RS";
299 if (RS2_param.flag && !SetRSP(2, RS2_lengths, RS2_types, all_possible_paths, RS2_param.v)) {
300 ADEBUG << "Fail at SetRSP with SLS_param";
301 return false;
302 }
303 return true;
304}
apollo::planning::ReedShepp::LS
void LS(const double x, const double y, const double phi, RSPParam *param)
Definition reeds_shepp_path.cc:787

◆ CSC()

bool apollo::planning::ReedShepp::CSC ( const double  x,
const double  y,
const double  phi,
std::vector< ReedSheppPath > *  all_possible_paths 
)
protected

在文件 reeds_shepp_path.cc 第 306 行定义.

306 {
307 RSPParam LSL1_param;
308 LSL(x, y, phi, &LSL1_param);
309 double LSL1_lengths[3] = {LSL1_param.t, LSL1_param.u, LSL1_param.v};
310 char LSL1_types[] = "LSL";
311 if (LSL1_param.flag && !SetRSP(3, LSL1_lengths, LSL1_types, all_possible_paths)) {
312 ADEBUG << "Fail at SetRSP with LSL_param";
313 return false;
314 }
315
316 RSPParam LSL2_param;
317 LSL(-x, y, -phi, &LSL2_param);
318 double LSL2_lengths[3] = {-LSL2_param.t, -LSL2_param.u, -LSL2_param.v};
319 char LSL2_types[] = "LSL";
320 if (LSL2_param.flag && !SetRSP(3, LSL2_lengths, LSL2_types, all_possible_paths)) {
321 ADEBUG << "Fail at SetRSP with LSL2_param";
322 return false;
323 }
324
325 RSPParam LSL3_param;
326 LSL(x, -y, -phi, &LSL3_param);
327 double LSL3_lengths[3] = {LSL3_param.t, LSL3_param.u, LSL3_param.v};
328 char LSL3_types[] = "RSR";
329 if (LSL3_param.flag && !SetRSP(3, LSL3_lengths, LSL3_types, all_possible_paths)) {
330 ADEBUG << "Fail at SetRSP with LSL3_param";
331 return false;
332 }
333
334 RSPParam LSL4_param;
335 LSL(-x, -y, phi, &LSL4_param);
336 double LSL4_lengths[3] = {-LSL4_param.t, -LSL4_param.u, -LSL4_param.v};
337 char LSL4_types[] = "RSR";
338 if (LSL4_param.flag && !SetRSP(3, LSL4_lengths, LSL4_types, all_possible_paths)) {
339 ADEBUG << "Fail at SetRSP with LSL4_param";
340 return false;
341 }
342
343 RSPParam LSR1_param;
344 LSR(x, y, phi, &LSR1_param);
345 double LSR1_lengths[3] = {LSR1_param.t, LSR1_param.u, LSR1_param.v};
346 char LSR1_types[] = "LSR";
347 if (LSR1_param.flag && !SetRSP(3, LSR1_lengths, LSR1_types, all_possible_paths)) {
348 ADEBUG << "Fail at SetRSP with LSR1_param";
349 return false;
350 }
351
352 RSPParam LSR2_param;
353 LSR(-x, y, -phi, &LSR2_param);
354 double LSR2_lengths[3] = {-LSR2_param.t, -LSR2_param.u, -LSR2_param.v};
355 char LSR2_types[] = "LSR";
356 if (LSR2_param.flag && !SetRSP(3, LSR2_lengths, LSR2_types, all_possible_paths)) {
357 ADEBUG << "Fail at SetRSP with LSR2_param";
358 return false;
359 }
360
361 RSPParam LSR3_param;
362 LSR(x, -y, -phi, &LSR3_param);
363 double LSR3_lengths[3] = {LSR3_param.t, LSR3_param.u, LSR3_param.v};
364 char LSR3_types[] = "RSL";
365 if (LSR3_param.flag && !SetRSP(3, LSR3_lengths, LSR3_types, all_possible_paths)) {
366 ADEBUG << "Fail at SetRSP with LSR3_param";
367 return false;
368 }
369
370 RSPParam LSR4_param;
371 LSR(-x, -y, phi, &LSR4_param);
372 double LSR4_lengths[3] = {-LSR4_param.t, -LSR4_param.u, -LSR4_param.v};
373 char LSR4_types[] = "RSL";
374 if (LSR4_param.flag && !SetRSP(3, LSR4_lengths, LSR4_types, all_possible_paths)) {
375 ADEBUG << "Fail at SetRSP with LSR4_param";
376 return false;
377 }
378 return true;
379}
apollo::planning::ReedShepp::LSL
void LSL(const double x, const double y, const double phi, RSPParam *param)
Definition reeds_shepp_path.cc:729
apollo::planning::ReedShepp::LSR
void LSR(const double x, const double y, const double phi, RSPParam *param)
Definition reeds_shepp_path.cc:745

◆ GenerateLocalConfigurations()

bool apollo::planning::ReedShepp::GenerateLocalConfigurations ( const std::shared_ptr< Node3d >  start_node,
const std::shared_ptr< Node3d >  end_node,
ReedSheppPath *  shortest_path 
)
protected

在文件 reeds_shepp_path.cc 第 973 行定义.

976 {
977 double radius = shortest_path->radius;
978 double step_scaled = planner_warm_start_config_.step_size() * max_kappa_;
979 ADEBUG << "shortest_path->total_length" << shortest_path->total_length;
980 size_t point_num = static_cast<size_t>(std::floor(
981 shortest_path->total_length / step_scaled * radius + static_cast<double>(shortest_path->segs_lengths.size())
982 + 4));
983 ADEBUG << "point_num" << point_num;
984 std::vector<double> px(point_num, 0.0);
985 std::vector<double> py(point_num, 0.0);
986 std::vector<double> pphi(point_num, 0.0);
987 std::vector<bool> pgear(point_num, true);
988 int index = 1;
989 double d = 0.0;
990 double pd = 0.0;
991 double ll = 0.0;
992
993 if (shortest_path->segs_lengths.at(0) > 0.0) {
994 pgear.at(0) = true;
995 d = step_scaled;
996 } else {
997 pgear.at(0) = false;
998 d = -step_scaled;
999 }
1000 pd = d;
1001 for (size_t i = 0; i < shortest_path->segs_types.size(); ++i) {
1002 char m = shortest_path->segs_types.at(i);
1003 double l = shortest_path->segs_lengths.at(i);
1004 d = l > 0.0 ? step_scaled : -step_scaled;
1005 d = m == 'S' ? d : d / radius;
1006 double ox = px.at(index);
1007 double oy = py.at(index);
1008 double ophi = pphi.at(index);
1009 index--;
1010 if (i >= 1 && shortest_path->segs_lengths.at(i - 1) * shortest_path->segs_lengths.at(i) > 0) {
1011 pd = -d - ll;
1012 } else {
1013 pd = d - ll;
1014 }
1015 while (std::abs(pd) <= std::abs(l)) {
1016 index++;
1017 Interpolation(index, pd, m, ox, oy, ophi, radius, &px, &py, &pphi, &pgear);
1018 pd += d;
1019 }
1020 ll = l - pd - d;
1021 index++;
1022 Interpolation(index, l, m, ox, oy, ophi, radius, &px, &py, &pphi, &pgear);
1023 }
1024 double epsilon = 1e-15;
1025 while (std::fabs(px.back()) < epsilon && std::fabs(py.back()) < epsilon && std::fabs(pphi.back()) < epsilon
1026 && pgear.back()) {
1027 px.pop_back();
1028 py.pop_back();
1029 pphi.pop_back();
1030 pgear.pop_back();
1031 }
1032 for (size_t i = 0; i < px.size(); ++i) {
1033 shortest_path->x.push_back(
1034 std::cos(-start_node->GetPhi()) * px.at(i) + std::sin(-start_node->GetPhi()) * py.at(i)
1035 + start_node->GetX());
1036 shortest_path->y.push_back(
1037 -std::sin(-start_node->GetPhi()) * px.at(i) + std::cos(-start_node->GetPhi()) * py.at(i)
1038 + start_node->GetY());
1039 shortest_path->phi.push_back(common::math::NormalizeAngle(pphi.at(i) + start_node->GetPhi()));
1040 }
1041 shortest_path->gear = pgear;
1042 for (size_t i = 0; i < shortest_path->segs_lengths.size(); ++i) {
1043 shortest_path->segs_lengths.at(i) = shortest_path->segs_lengths.at(i) / max_kappa_;
1044 }
1045 shortest_path->total_length = shortest_path->total_length / max_kappa_;
1046 return true;
1047}
apollo::planning::ReedShepp::Interpolation
void Interpolation(const int index, const double pd, const char m, const double ox, const double oy, const double ophi, const double radius, std::vector< double > *px, std::vector< double > *py, std::vector< double > *pphi, std::vector< bool > *pgear)
Definition reeds_shepp_path.cc:1049

◆ GenerateRSP()

bool apollo::planning::ReedShepp::GenerateRSP ( const std::shared_ptr< Node3d >  start_node,
const std::shared_ptr< Node3d >  end_node,
std::vector< ReedSheppPath > *  all_possible_paths 
)
protected

在文件 reeds_shepp_path.cc 第 198 行定义.

201 {
202 double dx = end_node->GetX() - start_node->GetX();
203 double dy = end_node->GetY() - start_node->GetY();
204 double dphi = end_node->GetPhi() - start_node->GetPhi();
205 double c = std::cos(start_node->GetPhi());
206 double s = std::sin(start_node->GetPhi());
207 // normalize the initial point to (0,0,0)
208 double x = (c * dx + s * dy) * max_kappa_;
209 double y = (-s * dx + c * dy) * max_kappa_;
210 // if (!CS(x, y, dphi, all_possible_paths)) {
211 // ADEBUG << "Fail at CS";
212 // }
213 if (!SCS(x, y, dphi, all_possible_paths)) {
214 ADEBUG << "Fail at SCS";
215 }
216
217 if (reeds_sheep_last_straight_) {
218 if (all_possible_paths->empty()) {
219 ADEBUG << "No path generated by certain two configurations";
220 return false;
221 } else {
222 return true;
223 }
224 }
225
226 if (!CSC(x, y, dphi, all_possible_paths)) {
227 ADEBUG << "Fail at CSC";
228 }
229 if (!CCC(x, y, dphi, all_possible_paths)) {
230 ADEBUG << "Fail at CCC";
231 }
232 if (!CCCC(x, y, dphi, all_possible_paths)) {
233 ADEBUG << "Fail at CCCC";
234 }
235 if (!CCSC(x, y, dphi, all_possible_paths)) {
236 ADEBUG << "Fail at CCSC";
237 }
238 if (!CCSCC(x, y, dphi, all_possible_paths)) {
239 ADEBUG << "Fail at CCSCC";
240 }
241 if (all_possible_paths->empty()) {
242 ADEBUG << "No path generated by certain two configurations";
243 return false;
244 }
245 return true;
246}
apollo::planning::ReedShepp::CCCC
bool CCCC(const double x, const double y, const double phi, std::vector< ReedSheppPath > *all_possible_paths)
Definition reeds_shepp_path.cc:460
apollo::planning::ReedShepp::CCC
bool CCC(const double x, const double y, const double phi, std::vector< ReedSheppPath > *all_possible_paths)
Definition reeds_shepp_path.cc:381
apollo::planning::ReedShepp::CCSCC
bool CCSCC(const double x, const double y, const double phi, std::vector< ReedSheppPath > *all_possible_paths)
Definition reeds_shepp_path.cc:686
apollo::planning::ReedShepp::CCSC
bool CCSC(const double x, const double y, const double phi, std::vector< ReedSheppPath > *all_possible_paths)
Definition reeds_shepp_path.cc:535
apollo::planning::ReedShepp::CSC
bool CSC(const double x, const double y, const double phi, std::vector< ReedSheppPath > *all_possible_paths)
Definition reeds_shepp_path.cc:306
apollo::planning::ReedShepp::SCS
bool SCS(const double x, const double y, const double phi, std::vector< ReedSheppPath > *all_possible_paths)
Definition reeds_shepp_path.cc:248

◆ GenerateRSPPar()

bool apollo::planning::ReedShepp::GenerateRSPPar ( const std::shared_ptr< Node3d >  start_node,
const std::shared_ptr< Node3d >  end_node,
std::vector< ReedSheppPath > *  all_possible_paths 
)
protected

在文件 reeds_shepp_path.cc 第 1120 行定义.

1123 {
1124 double dx = end_node->GetX() - start_node->GetX();
1125 double dy = end_node->GetY() - start_node->GetY();
1126 double dphi = end_node->GetPhi() - start_node->GetPhi();
1127 double c = std::cos(start_node->GetPhi());
1128 double s = std::sin(start_node->GetPhi());
1129 // normalize the initial point to (0,0,0)
1130 double x = (c * dx + s * dy) * this->max_kappa_;
1131 double y = (-s * dx + c * dy) * this->max_kappa_;
1132 // backward
1133 double xb = x * std::cos(dphi) + y * std::sin(dphi);
1134 double yb = x * std::sin(dphi) - y * std::cos(dphi);
1135
1136 int RSP_nums = 46;
1137 all_possible_paths->resize(RSP_nums);
1138 bool succ = true;
1139#pragma omp parallel for schedule(dynamic, 2) num_threads(8)
1140 for (int i = 0; i < RSP_nums; ++i) {
1141 RSPParam RSP_param;
1142 int tmp_length = 0;
1143 double RSP_lengths[5] = {0.0, 0.0, 0.0, 0.0, 0.0};
1144 double x_param = 1.0;
1145 double y_param = 1.0;
1146 std::string rd_type;
1147
1148 if (i > 2 && i % 2) {
1149 x_param = -1.0;
1150 }
1151 if (i > 2 && i % 4 < 2) {
1152 y_param = -1.0;
1153 }
1154
1155 if (i < 2) { // SCS case
1156 if (i == 1) {
1157 y_param = -1.0;
1158 rd_type = "SRS";
1159 } else {
1160 rd_type = "SLS";
1161 }
1162 SLS(x, y_param * y, y_param * dphi, &RSP_param);
1163 tmp_length = 3;
1164 RSP_lengths[0] = RSP_param.t;
1165 RSP_lengths[1] = RSP_param.u;
1166 RSP_lengths[2] = RSP_param.v;
1167 } else if (i < 6) { // CSC, LSL case
1168 LSL(x_param * x, y_param * y, x_param * y_param * dphi, &RSP_param);
1169 if (y_param > 0) {
1170 rd_type = "LSL";
1171 } else {
1172 rd_type = "RSR";
1173 }
1174 tmp_length = 3;
1175 RSP_lengths[0] = x_param * RSP_param.t;
1176 RSP_lengths[1] = x_param * RSP_param.u;
1177 RSP_lengths[2] = x_param * RSP_param.v;
1178 } else if (i < 10) { // CSC, LSR case
1179 LSR(x_param * x, y_param * y, x_param * y_param * dphi, &RSP_param);
1180 if (y_param > 0) {
1181 rd_type = "LSR";
1182 } else {
1183 rd_type = "RSL";
1184 }
1185 tmp_length = 3;
1186 RSP_lengths[0] = x_param * RSP_param.t;
1187 RSP_lengths[1] = x_param * RSP_param.u;
1188 RSP_lengths[2] = x_param * RSP_param.v;
1189 } else if (i < 14) { // CCC, LRL case
1190 LRL(x_param * x, y_param * y, x_param * y_param * dphi, &RSP_param);
1191 if (y_param > 0) {
1192 rd_type = "LRL";
1193 } else {
1194 rd_type = "RLR";
1195 }
1196 tmp_length = 3;
1197 RSP_lengths[0] = x_param * RSP_param.t;
1198 RSP_lengths[1] = x_param * RSP_param.u;
1199 RSP_lengths[2] = x_param * RSP_param.v;
1200 } else if (i < 18) { // CCC, LRL case, backward
1201 LRL(x_param * xb, y_param * yb, x_param * y_param * dphi, &RSP_param);
1202 if (y_param > 0) {
1203 rd_type = "LRL";
1204 } else {
1205 rd_type = "RLR";
1206 }
1207 tmp_length = 3;
1208 RSP_lengths[0] = x_param * RSP_param.v;
1209 RSP_lengths[1] = x_param * RSP_param.u;
1210 RSP_lengths[2] = x_param * RSP_param.t;
1211 } else if (i < 22) { // CCCC, LRLRn
1212 LRLRn(x_param * x, y_param * y, x_param * y_param * dphi, &RSP_param);
1213 if (y_param > 0.0) {
1214 rd_type = "LRLR";
1215 } else {
1216 rd_type = "RLRL";
1217 }
1218 tmp_length = 4;
1219 RSP_lengths[0] = x_param * RSP_param.t;
1220 RSP_lengths[1] = x_param * RSP_param.u;
1221 RSP_lengths[2] = -x_param * RSP_param.u;
1222 RSP_lengths[3] = x_param * RSP_param.v;
1223 } else if (i < 26) { // CCCC, LRLRp
1224 LRLRp(x_param * x, y_param * y, x_param * y_param * dphi, &RSP_param);
1225 if (y_param > 0.0) {
1226 rd_type = "LRLR";
1227 } else {
1228 rd_type = "RLRL";
1229 }
1230 tmp_length = 4;
1231 RSP_lengths[0] = x_param * RSP_param.t;
1232 RSP_lengths[1] = x_param * RSP_param.u;
1233 RSP_lengths[2] = -x_param * RSP_param.u;
1234 RSP_lengths[3] = x_param * RSP_param.v;
1235 } else if (i < 30) { // CCSC, LRLRn
1236 tmp_length = 4;
1237 LRLRn(x_param * x, y_param * y, x_param * y_param * dphi, &RSP_param);
1238 if (y_param > 0.0) {
1239 rd_type = "LRSL";
1240 } else {
1241 rd_type = "RLSR";
1242 }
1243 RSP_lengths[0] = x_param * RSP_param.t;
1244 if (x_param < 0 && y_param > 0) {
1245 RSP_lengths[1] = 0.5 * M_PI;
1246 } else {
1247 RSP_lengths[1] = -0.5 * M_PI;
1248 }
1249 if (x_param > 0 && y_param < 0) {
1250 RSP_lengths[2] = RSP_param.u;
1251 } else {
1252 RSP_lengths[2] = -RSP_param.u;
1253 }
1254 RSP_lengths[3] = x_param * RSP_param.v;
1255 } else if (i < 34) { // CCSC, LRLRp
1256 tmp_length = 4;
1257 LRLRp(x_param * x, y_param * y, x_param * y_param * dphi, &RSP_param);
1258 if (y_param) {
1259 rd_type = "LRSR";
1260 } else {
1261 rd_type = "RLSL";
1262 }
1263 RSP_lengths[0] = x_param * RSP_param.t;
1264 if (x_param < 0 && y_param > 0) {
1265 RSP_lengths[1] = 0.5 * M_PI;
1266 } else {
1267 RSP_lengths[1] = -0.5 * M_PI;
1268 }
1269 RSP_lengths[2] = x_param * RSP_param.u;
1270 RSP_lengths[3] = x_param * RSP_param.v;
1271 } else if (i < 38) { // CCSC, LRLRn, backward
1272 tmp_length = 4;
1273 LRLRn(x_param * xb, y_param * yb, x_param * y_param * dphi, &RSP_param);
1274 if (y_param > 0) {
1275 rd_type = "LSRL";
1276 } else {
1277 rd_type = "RSLR";
1278 }
1279 RSP_lengths[0] = x_param * RSP_param.v;
1280 RSP_lengths[1] = x_param * RSP_param.u;
1281 RSP_lengths[2] = -x_param * 0.5 * M_PI;
1282 RSP_lengths[3] = x_param * RSP_param.t;
1283 } else if (i < 42) { // CCSC, LRLRp, backward
1284 tmp_length = 4;
1285 LRLRp(x_param * xb, y_param * yb, x_param * y_param * dphi, &RSP_param);
1286 if (y_param > 0) {
1287 rd_type = "RSRL";
1288 } else {
1289 rd_type = "LSLR";
1290 }
1291 RSP_lengths[0] = x_param * RSP_param.v;
1292 RSP_lengths[1] = x_param * RSP_param.u;
1293 RSP_lengths[2] = -x_param * M_PI * 0.5;
1294 RSP_lengths[3] = x_param * RSP_param.t;
1295 } else { // CCSCC, LRSLR
1296 tmp_length = 5;
1297 LRSLR(x_param * x, y_param * y, x_param * y_param * dphi, &RSP_param);
1298 if (y_param > 0.0) {
1299 rd_type = "LRSLR";
1300 } else {
1301 rd_type = "RLSRL";
1302 }
1303 RSP_lengths[0] = x_param * RSP_param.t;
1304 RSP_lengths[1] = -x_param * 0.5 * M_PI;
1305 RSP_lengths[2] = x_param * RSP_param.u;
1306 RSP_lengths[3] = -x_param * 0.5 * M_PI;
1307 RSP_lengths[4] = x_param * RSP_param.v;
1308 }
1309
1310 if (tmp_length > 0) {
1311 if (RSP_param.flag && !SetRSPPar(tmp_length, RSP_lengths, rd_type, all_possible_paths, i)) {
1312 AERROR << "Fail at SetRSP, idx#: " << i;
1313 succ = false;
1314 }
1315 }
1316 }
1317
1318 if (!succ) {
1319 AERROR << "RSP parallel fails";
1320 return false;
1321 }
1322 if (all_possible_paths->size() == 0) {
1323 AERROR << "No path generated by certain two configurations";
1324 return false;
1325 }
1326 return true;
1327}
apollo::planning::ReedShepp::SLS
void SLS(const double x, const double y, const double phi, RSPParam *param)
Definition reeds_shepp_path.cc:810
apollo::planning::ReedShepp::SetRSPPar
bool SetRSPPar(const int size, const double *lengths, const std::string &types, std::vector< ReedSheppPath > *all_possible_paths, const int idx)
Definition reeds_shepp_path.cc:1095
AERROR
#define AERROR
Definition log.h:44

◆ GenerateRSPs()

bool apollo::planning::ReedShepp::GenerateRSPs ( const std::shared_ptr< Node3d >  start_node,
const std::shared_ptr< Node3d >  end_node,
std::vector< ReedSheppPath > *  all_possible_paths 
)
protected

在文件 reeds_shepp_path.cc 第 179 行定义.

182 {
183 if (FLAGS_enable_parallel_hybrid_a) {
184 // AINFO << "parallel hybrid a*";
185 if (!GenerateRSPPar(start_node, end_node, all_possible_paths)) {
186 ADEBUG << "Fail to generate general profile of different RSPs";
187 return false;
188 }
189 } else {
190 if (!GenerateRSP(start_node, end_node, all_possible_paths)) {
191 AERROR << "Fail to generate general profile of different RSPs";
192 return false;
193 }
194 }
195 return true;
196}
apollo::planning::ReedShepp::GenerateRSPPar
bool GenerateRSPPar(const std::shared_ptr< Node3d > start_node, const std::shared_ptr< Node3d > end_node, std::vector< ReedSheppPath > *all_possible_paths)
Definition reeds_shepp_path.cc:1120
apollo::planning::ReedShepp::GenerateRSP
bool GenerateRSP(const std::shared_ptr< Node3d > start_node, const std::shared_ptr< Node3d > end_node, std::vector< ReedSheppPath > *all_possible_paths)
Definition reeds_shepp_path.cc:198

◆ Interpolation()

void apollo::planning::ReedShepp::Interpolation ( const int  index,
const double  pd,
const char  m,
const double  ox,
const double  oy,
const double  ophi,
const double  radius,
std::vector< double > *  px,
std::vector< double > *  py,
std::vector< double > *  pphi,
std::vector< bool > *  pgear 
)
protected

在文件 reeds_shepp_path.cc 第 1049 行定义.

1060 {
1061 double ldx = 0.0;
1062 double ldy = 0.0;
1063 double gdx = 0.0;
1064 double gdy = 0.0;
1065 if (m == 'S') {
1066 px->at(index) = ox + pd / max_kappa_ * std::cos(ophi);
1067 py->at(index) = oy + pd / max_kappa_ * std::sin(ophi);
1068 pphi->at(index) = ophi;
1069 } else {
1070 ldx = std::sin(pd) / max_kappa_ * radius;
1071 if (m == 'L') {
1072 ldy = (1.0 - std::cos(pd)) / max_kappa_ * radius;
1073 } else if (m == 'R') {
1074 ldy = (1.0 - std::cos(pd)) / -max_kappa_ * radius;
1075 }
1076 gdx = std::cos(-ophi) * ldx + std::sin(-ophi) * ldy;
1077 gdy = -std::sin(-ophi) * ldx + std::cos(-ophi) * ldy;
1078 px->at(index) = ox + gdx;
1079 py->at(index) = oy + gdy;
1080 }
1081
1082 if (pd > 0.0) {
1083 pgear->at(index) = true;
1084 } else {
1085 pgear->at(index) = false;
1086 }
1087
1088 if (m == 'L') {
1089 pphi->at(index) = ophi + pd;
1090 } else if (m == 'R') {
1091 pphi->at(index) = ophi - pd;
1092 }
1093}

◆ LRL()

void apollo::planning::ReedShepp::LRL ( const double  x,
const double  y,
const double  phi,
RSPParam *  param 
)
protected

在文件 reeds_shepp_path.cc 第 767 行定义.

767 {
768 std::pair<double, double> polar = common::math::Cartesian2Polar(x - std::sin(phi), y - 1.0 + std::cos(phi));
769 double u1 = polar.first;
770 double t1 = polar.second;
771 double u = 0.0;
772 double t = 0.0;
773 double v = 0.0;
774 if (u1 <= 4.0) {
775 u = -2.0 * std::asin(0.25 * u1);
776 t = common::math::NormalizeAngle(t1 + 0.5 * u + M_PI);
777 v = common::math::NormalizeAngle(phi - t + u);
778 if (t >= 0.0 && u <= 0.0) {
779 param->flag = true;
780 param->u = u;
781 param->t = t;
782 param->v = v;
783 }
784 }
785}
apollo::common::math::Cartesian2Polar
std::pair< double, double > Cartesian2Polar(double x, double y)
Definition math_utils.cc:93

◆ LRLRn()

void apollo::planning::ReedShepp::LRLRn ( const double  x,
const double  y,
const double  phi,
RSPParam *  param 
)
protected

在文件 reeds_shepp_path.cc 第 838 行定义.

838 {
839 double xi = x + std::sin(phi);
840 double eta = y - 1.0 - std::cos(phi);
841 double rho = 0.25 * (2.0 + std::sqrt(xi * xi + eta * eta));
842 double u = 0.0;
843 if (rho <= 1.0 && rho >= 0.0) {
844 u = std::acos(rho);
845 if (u >= 0 && u <= 0.5 * M_PI) {
846 std::pair<double, double> tau_omega = calc_tau_omega(u, -u, xi, eta, phi);
847 if (tau_omega.first >= 0.0 && tau_omega.second <= 0.0) {
848 param->flag = true;
849 param->u = u;
850 param->t = tau_omega.first;
851 param->v = tau_omega.second;
852 }
853 }
854 }
855}
apollo::planning::ReedShepp::calc_tau_omega
std::pair< double, double > calc_tau_omega(const double u, const double v, const double xi, const double eta, const double phi)
Definition reeds_shepp_path.cc:63

◆ LRLRp()

void apollo::planning::ReedShepp::LRLRp ( const double  x,
const double  y,
const double  phi,
RSPParam *  param 
)
protected

在文件 reeds_shepp_path.cc 第 857 行定义.

857 {
858 double xi = x + std::sin(phi);
859 double eta = y - 1.0 - std::cos(phi);
860 double rho = (20.0 - xi * xi - eta * eta) / 16.0;
861 double u = 0.0;
862 if (rho <= 1.0 && rho >= 0.0) {
863 u = -std::acos(rho);
864 if (u >= 0 && u <= 0.5 * M_PI) {
865 std::pair<double, double> tau_omega = calc_tau_omega(u, u, xi, eta, phi);
866 if (tau_omega.first >= 0.0 && tau_omega.second >= 0.0) {
867 param->flag = true;
868 param->u = u;
869 param->t = tau_omega.first;
870 param->v = tau_omega.second;
871 }
872 }
873 }
874}

◆ LRSL()

void apollo::planning::ReedShepp::LRSL ( const double  x,
const double  y,
const double  phi,
RSPParam *  param 
)
protected

在文件 reeds_shepp_path.cc 第 898 行定义.

898 {
899 double xi = x - std::sin(phi);
900 double eta = y - 1.0 + std::cos(phi);
901 std::pair<double, double> polar = common::math::Cartesian2Polar(xi, eta);
902 double rho = polar.first;
903 double theta = polar.second;
904 double r = 0.0;
905 double t = 0.0;
906 double u = 0.0;
907 double v = 0.0;
908
909 if (rho >= 2.0) {
910 r = std::sqrt(rho * rho - 4.0);
911 u = 2.0 - r;
912 t = common::math::NormalizeAngle(theta + std::atan2(r, -2.0));
913 v = common::math::NormalizeAngle(phi - 0.5 * M_PI - t);
914 if (t >= 0.0 && u <= 0.0 && v <= 0.0) {
915 param->flag = true;
916 param->u = u;
917 param->t = t;
918 param->v = v;
919 }
920 }
921}

◆ LRSLR()

void apollo::planning::ReedShepp::LRSLR ( const double  x,
const double  y,
const double  phi,
RSPParam *  param 
)
protected

在文件 reeds_shepp_path.cc 第 923 行定义.

923 {
924 double xi = x + std::sin(phi);
925 double eta = y - 1.0 - std::cos(phi);
926 std::pair<double, double> polar = common::math::Cartesian2Polar(xi, eta);
927 double rho = polar.first;
928 double t = 0.0;
929 double u = 0.0;
930 double v = 0.0;
931 if (rho >= 2.0) {
932 u = 4.0 - std::sqrt(rho * rho - 4.0);
933 if (u <= 0.0) {
934 t = common::math::NormalizeAngle(atan2((4.0 - u) * xi - 2.0 * eta, -2.0 * xi + (u - 4.0) * eta));
935 v = common::math::NormalizeAngle(t - phi);
936
937 if (t >= 0.0 && v >= 0.0) {
938 param->flag = true;
939 param->u = u;
940 param->t = t;
941 param->v = v;
942 }
943 }
944 }
945}

◆ LRSR()

void apollo::planning::ReedShepp::LRSR ( const double  x,
const double  y,
const double  phi,
RSPParam *  param 
)
protected

在文件 reeds_shepp_path.cc 第 876 行定义.

876 {
877 double xi = x + std::sin(phi);
878 double eta = y - 1.0 - std::cos(phi);
879 std::pair<double, double> polar = common::math::Cartesian2Polar(-eta, xi);
880 double rho = polar.first;
881 double theta = polar.second;
882 double t = 0.0;
883 double u = 0.0;
884 double v = 0.0;
885 if (rho >= 2.0) {
886 t = theta;
887 u = 2.0 - rho;
888 v = common::math::NormalizeAngle(t + 0.5 * M_PI - phi);
889 if (t >= 0.0 && u <= 0.0 && v <= 0.0) {
890 param->flag = true;
891 param->u = u;
892 param->t = t;
893 param->v = v;
894 }
895 }
896}

◆ LS()

void apollo::planning::ReedShepp::LS ( const double  x,
const double  y,
const double  phi,
RSPParam *  param 
)
protected

在文件 reeds_shepp_path.cc 第 787 行定义.

787 {
788 double phi_mod = common::math::NormalizeAngle(phi);
789 double epsilon = 1e-6;
790 if (std::fabs(phi_mod - M_PI_2) < epsilon || std::fabs(phi_mod) < epsilon || std::fabs(phi_mod + M_PI_2) < epsilon
791 || std::fabs(phi_mod + M_PI) < epsilon) {
792 return;
793 }
794 double r = (y - x * std::tan(phi_mod)) / (1 - 1 / std::cos(phi_mod));
795 if (r < 1.0) {
796 return;
797 }
798 // u>0 :lpsp ,u<0 :lpsn
799 double u = x / std::cos(phi_mod) - r * std::tan(phi_mod);
800 double t = phi_mod;
801 if (phi_mod < 0) {
802 t = phi_mod + 2 * M_PI;
803 }
804 param->flag = true;
805 param->u = u;
806 param->t = t;
807 param->v = r;
808}

◆ LSL()

void apollo::planning::ReedShepp::LSL ( const double  x,
const double  y,
const double  phi,
RSPParam *  param 
)
protected

在文件 reeds_shepp_path.cc 第 729 行定义.

729 {
730 std::pair<double, double> polar = common::math::Cartesian2Polar(x - std::sin(phi), y - 1.0 + std::cos(phi));
731 double u = polar.first;
732 double t = polar.second;
733 double v = 0.0;
734 if (t >= 0.0) {
735 v = common::math::NormalizeAngle(phi - t);
736 if (v >= 0.0) {
737 param->flag = true;
738 param->u = u;
739 param->t = t;
740 param->v = v;
741 }
742 }
743}

◆ LSR()

void apollo::planning::ReedShepp::LSR ( const double  x,
const double  y,
const double  phi,
RSPParam *  param 
)
protected

在文件 reeds_shepp_path.cc 第 745 行定义.

745 {
746 std::pair<double, double> polar = common::math::Cartesian2Polar(x + std::sin(phi), y - 1.0 - std::cos(phi));
747 double u1 = polar.first * polar.first;
748 double t1 = polar.second;
749 double u = 0.0;
750 double theta = 0.0;
751 double t = 0.0;
752 double v = 0.0;
753 if (u1 >= 4.0) {
754 u = std::sqrt(u1 - 4.0);
755 theta = std::atan2(2.0, u);
756 t = common::math::NormalizeAngle(t1 + theta);
757 v = common::math::NormalizeAngle(t - phi);
758 if (t >= 0.0 && v >= 0.0) {
759 param->flag = true;
760 param->u = u;
761 param->t = t;
762 param->v = v;
763 }
764 }
765}

◆ SCS()

bool apollo::planning::ReedShepp::SCS ( const double  x,
const double  y,
const double  phi,
std::vector< ReedSheppPath > *  all_possible_paths 
)
protected

在文件 reeds_shepp_path.cc 第 248 行定义.

248 {
249 RSPParam SLS_param;
250 SLS(x, y, phi, &SLS_param);
251 double SLS_lengths[3] = {SLS_param.t, SLS_param.u, SLS_param.v};
252 char SLS_types[] = "SLS";
253 if (SLS_param.flag && !SetRSP(3, SLS_lengths, SLS_types, all_possible_paths)) {
254 ADEBUG << "Fail at SetRSP with SLS_param";
255 return false;
256 }
257
258 RSPParam SRS_param;
259 SLS(x, -y, -phi, &SRS_param);
260 double SRS_lengths[3] = {SRS_param.t, SRS_param.u, SRS_param.v};
261 char SRS_types[] = "SRS";
262 if (SRS_param.flag && !SetRSP(3, SRS_lengths, SRS_types, all_possible_paths)) {
263 ADEBUG << "Fail at SetRSP with SRS_param";
264 return false;
265 }
266 return true;
267}

◆ SetRSP()

bool apollo::planning::ReedShepp::SetRSP ( const int  size,
const double *  lengths,
const char *  types,
std::vector< ReedSheppPath > *  all_possible_paths,
double  radius = 1.0 
)
protected

在文件 reeds_shepp_path.cc 第 947 行定义.

952 {
953 ReedSheppPath path;
954 std::vector<double> length_vec(lengths, lengths + size);
955 std::vector<char> type_vec(types, types + size);
956 path.segs_lengths = length_vec;
957 path.segs_types = type_vec;
958 path.radius = radius;
959 double sum = 0.0;
960 for (int i = 0; i < size; ++i) {
961 sum += std::abs(lengths[i]);
962 }
963 path.total_length = sum;
964 if (path.total_length <= 0.0) {
965 AERROR << "total length smaller than 0";
966 return false;
967 }
968 all_possible_paths->emplace_back(path);
969 return true;
970}

◆ SetRSPPar()

bool apollo::planning::ReedShepp::SetRSPPar ( const int  size,
const double *  lengths,
const std::string &  types,
std::vector< ReedSheppPath > *  all_possible_paths,
const int  idx 
)
protected

在文件 reeds_shepp_path.cc 第 1095 行定义.

1100 {
1101 ReedSheppPath path;
1102 std::vector<double> length_vec(lengths, lengths + size);
1103 std::vector<char> type_vec(types.begin(), types.begin() + size);
1104 path.segs_lengths = length_vec;
1105 path.segs_types = type_vec;
1106 double sum = 0.0;
1107 for (int i = 0; i < size; ++i) {
1108 sum += std::abs(lengths[i]);
1109 }
1110 path.total_length = sum;
1111 if (path.total_length <= 0.0) {
1112 AERROR << "total length smaller than 0";
1113 return false;
1114 }
1115
1116 all_possible_paths->at(idx) = path;
1117 return true;
1118}

◆ ShortestRSP()

bool apollo::planning::ReedShepp::ShortestRSP ( const std::shared_ptr< Node3d >  start_node,
const std::shared_ptr< Node3d >  end_node,
std::shared_ptr< ReedSheppPath >  optimal_path 
)

在文件 reeds_shepp_path.cc 第 85 行定义.

88 {
89 std::vector<ReedSheppPath> all_possible_paths;
90 if (!GenerateRSPs(start_node, end_node, &all_possible_paths)) {
91 ADEBUG << "Fail to generate different combination of Reed Shepp "
92 "paths";
93 return false;
94 }
95
96 double start_dire = 1;
97 if (start_node->GetDirec() == false)
98 start_dire = -1;
99
100 size_t optimal_path_index = 0;
101 size_t paths_size = all_possible_paths.size();
102 double min_cost = std::numeric_limits<double>::max();
103 for (size_t i = 0; i < paths_size; ++i) {
104 double cost = 0;
105 auto& path = all_possible_paths[i];
106 double steering_radius = path.radius / max_kappa_;
107 double steer_change_penalty_cost
108 = std::atan(vehicle_param_.wheel_base() / steering_radius * 2.0) * traj_steer_change_penalty_;
109 for (size_t j = 0; j < path.segs_lengths.size(); j++) {
110 if (path.segs_types[j] != 'S') {
111 cost += std::fabs(path.segs_lengths[j]) * (traj_steer_penalty_) / max_kappa_ * path.radius;
112 if (j > 0 && (path.segs_types[j - 1] != 'S') && (path.segs_types[j - 1] != path.segs_types[j])) {
113 cost += steer_change_penalty_cost;
114 }
115 if (std::fabs(path.segs_lengths[j]) / max_kappa_ * path.radius < traj_expected_shortest_length_) {
116 cost += traj_short_length_penalty_;
117 }
118 } else {
119 if (path.segs_lengths[j] < 0) {
120 cost += -path.segs_lengths[j] * traj_back_penalty_ / max_kappa_;
121 } else {
122 cost += path.segs_lengths[j] * traj_forward_penalty_ / max_kappa_;
123 }
124 if (std::fabs(path.segs_lengths[j]) / max_kappa_ < traj_expected_shortest_length_) {
125 cost += traj_short_length_penalty_;
126 }
127 }
128
129 if (j > 0 && path.segs_lengths[j] * path.segs_lengths[j - 1] < 0) {
130 cost += traj_gear_switch_penalty_;
131 }
132 if (j == 0 && start_dire * path.segs_lengths[j] < 0) {
133 cost += traj_gear_switch_penalty_;
134 }
135 }
136 if (cost < min_cost) {
137 optimal_path_index = i;
138 min_cost = cost;
139 }
140 }
141
142 if (!GenerateLocalConfigurations(start_node, end_node, &(all_possible_paths[optimal_path_index]))) {
143 ADEBUG << "Fail to generate local configurations(x, y, phi) in SetRSP";
144 return false;
145 }
146 // std::stringstream ssm;
147 // auto& path = all_possible_paths[optimal_path_index];
148 // for (size_t i = 0; i < path.x.size(); i++) {
149 // ssm << path.x[i] << "," << path.y[i] << "," << path.phi[i] << "\n";
150 // }
151 // ssm << "--rspath\n";
152 // AERROR << ssm.str();
153
154 if (std::abs(all_possible_paths[optimal_path_index].x.back() - end_node->GetX()) > 1e-3
155 || std::abs(all_possible_paths[optimal_path_index].y.back() - end_node->GetY()) > 1e-3
156 || common::math::NormalizeAngle(all_possible_paths[optimal_path_index].phi.back() - end_node->GetPhi())
157 > 1e-3) {
158 ADEBUG << "RSP end position not right";
159 for (size_t i = 0; i < all_possible_paths[optimal_path_index].segs_types.size(); ++i) {
160 ADEBUG << "types are " << all_possible_paths[optimal_path_index].segs_types[i];
161 }
162 ADEBUG << "x, y, phi are: " << all_possible_paths[optimal_path_index].x.back() << ", "
163 << all_possible_paths[optimal_path_index].y.back() << ", "
164 << all_possible_paths[optimal_path_index].phi.back();
165 ADEBUG << "end x, y, phi are: " << end_node->GetX() << ", " << end_node->GetY() << ", " << end_node->GetPhi();
166 return false;
167 }
168 (*optimal_path).cost = min_cost;
169 (*optimal_path).x = all_possible_paths[optimal_path_index].x;
170 (*optimal_path).y = all_possible_paths[optimal_path_index].y;
171 (*optimal_path).phi = all_possible_paths[optimal_path_index].phi;
172 (*optimal_path).gear = all_possible_paths[optimal_path_index].gear;
173 (*optimal_path).total_length = all_possible_paths[optimal_path_index].total_length;
174 (*optimal_path).segs_types = all_possible_paths[optimal_path_index].segs_types;
175 (*optimal_path).segs_lengths = all_possible_paths[optimal_path_index].segs_lengths;
176 return true;
177}
apollo::planning::ReedShepp::GenerateLocalConfigurations
bool GenerateLocalConfigurations(const std::shared_ptr< Node3d > start_node, const std::shared_ptr< Node3d > end_node, ReedSheppPath *shortest_path)
Definition reeds_shepp_path.cc:973
apollo::planning::ReedShepp::GenerateRSPs
bool GenerateRSPs(const std::shared_ptr< Node3d > start_node, const std::shared_ptr< Node3d > end_node, std::vector< ReedSheppPath > *all_possible_paths)
Definition reeds_shepp_path.cc:179

◆ SLS()

void apollo::planning::ReedShepp::SLS ( const double  x,
const double  y,
const double  phi,
RSPParam *  param 
)
protected

在文件 reeds_shepp_path.cc 第 810 行定义.

810 {
811 double phi_mod = common::math::NormalizeAngle(phi);
812 double xd = 0.0;
813 double u = 0.0;
814 double t = 0.0;
815 double v = 0.0;
816 double epsilon = 1e-1;
817 if (y > 0.0 && phi_mod > epsilon && phi_mod < M_PI) {
818 xd = -y / std::tan(phi_mod) + x;
819 t = xd - std::tan(phi_mod / 2.0);
820 u = phi_mod;
821 v = std::sqrt((x - xd) * (x - xd) + y * y) - tan(phi_mod / 2.0);
822 param->flag = true;
823 param->u = u;
824 param->t = t;
825 param->v = v;
826 } else if (y < 0.0 && phi_mod > epsilon && phi_mod < M_PI) {
827 xd = -y / std::tan(phi_mod) + x;
828 t = xd - std::tan(phi_mod / 2.0);
829 u = phi_mod;
830 v = -std::sqrt((x - xd) * (x - xd) + y * y) - std::tan(phi_mod / 2.0);
831 param->flag = true;
832 param->u = u;
833 param->t = t;
834 param->v = v;
835 }
836}
apollo::common::math::tan
float tan(Angle16 a)
Definition angle.cc:47

类成员变量说明

◆ max_kappa_

double apollo::planning::ReedShepp::max_kappa_
protected

在文件 reeds_shepp_path.h 第 157 行定义.

◆ planner_open_space_config_

PlannerOpenSpaceConfig apollo::planning::ReedShepp::planner_open_space_config_
protected

在文件 reeds_shepp_path.h 第 156 行定义.

◆ planner_warm_start_config_

WarmStartConfig apollo::planning::ReedShepp::planner_warm_start_config_
protected

在文件 reeds_shepp_path.h 第 173 行定义.

◆ reeds_sheep_last_straight_

bool apollo::planning::ReedShepp::reeds_sheep_last_straight_ = false

在文件 reeds_shepp_path.h 第 170 行定义.

◆ traj_back_penalty_

double apollo::planning::ReedShepp::traj_back_penalty_
protected

在文件 reeds_shepp_path.h 第 159 行定义.

◆ traj_expected_shortest_length_

double apollo::planning::ReedShepp::traj_expected_shortest_length_
protected

在文件 reeds_shepp_path.h 第 164 行定义.

◆ traj_forward_penalty_

double apollo::planning::ReedShepp::traj_forward_penalty_
protected

在文件 reeds_shepp_path.h 第 158 行定义.

◆ traj_gear_switch_penalty_

double apollo::planning::ReedShepp::traj_gear_switch_penalty_
protected

在文件 reeds_shepp_path.h 第 160 行定义.

◆ traj_short_length_penalty_

double apollo::planning::ReedShepp::traj_short_length_penalty_
protected

在文件 reeds_shepp_path.h 第 163 行定义.

◆ traj_steer_change_penalty_

double apollo::planning::ReedShepp::traj_steer_change_penalty_
protected

在文件 reeds_shepp_path.h 第 162 行定义.

◆ traj_steer_penalty_

double apollo::planning::ReedShepp::traj_steer_penalty_
protected

在文件 reeds_shepp_path.h 第 161 行定义.

◆ vehicle_param_

common::VehicleParam apollo::planning::ReedShepp::vehicle_param_
protected

在文件 reeds_shepp_path.h 第 155 行定义.

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