Apollo 10.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 行定义.

28 : vehicle_param_(vehicle_param),
29 planner_open_space_config_(open_space_conf) {
30 max_kappa_ = std::tan(vehicle_param_.max_steer_angle() *
31 planner_open_space_config_.warm_start_config()
32 .traj_kappa_contraint_ratio() /
33 vehicle_param_.steer_ratio()) /
34 vehicle_param_.wheel_base();
35 traj_forward_penalty_ =
36 planner_open_space_config_.warm_start_config().traj_forward_penalty();
37 traj_back_penalty_ =
38 planner_open_space_config_.warm_start_config().traj_back_penalty();
39 traj_gear_switch_penalty_ =
40 planner_open_space_config_.warm_start_config().traj_gear_switch_penalty();
41 traj_steer_penalty_ =
42 planner_open_space_config_.warm_start_config().traj_steer_penalty();
43 traj_steer_change_penalty_ = planner_open_space_config_.warm_start_config()
44 .traj_steer_change_penalty();
45 traj_short_length_penalty_ = planner_open_space_config_.warm_start_config()
46 .traj_short_length_penalty();
47 traj_expected_shortest_length_ =
48 planner_open_space_config_.warm_start_config()
49 .traj_expected_shortest_length();
50 AINFO << "max kappa: " << max_kappa_;
51 AINFO << "traj_short_length_penalty_: " << traj_short_length_penalty_;
52 AINFO_IF(FLAGS_enable_parallel_hybrid_a) << "parallel REEDShepp";
53}
apollo::planning::ReedShepp::planner_open_space_config_
PlannerOpenSpaceConfig planner_open_space_config_
Definition reeds_shepp_path.h:162
apollo::planning::ReedShepp::vehicle_param_
common::VehicleParam vehicle_param_
Definition reeds_shepp_path.h:161
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 
)

成员函数说明

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

60 {
61 double delta = common::math::NormalizeAngle(u - v);
62 double A = std::sin(u) - std::sin(delta);
63 double B = std::cos(u) - std::cos(delta) - 1.0;
64
65 double t1 = std::atan2(eta * A - xi * B, xi * A + eta * B);
66 double t2 = 2.0 * (std::cos(delta) - std::cos(v) - std::cos(u)) + 3.0;
67 double tau = 0.0;
68 if (t2 < 0) {
69 tau = common::math::NormalizeAngle(t1 + M_PI);
70 } else {
71 tau = common::math::NormalizeAngle(t1);
72 }
73 double omega = common::math::NormalizeAngle(tau - u + v - phi);
74 return std::make_pair(tau, omega);
75}
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:160

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

398 {
399 RSPParam LRL1_param;
400 LRL(x, y, phi, &LRL1_param);
401 double LRL1_lengths[3] = {LRL1_param.t, LRL1_param.u, LRL1_param.v};
402 char LRL1_types[] = "LRL";
403 if (LRL1_param.flag &&
404 !SetRSP(3, LRL1_lengths, LRL1_types, all_possible_paths)) {
405 ADEBUG << "Fail at SetRSP with LRL_param";
406 return false;
407 }
408
409 RSPParam LRL2_param;
410 LRL(-x, y, -phi, &LRL2_param);
411 double LRL2_lengths[3] = {-LRL2_param.t, -LRL2_param.u, -LRL2_param.v};
412 char LRL2_types[] = "LRL";
413 if (LRL2_param.flag &&
414 !SetRSP(3, LRL2_lengths, LRL2_types, all_possible_paths)) {
415 ADEBUG << "Fail at SetRSP with LRL2_param";
416 return false;
417 }
418
419 RSPParam LRL3_param;
420 LRL(x, -y, -phi, &LRL3_param);
421 double LRL3_lengths[3] = {LRL3_param.t, LRL3_param.u, LRL3_param.v};
422 char LRL3_types[] = "RLR";
423 if (LRL3_param.flag &&
424 !SetRSP(3, LRL3_lengths, LRL3_types, all_possible_paths)) {
425 ADEBUG << "Fail at SetRSP with LRL3_param";
426 return false;
427 }
428
429 RSPParam LRL4_param;
430 LRL(-x, -y, phi, &LRL4_param);
431 double LRL4_lengths[3] = {-LRL4_param.t, -LRL4_param.u, -LRL4_param.v};
432 char LRL4_types[] = "RLR";
433 if (LRL4_param.flag &&
434 !SetRSP(3, LRL4_lengths, LRL4_types, all_possible_paths)) {
435 ADEBUG << "Fail at SetRSP with LRL4_param";
436 return false;
437 }
438
439 // backward
440 double xb = x * std::cos(phi) + y * std::sin(phi);
441 double yb = x * std::sin(phi) - y * std::cos(phi);
442
443 RSPParam LRL5_param;
444 LRL(xb, yb, phi, &LRL5_param);
445 double LRL5_lengths[3] = {LRL5_param.v, LRL5_param.u, LRL5_param.t};
446 char LRL5_types[] = "LRL";
447 if (LRL5_param.flag &&
448 !SetRSP(3, LRL5_lengths, LRL5_types, all_possible_paths)) {
449 ADEBUG << "Fail at SetRSP with LRL5_param";
450 return false;
451 }
452
453 RSPParam LRL6_param;
454 LRL(-xb, yb, -phi, &LRL6_param);
455 double LRL6_lengths[3] = {-LRL6_param.v, -LRL6_param.u, -LRL6_param.t};
456 char LRL6_types[] = "LRL";
457 if (LRL6_param.flag &&
458 !SetRSP(3, LRL6_lengths, LRL6_types, all_possible_paths)) {
459 ADEBUG << "Fail at SetRSP with LRL6_param";
460 return false;
461 }
462
463 RSPParam LRL7_param;
464 LRL(xb, -yb, -phi, &LRL7_param);
465 double LRL7_lengths[3] = {LRL7_param.v, LRL7_param.u, LRL7_param.t};
466 char LRL7_types[] = "RLR";
467 if (LRL7_param.flag &&
468 !SetRSP(3, LRL7_lengths, LRL7_types, all_possible_paths)) {
469 ADEBUG << "Fail at SetRSP with LRL7_param";
470 return false;
471 }
472
473 RSPParam LRL8_param;
474 LRL(-xb, -yb, phi, &LRL8_param);
475 double LRL8_lengths[3] = {-LRL8_param.v, -LRL8_param.u, -LRL8_param.t};
476 char LRL8_types[] = "RLR";
477 if (LRL8_param.flag &&
478 !SetRSP(3, LRL8_lengths, LRL8_types, all_possible_paths)) {
479 ADEBUG << "Fail at SetRSP with LRL8_param";
480 return false;
481 }
482 return true;
483}
apollo::planning::ReedShepp::LRL
void LRL(const double x, const double y, const double phi, RSPParam *param)
Definition reeds_shepp_path.cc:851
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:1042
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 第 485 行定义.

486 {
487 RSPParam LRLRn1_param;
488 LRLRn(x, y, phi, &LRLRn1_param);
489 double LRLRn1_lengths[4] = {LRLRn1_param.t, LRLRn1_param.u, -LRLRn1_param.u,
490 LRLRn1_param.v};
491 char LRLRn1_types[] = "LRLR";
492 if (LRLRn1_param.flag &&
493 !SetRSP(4, LRLRn1_lengths, LRLRn1_types, all_possible_paths)) {
494 ADEBUG << "Fail at SetRSP with LRLRn_param";
495 return false;
496 }
497
498 RSPParam LRLRn2_param;
499 LRLRn(-x, y, -phi, &LRLRn2_param);
500 double LRLRn2_lengths[4] = {-LRLRn2_param.t, -LRLRn2_param.u, LRLRn2_param.u,
501 -LRLRn2_param.v};
502 char LRLRn2_types[] = "LRLR";
503 if (LRLRn2_param.flag &&
504 !SetRSP(4, LRLRn2_lengths, LRLRn2_types, all_possible_paths)) {
505 ADEBUG << "Fail at SetRSP with LRLRn2_param";
506 return false;
507 }
508
509 RSPParam LRLRn3_param;
510 LRLRn(x, -y, -phi, &LRLRn3_param);
511 double LRLRn3_lengths[4] = {LRLRn3_param.t, LRLRn3_param.u, -LRLRn3_param.u,
512 LRLRn3_param.v};
513 char LRLRn3_types[] = "RLRL";
514 if (LRLRn3_param.flag &&
515 !SetRSP(4, LRLRn3_lengths, LRLRn3_types, all_possible_paths)) {
516 ADEBUG << "Fail at SetRSP with LRLRn3_param";
517 return false;
518 }
519
520 RSPParam LRLRn4_param;
521 LRLRn(-x, -y, phi, &LRLRn4_param);
522 double LRLRn4_lengths[4] = {-LRLRn4_param.t, -LRLRn4_param.u, LRLRn4_param.u,
523 -LRLRn4_param.v};
524 char LRLRn4_types[] = "RLRL";
525 if (LRLRn4_param.flag &&
526 !SetRSP(4, LRLRn4_lengths, LRLRn4_types, all_possible_paths)) {
527 ADEBUG << "Fail at SetRSP with LRLRn4_param";
528 return false;
529 }
530
531 RSPParam LRLRp1_param;
532 LRLRp(x, y, phi, &LRLRp1_param);
533 double LRLRp1_lengths[4] = {LRLRp1_param.t, LRLRp1_param.u, LRLRp1_param.u,
534 LRLRp1_param.v};
535 char LRLRp1_types[] = "LRLR";
536 if (LRLRp1_param.flag &&
537 !SetRSP(4, LRLRp1_lengths, LRLRp1_types, all_possible_paths)) {
538 ADEBUG << "Fail at SetRSP with LRLRp1_param";
539 return false;
540 }
541
542 RSPParam LRLRp2_param;
543 LRLRp(-x, y, -phi, &LRLRp2_param);
544 double LRLRp2_lengths[4] = {-LRLRp2_param.t, -LRLRp2_param.u, -LRLRp2_param.u,
545 -LRLRp2_param.v};
546 char LRLRp2_types[] = "LRLR";
547 if (LRLRp2_param.flag &&
548 !SetRSP(4, LRLRp2_lengths, LRLRp2_types, all_possible_paths)) {
549 ADEBUG << "Fail at SetRSP with LRLRp2_param";
550 return false;
551 }
552
553 RSPParam LRLRp3_param;
554 LRLRp(x, -y, -phi, &LRLRp3_param);
555 double LRLRp3_lengths[4] = {LRLRp3_param.t, LRLRp3_param.u, LRLRp3_param.u,
556 LRLRp3_param.v};
557 char LRLRp3_types[] = "RLRL";
558 if (LRLRp3_param.flag &&
559 !SetRSP(4, LRLRp3_lengths, LRLRp3_types, all_possible_paths)) {
560 ADEBUG << "Fail at SetRSP with LRLRp3_param";
561 return false;
562 }
563
564 RSPParam LRLRp4_param;
565 LRLRp(-x, -y, phi, &LRLRp4_param);
566 double LRLRp4_lengths[4] = {-LRLRp4_param.t, -LRLRp4_param.u, -LRLRp4_param.u,
567 -LRLRp4_param.v};
568 char LRLRp4_types[] = "RLRL";
569 if (LRLRp4_param.flag &&
570 !SetRSP(4, LRLRp4_lengths, LRLRp4_types, all_possible_paths)) {
571 ADEBUG << "Fail at SetRSP with LRLRp4_param";
572 return false;
573 }
574 return true;
575}
apollo::planning::ReedShepp::LRLRn
void LRLRn(const double x, const double y, const double phi, RSPParam *param)
Definition reeds_shepp_path.cc:927
apollo::planning::ReedShepp::LRLRp
void LRLRp(const double x, const double y, const double phi, RSPParam *param)
Definition reeds_shepp_path.cc:947

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

578 {
579 RSPParam LRSL1_param;
580 LRSL(x, y, phi, &LRSL1_param);
581 double LRSL1_lengths[4] = {LRSL1_param.t, -0.5 * M_PI, LRSL1_param.u,
582 LRSL1_param.v};
583 char LRSL1_types[] = "LRSL";
584 if (LRSL1_param.flag &&
585 !SetRSP(4, LRSL1_lengths, LRSL1_types, all_possible_paths)) {
586 ADEBUG << "Fail at SetRSP with LRSL1_param";
587 return false;
588 }
589
590 RSPParam LRSL2_param;
591 LRSL(-x, y, -phi, &LRSL2_param);
592 double LRSL2_lengths[4] = {-LRSL2_param.t, 0.5 * M_PI, -LRSL2_param.u,
593 -LRSL2_param.v};
594 char LRSL2_types[] = "LRSL";
595 if (LRSL2_param.flag &&
596 !SetRSP(4, LRSL2_lengths, LRSL2_types, all_possible_paths)) {
597 ADEBUG << "Fail at SetRSP with LRSL2_param";
598 return false;
599 }
600
601 RSPParam LRSL3_param;
602 LRSL(x, -y, -phi, &LRSL3_param);
603 double LRSL3_lengths[4] = {LRSL3_param.t, -0.5 * M_PI, LRSL3_param.u,
604 LRSL3_param.v};
605 char LRSL3_types[] = "RLSR";
606 if (LRSL3_param.flag &&
607 !SetRSP(4, LRSL3_lengths, LRSL3_types, all_possible_paths)) {
608 ADEBUG << "Fail at SetRSP with LRSL3_param";
609 return false;
610 }
611
612 RSPParam LRSL4_param;
613 LRSL(-x, -y, phi, &LRSL4_param);
614 double LRSL4_lengths[4] = {-LRSL4_param.t, 0.5 * M_PI, -LRSL4_param.u,
615 -LRSL4_param.v};
616 char LRSL4_types[] = "RLSR";
617 if (LRSL4_param.flag &&
618 !SetRSP(4, LRSL4_lengths, LRSL4_types, all_possible_paths)) {
619 ADEBUG << "Fail at SetRSP with LRSL4_param";
620 return false;
621 }
622
623 RSPParam LRSR1_param;
624 LRSR(x, y, phi, &LRSR1_param);
625 double LRSR1_lengths[4] = {LRSR1_param.t, -0.5 * M_PI, LRSR1_param.u,
626 LRSR1_param.v};
627 char LRSR1_types[] = "LRSR";
628 if (LRSR1_param.flag &&
629 !SetRSP(4, LRSR1_lengths, LRSR1_types, all_possible_paths)) {
630 ADEBUG << "Fail at SetRSP with LRSR1_param";
631 return false;
632 }
633
634 RSPParam LRSR2_param;
635 LRSR(-x, y, -phi, &LRSR2_param);
636 double LRSR2_lengths[4] = {-LRSR2_param.t, 0.5 * M_PI, -LRSR2_param.u,
637 -LRSR2_param.v};
638 char LRSR2_types[] = "LRSR";
639 if (LRSR2_param.flag &&
640 !SetRSP(4, LRSR2_lengths, LRSR2_types, all_possible_paths)) {
641 ADEBUG << "Fail at SetRSP with LRSR2_param";
642 return false;
643 }
644
645 RSPParam LRSR3_param;
646 LRSR(x, -y, -phi, &LRSR3_param);
647 double LRSR3_lengths[4] = {LRSR3_param.t, -0.5 * M_PI, LRSR3_param.u,
648 LRSR3_param.v};
649 char LRSR3_types[] = "RLSL";
650 if (LRSR3_param.flag &&
651 !SetRSP(4, LRSR3_lengths, LRSR3_types, all_possible_paths)) {
652 ADEBUG << "Fail at SetRSP with LRSR3_param";
653 return false;
654 }
655
656 RSPParam LRSR4_param;
657 LRSR(-x, -y, phi, &LRSR4_param);
658 double LRSR4_lengths[4] = {-LRSR4_param.t, 0.5 * M_PI, -LRSR4_param.u,
659 -LRSR4_param.v};
660 char LRSR4_types[] = "RLSL";
661 if (LRSR4_param.flag &&
662 !SetRSP(4, LRSR4_lengths, LRSR4_types, all_possible_paths)) {
663 ADEBUG << "Fail at SetRSP with LRSR4_param";
664 return false;
665 }
666
667 // backward
668 double xb = x * std::cos(phi) + y * std::sin(phi);
669 double yb = x * std::sin(phi) - y * std::cos(phi);
670
671 RSPParam LRSL5_param;
672 LRSL(xb, yb, phi, &LRSL5_param);
673 double LRSL5_lengths[4] = {LRSL5_param.v, LRSL5_param.u, -0.5 * M_PI,
674 LRSL5_param.t};
675 char LRSL5_types[] = "LSRL";
676 if (LRSL5_param.flag &&
677 !SetRSP(4, LRSL5_lengths, LRSL5_types, all_possible_paths)) {
678 ADEBUG << "Fail at SetRSP with LRLRn_param";
679 return false;
680 }
681
682 RSPParam LRSL6_param;
683 LRSL(-xb, yb, -phi, &LRSL6_param);
684 double LRSL6_lengths[4] = {-LRSL6_param.v, -LRSL6_param.u, 0.5 * M_PI,
685 -LRSL6_param.t};
686 char LRSL6_types[] = "LSRL";
687 if (LRSL6_param.flag &&
688 !SetRSP(4, LRSL6_lengths, LRSL6_types, all_possible_paths)) {
689 ADEBUG << "Fail at SetRSP with LRSL6_param";
690 return false;
691 }
692
693 RSPParam LRSL7_param;
694 LRSL(xb, -yb, -phi, &LRSL7_param);
695 double LRSL7_lengths[4] = {LRSL7_param.v, LRSL7_param.u, -0.5 * M_PI,
696 LRSL7_param.t};
697 char LRSL7_types[] = "RSLR";
698 if (LRSL7_param.flag &&
699 !SetRSP(4, LRSL7_lengths, LRSL7_types, all_possible_paths)) {
700 ADEBUG << "Fail at SetRSP with LRSL7_param";
701 return false;
702 }
703
704 RSPParam LRSL8_param;
705 LRSL(-xb, -yb, phi, &LRSL8_param);
706 double LRSL8_lengths[4] = {-LRSL8_param.v, -LRSL8_param.u, 0.5 * M_PI,
707 -LRSL8_param.t};
708 char LRSL8_types[] = "RSLR";
709 if (LRSL8_param.flag &&
710 !SetRSP(4, LRSL8_lengths, LRSL8_types, all_possible_paths)) {
711 ADEBUG << "Fail at SetRSP with LRSL8_param";
712 return false;
713 }
714
715 RSPParam LRSR5_param;
716 LRSR(xb, yb, phi, &LRSR5_param);
717 double LRSR5_lengths[4] = {LRSR5_param.v, LRSR5_param.u, -0.5 * M_PI,
718 LRSR5_param.t};
719 char LRSR5_types[] = "RSRL";
720 if (LRSR5_param.flag &&
721 !SetRSP(4, LRSR5_lengths, LRSR5_types, all_possible_paths)) {
722 ADEBUG << "Fail at SetRSP with LRSR5_param";
723 return false;
724 }
725
726 RSPParam LRSR6_param;
727 LRSR(-xb, yb, -phi, &LRSR6_param);
728 double LRSR6_lengths[4] = {-LRSR6_param.v, -LRSR6_param.u, 0.5 * M_PI,
729 -LRSR6_param.t};
730 char LRSR6_types[] = "RSRL";
731 if (LRSR6_param.flag &&
732 !SetRSP(4, LRSR6_lengths, LRSR6_types, all_possible_paths)) {
733 ADEBUG << "Fail at SetRSP with LRSR6_param";
734 return false;
735 }
736
737 RSPParam LRSR7_param;
738 LRSR(xb, -yb, -phi, &LRSR7_param);
739 double LRSR7_lengths[4] = {LRSR7_param.v, LRSR7_param.u, -0.5 * M_PI,
740 LRSR7_param.t};
741 char LRSR7_types[] = "LSLR";
742 if (LRSR7_param.flag &&
743 !SetRSP(4, LRSR7_lengths, LRSR7_types, all_possible_paths)) {
744 ADEBUG << "Fail at SetRSP with LRSR7_param";
745 return false;
746 }
747
748 RSPParam LRSR8_param;
749 LRSR(-xb, -yb, phi, &LRSR8_param);
750 double LRSR8_lengths[4] = {-LRSR8_param.v, -LRSR8_param.u, 0.5 * M_PI,
751 -LRSR8_param.t};
752 char LRSR8_types[] = "LSLR";
753 if (LRSR8_param.flag &&
754 !SetRSP(4, LRSR8_lengths, LRSR8_types, all_possible_paths)) {
755 ADEBUG << "Fail at SetRSP with LRSR8_param";
756 return false;
757 }
758 return true;
759}
apollo::planning::ReedShepp::LRSL
void LRSL(const double x, const double y, const double phi, RSPParam *param)
Definition reeds_shepp_path.cc:990
apollo::planning::ReedShepp::LRSR
void LRSR(const double x, const double y, const double phi, RSPParam *param)
Definition reeds_shepp_path.cc:967

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

762 {
763 RSPParam LRSLR1_param;
764 LRSLR(x, y, phi, &LRSLR1_param);
765 double LRSLR1_lengths[5] = {LRSLR1_param.t, -0.5 * M_PI, LRSLR1_param.u,
766 -0.5 * M_PI, LRSLR1_param.v};
767 char LRSLR1_types[] = "LRSLR";
768 if (LRSLR1_param.flag &&
769 !SetRSP(5, LRSLR1_lengths, LRSLR1_types, all_possible_paths)) {
770 ADEBUG << "Fail at SetRSP with LRSLR1_param";
771 return false;
772 }
773
774 RSPParam LRSLR2_param;
775 LRSLR(-x, y, -phi, &LRSLR2_param);
776 double LRSLR2_lengths[5] = {-LRSLR2_param.t, 0.5 * M_PI, -LRSLR2_param.u,
777 0.5 * M_PI, -LRSLR2_param.v};
778 char LRSLR2_types[] = "LRSLR";
779 if (LRSLR2_param.flag &&
780 !SetRSP(5, LRSLR2_lengths, LRSLR2_types, all_possible_paths)) {
781 ADEBUG << "Fail at SetRSP with LRSLR2_param";
782 return false;
783 }
784
785 RSPParam LRSLR3_param;
786 LRSLR(x, -y, -phi, &LRSLR3_param);
787 double LRSLR3_lengths[5] = {LRSLR3_param.t, -0.5 * M_PI, LRSLR3_param.u,
788 -0.5 * M_PI, LRSLR3_param.v};
789 char LRSLR3_types[] = "RLSRL";
790 if (LRSLR3_param.flag &&
791 !SetRSP(5, LRSLR3_lengths, LRSLR3_types, all_possible_paths)) {
792 ADEBUG << "Fail at SetRSP with LRSLR3_param";
793 return false;
794 }
795
796 RSPParam LRSLR4_param;
797 LRSLR(-x, -y, phi, &LRSLR4_param);
798 double LRSLR4_lengths[5] = {-LRSLR4_param.t, 0.5 * M_PI, -LRSLR4_param.u,
799 0.5 * M_PI, -LRSLR4_param.v};
800 char LRSLR4_types[] = "RLSRL";
801 if (LRSLR4_param.flag &&
802 !SetRSP(5, LRSLR4_lengths, LRSLR4_types, all_possible_paths)) {
803 ADEBUG << "Fail at SetRSP with LRSLR4_param";
804 return false;
805 }
806 return true;
807}
apollo::planning::ReedShepp::LRSLR
void LRSLR(const double x, const double y, const double phi, RSPParam *param)
Definition reeds_shepp_path.cc:1016

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

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

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

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

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

1067 {
1068 double radius = shortest_path->radius;
1069 double step_scaled =
1070 planner_open_space_config_.warm_start_config().step_size() * max_kappa_;
1071 ADEBUG << "shortest_path->total_length" << shortest_path->total_length;
1072 size_t point_num = static_cast<size_t>(
1073 std::floor(shortest_path->total_length / step_scaled * radius +
1074 static_cast<double>(shortest_path->segs_lengths.size()) + 4));
1075 ADEBUG << "point_num" << point_num;
1076 std::vector<double> px(point_num, 0.0);
1077 std::vector<double> py(point_num, 0.0);
1078 std::vector<double> pphi(point_num, 0.0);
1079 std::vector<bool> pgear(point_num, true);
1080 int index = 1;
1081 double d = 0.0;
1082 double pd = 0.0;
1083 double ll = 0.0;
1084
1085 if (shortest_path->segs_lengths.at(0) > 0.0) {
1086 pgear.at(0) = true;
1087 d = step_scaled;
1088 } else {
1089 pgear.at(0) = false;
1090 d = -step_scaled;
1091 }
1092 pd = d;
1093 for (size_t i = 0; i < shortest_path->segs_types.size(); ++i) {
1094 char m = shortest_path->segs_types.at(i);
1095 double l = shortest_path->segs_lengths.at(i);
1096 d = l > 0.0 ? step_scaled : -step_scaled;
1097 d = m == 'S' ? d : d / radius;
1098 double ox = px.at(index);
1099 double oy = py.at(index);
1100 double ophi = pphi.at(index);
1101 index--;
1102 if (i >= 1 && shortest_path->segs_lengths.at(i - 1) *
1103 shortest_path->segs_lengths.at(i) >
1104 0) {
1105 pd = -d - ll;
1106 } else {
1107 pd = d - ll;
1108 }
1109 while (std::abs(pd) <= std::abs(l)) {
1110 index++;
1111 Interpolation(index, pd, m, ox, oy, ophi, radius, &px, &py, &pphi,
1112 &pgear);
1113 pd += d;
1114 }
1115 ll = l - pd - d;
1116 index++;
1117 Interpolation(index, l, m, ox, oy, ophi, radius, &px, &py, &pphi, &pgear);
1118 }
1119 double epsilon = 1e-15;
1120 while (std::fabs(px.back()) < epsilon && std::fabs(py.back()) < epsilon &&
1121 std::fabs(pphi.back()) < epsilon && pgear.back()) {
1122 px.pop_back();
1123 py.pop_back();
1124 pphi.pop_back();
1125 pgear.pop_back();
1126 }
1127 for (size_t i = 0; i < px.size(); ++i) {
1128 shortest_path->x.push_back(std::cos(-start_node->GetPhi()) * px.at(i) +
1129 std::sin(-start_node->GetPhi()) * py.at(i) +
1130 start_node->GetX());
1131 shortest_path->y.push_back(-std::sin(-start_node->GetPhi()) * px.at(i) +
1132 std::cos(-start_node->GetPhi()) * py.at(i) +
1133 start_node->GetY());
1134 shortest_path->phi.push_back(
1135 common::math::NormalizeAngle(pphi.at(i) + start_node->GetPhi()));
1136 }
1137 shortest_path->gear = pgear;
1138 for (size_t i = 0; i < shortest_path->segs_lengths.size(); ++i) {
1139 shortest_path->segs_lengths.at(i) =
1140 shortest_path->segs_lengths.at(i) / max_kappa_;
1141 }
1142 shortest_path->total_length = shortest_path->total_length / max_kappa_;
1143 return true;
1144}
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:1146

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

210 {
211 double dx = end_node->GetX() - start_node->GetX();
212 double dy = end_node->GetY() - start_node->GetY();
213 double dphi = end_node->GetPhi() - start_node->GetPhi();
214 double c = std::cos(start_node->GetPhi());
215 double s = std::sin(start_node->GetPhi());
216 // normalize the initial point to (0,0,0)
217 double x = (c * dx + s * dy) * max_kappa_;
218 double y = (-s * dx + c * dy) * max_kappa_;
219 // if (!CS(x, y, dphi, all_possible_paths)) {
220 // ADEBUG << "Fail at CS";
221 // }
222 if (!SCS(x, y, dphi, all_possible_paths)) {
223 ADEBUG << "Fail at SCS";
224 }
225 if (!CSC(x, y, dphi, all_possible_paths)) {
226 ADEBUG << "Fail at CSC";
227 }
228 if (!CCC(x, y, dphi, all_possible_paths)) {
229 ADEBUG << "Fail at CCC";
230 }
231 if (!CCCC(x, y, dphi, all_possible_paths)) {
232 ADEBUG << "Fail at CCCC";
233 }
234 if (!CCSC(x, y, dphi, all_possible_paths)) {
235 ADEBUG << "Fail at CCSC";
236 }
237 if (!CCSCC(x, y, dphi, all_possible_paths)) {
238 ADEBUG << "Fail at CCSCC";
239 }
240 if (all_possible_paths->empty()) {
241 ADEBUG << "No path generated by certain two configurations";
242 return false;
243 }
244 return true;
245}
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:485
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:397
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:761
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:577
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:313
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:247

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

1211 {
1212 double dx = end_node->GetX() - start_node->GetX();
1213 double dy = end_node->GetY() - start_node->GetY();
1214 double dphi = end_node->GetPhi() - start_node->GetPhi();
1215 double c = std::cos(start_node->GetPhi());
1216 double s = std::sin(start_node->GetPhi());
1217 // normalize the initial point to (0,0,0)
1218 double x = (c * dx + s * dy) * this->max_kappa_;
1219 double y = (-s * dx + c * dy) * this->max_kappa_;
1220 // backward
1221 double xb = x * std::cos(dphi) + y * std::sin(dphi);
1222 double yb = x * std::sin(dphi) - y * std::cos(dphi);
1223
1224 int RSP_nums = 46;
1225 all_possible_paths->resize(RSP_nums);
1226 bool succ = true;
1227#pragma omp parallel for schedule(dynamic, 2) num_threads(8)
1228 for (int i = 0; i < RSP_nums; ++i) {
1229 RSPParam RSP_param;
1230 int tmp_length = 0;
1231 double RSP_lengths[5] = {0.0, 0.0, 0.0, 0.0, 0.0};
1232 double x_param = 1.0;
1233 double y_param = 1.0;
1234 std::string rd_type;
1235
1236 if (i > 2 && i % 2) {
1237 x_param = -1.0;
1238 }
1239 if (i > 2 && i % 4 < 2) {
1240 y_param = -1.0;
1241 }
1242
1243 if (i < 2) { // SCS case
1244 if (i == 1) {
1245 y_param = -1.0;
1246 rd_type = "SRS";
1247 } else {
1248 rd_type = "SLS";
1249 }
1250 SLS(x, y_param * y, y_param * dphi, &RSP_param);
1251 tmp_length = 3;
1252 RSP_lengths[0] = RSP_param.t;
1253 RSP_lengths[1] = RSP_param.u;
1254 RSP_lengths[2] = RSP_param.v;
1255 } else if (i < 6) { // CSC, LSL case
1256 LSL(x_param * x, y_param * y, x_param * y_param * dphi, &RSP_param);
1257 if (y_param > 0) {
1258 rd_type = "LSL";
1259 } else {
1260 rd_type = "RSR";
1261 }
1262 tmp_length = 3;
1263 RSP_lengths[0] = x_param * RSP_param.t;
1264 RSP_lengths[1] = x_param * RSP_param.u;
1265 RSP_lengths[2] = x_param * RSP_param.v;
1266 } else if (i < 10) { // CSC, LSR case
1267 LSR(x_param * x, y_param * y, x_param * y_param * dphi, &RSP_param);
1268 if (y_param > 0) {
1269 rd_type = "LSR";
1270 } else {
1271 rd_type = "RSL";
1272 }
1273 tmp_length = 3;
1274 RSP_lengths[0] = x_param * RSP_param.t;
1275 RSP_lengths[1] = x_param * RSP_param.u;
1276 RSP_lengths[2] = x_param * RSP_param.v;
1277 } else if (i < 14) { // CCC, LRL case
1278 LRL(x_param * x, y_param * y, x_param * y_param * dphi, &RSP_param);
1279 if (y_param > 0) {
1280 rd_type = "LRL";
1281 } else {
1282 rd_type = "RLR";
1283 }
1284 tmp_length = 3;
1285 RSP_lengths[0] = x_param * RSP_param.t;
1286 RSP_lengths[1] = x_param * RSP_param.u;
1287 RSP_lengths[2] = x_param * RSP_param.v;
1288 } else if (i < 18) { // CCC, LRL case, backward
1289 LRL(x_param * xb, y_param * yb, x_param * y_param * dphi, &RSP_param);
1290 if (y_param > 0) {
1291 rd_type = "LRL";
1292 } else {
1293 rd_type = "RLR";
1294 }
1295 tmp_length = 3;
1296 RSP_lengths[0] = x_param * RSP_param.v;
1297 RSP_lengths[1] = x_param * RSP_param.u;
1298 RSP_lengths[2] = x_param * RSP_param.t;
1299 } else if (i < 22) { // CCCC, LRLRn
1300 LRLRn(x_param * x, y_param * y, x_param * y_param * dphi, &RSP_param);
1301 if (y_param > 0.0) {
1302 rd_type = "LRLR";
1303 } else {
1304 rd_type = "RLRL";
1305 }
1306 tmp_length = 4;
1307 RSP_lengths[0] = x_param * RSP_param.t;
1308 RSP_lengths[1] = x_param * RSP_param.u;
1309 RSP_lengths[2] = -x_param * RSP_param.u;
1310 RSP_lengths[3] = x_param * RSP_param.v;
1311 } else if (i < 26) { // CCCC, LRLRp
1312 LRLRp(x_param * x, y_param * y, x_param * y_param * dphi, &RSP_param);
1313 if (y_param > 0.0) {
1314 rd_type = "LRLR";
1315 } else {
1316 rd_type = "RLRL";
1317 }
1318 tmp_length = 4;
1319 RSP_lengths[0] = x_param * RSP_param.t;
1320 RSP_lengths[1] = x_param * RSP_param.u;
1321 RSP_lengths[2] = -x_param * RSP_param.u;
1322 RSP_lengths[3] = x_param * RSP_param.v;
1323 } else if (i < 30) { // CCSC, LRLRn
1324 tmp_length = 4;
1325 LRLRn(x_param * x, y_param * y, x_param * y_param * dphi, &RSP_param);
1326 if (y_param > 0.0) {
1327 rd_type = "LRSL";
1328 } else {
1329 rd_type = "RLSR";
1330 }
1331 RSP_lengths[0] = x_param * RSP_param.t;
1332 if (x_param < 0 && y_param > 0) {
1333 RSP_lengths[1] = 0.5 * M_PI;
1334 } else {
1335 RSP_lengths[1] = -0.5 * M_PI;
1336 }
1337 if (x_param > 0 && y_param < 0) {
1338 RSP_lengths[2] = RSP_param.u;
1339 } else {
1340 RSP_lengths[2] = -RSP_param.u;
1341 }
1342 RSP_lengths[3] = x_param * RSP_param.v;
1343 } else if (i < 34) { // CCSC, LRLRp
1344 tmp_length = 4;
1345 LRLRp(x_param * x, y_param * y, x_param * y_param * dphi, &RSP_param);
1346 if (y_param) {
1347 rd_type = "LRSR";
1348 } else {
1349 rd_type = "RLSL";
1350 }
1351 RSP_lengths[0] = x_param * RSP_param.t;
1352 if (x_param < 0 && y_param > 0) {
1353 RSP_lengths[1] = 0.5 * M_PI;
1354 } else {
1355 RSP_lengths[1] = -0.5 * M_PI;
1356 }
1357 RSP_lengths[2] = x_param * RSP_param.u;
1358 RSP_lengths[3] = x_param * RSP_param.v;
1359 } else if (i < 38) { // CCSC, LRLRn, backward
1360 tmp_length = 4;
1361 LRLRn(x_param * xb, y_param * yb, x_param * y_param * dphi, &RSP_param);
1362 if (y_param > 0) {
1363 rd_type = "LSRL";
1364 } else {
1365 rd_type = "RSLR";
1366 }
1367 RSP_lengths[0] = x_param * RSP_param.v;
1368 RSP_lengths[1] = x_param * RSP_param.u;
1369 RSP_lengths[2] = -x_param * 0.5 * M_PI;
1370 RSP_lengths[3] = x_param * RSP_param.t;
1371 } else if (i < 42) { // CCSC, LRLRp, backward
1372 tmp_length = 4;
1373 LRLRp(x_param * xb, y_param * yb, x_param * y_param * dphi, &RSP_param);
1374 if (y_param > 0) {
1375 rd_type = "RSRL";
1376 } else {
1377 rd_type = "LSLR";
1378 }
1379 RSP_lengths[0] = x_param * RSP_param.v;
1380 RSP_lengths[1] = x_param * RSP_param.u;
1381 RSP_lengths[2] = -x_param * M_PI * 0.5;
1382 RSP_lengths[3] = x_param * RSP_param.t;
1383 } else { // CCSCC, LRSLR
1384 tmp_length = 5;
1385 LRSLR(x_param * x, y_param * y, x_param * y_param * dphi, &RSP_param);
1386 if (y_param > 0.0) {
1387 rd_type = "LRSLR";
1388 } else {
1389 rd_type = "RLSRL";
1390 }
1391 RSP_lengths[0] = x_param * RSP_param.t;
1392 RSP_lengths[1] = -x_param * 0.5 * M_PI;
1393 RSP_lengths[2] = x_param * RSP_param.u;
1394 RSP_lengths[3] = -x_param * 0.5 * M_PI;
1395 RSP_lengths[4] = x_param * RSP_param.v;
1396 }
1397
1398 if (tmp_length > 0) {
1399 if (RSP_param.flag &&
1400 !SetRSPPar(tmp_length, RSP_lengths, rd_type, all_possible_paths, i)) {
1401 AERROR << "Fail at SetRSP, idx#: " << i;
1402 succ = false;
1403 }
1404 }
1405 }
1406
1407 if (!succ) {
1408 AERROR << "RSP parallel fails";
1409 return false;
1410 }
1411 if (all_possible_paths->size() == 0) {
1412 AERROR << "No path generated by certain two configurations";
1413 return false;
1414 }
1415 return true;
1416}
apollo::planning::ReedShepp::SLS
void SLS(const double x, const double y, const double phi, RSPParam *param)
Definition reeds_shepp_path.cc:898
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:1186
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 第 188 行定义.

191 {
192 if (FLAGS_enable_parallel_hybrid_a) {
193 // AINFO << "parallel hybrid a*";
194 if (!GenerateRSPPar(start_node, end_node, all_possible_paths)) {
195 ADEBUG << "Fail to generate general profile of different RSPs";
196 return false;
197 }
198 } else {
199 if (!GenerateRSP(start_node, end_node, all_possible_paths)) {
200 AERROR << "Fail to generate general profile of different RSPs";
201 return false;
202 }
203 }
204 return true;
205}
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:1209
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:207

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

1151 {
1152 double ldx = 0.0;
1153 double ldy = 0.0;
1154 double gdx = 0.0;
1155 double gdy = 0.0;
1156 if (m == 'S') {
1157 px->at(index) = ox + pd / max_kappa_ * std::cos(ophi);
1158 py->at(index) = oy + pd / max_kappa_ * std::sin(ophi);
1159 pphi->at(index) = ophi;
1160 } else {
1161 ldx = std::sin(pd) / max_kappa_ * radius;
1162 if (m == 'L') {
1163 ldy = (1.0 - std::cos(pd)) / max_kappa_ * radius;
1164 } else if (m == 'R') {
1165 ldy = (1.0 - std::cos(pd)) / -max_kappa_ * radius;
1166 }
1167 gdx = std::cos(-ophi) * ldx + std::sin(-ophi) * ldy;
1168 gdy = -std::sin(-ophi) * ldx + std::cos(-ophi) * ldy;
1169 px->at(index) = ox + gdx;
1170 py->at(index) = oy + gdy;
1171 }
1172
1173 if (pd > 0.0) {
1174 pgear->at(index) = true;
1175 } else {
1176 pgear->at(index) = false;
1177 }
1178
1179 if (m == 'L') {
1180 pphi->at(index) = ophi + pd;
1181 } else if (m == 'R') {
1182 pphi->at(index) = ophi - pd;
1183 }
1184}

◆ LRL()

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

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

852 {
853 std::pair<double, double> polar =
854 common::math::Cartesian2Polar(x - std::sin(phi), y - 1.0 + std::cos(phi));
855 double u1 = polar.first;
856 double t1 = polar.second;
857 double u = 0.0;
858 double t = 0.0;
859 double v = 0.0;
860 if (u1 <= 4.0) {
861 u = -2.0 * std::asin(0.25 * u1);
862 t = common::math::NormalizeAngle(t1 + 0.5 * u + M_PI);
863 v = common::math::NormalizeAngle(phi - t + u);
864 if (t >= 0.0 && u <= 0.0) {
865 param->flag = true;
866 param->u = u;
867 param->t = t;
868 param->v = v;
869 }
870 }
871}
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 第 927 行定义.

928 {
929 double xi = x + std::sin(phi);
930 double eta = y - 1.0 - std::cos(phi);
931 double rho = 0.25 * (2.0 + std::sqrt(xi * xi + eta * eta));
932 double u = 0.0;
933 if (rho <= 1.0 && rho >= 0.0) {
934 u = std::acos(rho);
935 if (u >= 0 && u <= 0.5 * M_PI) {
936 std::pair<double, double> tau_omega = calc_tau_omega(u, -u, xi, eta, phi);
937 if (tau_omega.first >= 0.0 && tau_omega.second <= 0.0) {
938 param->flag = true;
939 param->u = u;
940 param->t = tau_omega.first;
941 param->v = tau_omega.second;
942 }
943 }
944 }
945}
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:55

◆ LRLRp()

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

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

948 {
949 double xi = x + std::sin(phi);
950 double eta = y - 1.0 - std::cos(phi);
951 double rho = (20.0 - xi * xi - eta * eta) / 16.0;
952 double u = 0.0;
953 if (rho <= 1.0 && rho >= 0.0) {
954 u = -std::acos(rho);
955 if (u >= 0 && u <= 0.5 * M_PI) {
956 std::pair<double, double> tau_omega = calc_tau_omega(u, u, xi, eta, phi);
957 if (tau_omega.first >= 0.0 && tau_omega.second >= 0.0) {
958 param->flag = true;
959 param->u = u;
960 param->t = tau_omega.first;
961 param->v = tau_omega.second;
962 }
963 }
964 }
965}

◆ LRSL()

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

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

991 {
992 double xi = x - std::sin(phi);
993 double eta = y - 1.0 + std::cos(phi);
994 std::pair<double, double> polar = common::math::Cartesian2Polar(xi, eta);
995 double rho = polar.first;
996 double theta = polar.second;
997 double r = 0.0;
998 double t = 0.0;
999 double u = 0.0;
1000 double v = 0.0;
1001
1002 if (rho >= 2.0) {
1003 r = std::sqrt(rho * rho - 4.0);
1004 u = 2.0 - r;
1005 t = common::math::NormalizeAngle(theta + std::atan2(r, -2.0));
1006 v = common::math::NormalizeAngle(phi - 0.5 * M_PI - t);
1007 if (t >= 0.0 && u <= 0.0 && v <= 0.0) {
1008 param->flag = true;
1009 param->u = u;
1010 param->t = t;
1011 param->v = v;
1012 }
1013 }
1014}

◆ LRSLR()

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

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

1017 {
1018 double xi = x + std::sin(phi);
1019 double eta = y - 1.0 - std::cos(phi);
1020 std::pair<double, double> polar = common::math::Cartesian2Polar(xi, eta);
1021 double rho = polar.first;
1022 double t = 0.0;
1023 double u = 0.0;
1024 double v = 0.0;
1025 if (rho >= 2.0) {
1026 u = 4.0 - std::sqrt(rho * rho - 4.0);
1027 if (u <= 0.0) {
1028 t = common::math::NormalizeAngle(
1029 atan2((4.0 - u) * xi - 2.0 * eta, -2.0 * xi + (u - 4.0) * eta));
1030 v = common::math::NormalizeAngle(t - phi);
1031
1032 if (t >= 0.0 && v >= 0.0) {
1033 param->flag = true;
1034 param->u = u;
1035 param->t = t;
1036 param->v = v;
1037 }
1038 }
1039 }
1040}

◆ LRSR()

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

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

968 {
969 double xi = x + std::sin(phi);
970 double eta = y - 1.0 - std::cos(phi);
971 std::pair<double, double> polar = common::math::Cartesian2Polar(-eta, xi);
972 double rho = polar.first;
973 double theta = polar.second;
974 double t = 0.0;
975 double u = 0.0;
976 double v = 0.0;
977 if (rho >= 2.0) {
978 t = theta;
979 u = 2.0 - rho;
980 v = common::math::NormalizeAngle(t + 0.5 * M_PI - phi);
981 if (t >= 0.0 && u <= 0.0 && v <= 0.0) {
982 param->flag = true;
983 param->u = u;
984 param->t = t;
985 param->v = v;
986 }
987 }
988}

◆ LS()

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

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

874 {
875 double phi_mod = common::math::NormalizeAngle(phi);
876 double epsilon = 1e-6;
877 if (std::fabs(phi_mod - M_PI_2) < epsilon || std::fabs(phi_mod) < epsilon ||
878 std::fabs(phi_mod + M_PI_2) < epsilon ||
879 std::fabs(phi_mod + M_PI) < epsilon) {
880 return;
881 }
882 double r = (y - x * std::tan(phi_mod)) / (1 - 1 / std::cos(phi_mod));
883 if (r < 1.0) {
884 return;
885 }
886 // u>0 :lpsp ,u<0 :lpsn
887 double u = x / std::cos(phi_mod) - r * std::tan(phi_mod);
888 double t = phi_mod;
889 if (phi_mod < 0) {
890 t = phi_mod + 2 * M_PI;
891 }
892 param->flag = true;
893 param->u = u;
894 param->t = t;
895 param->v = r;
896}

◆ LSL()

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

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

810 {
811 std::pair<double, double> polar =
812 common::math::Cartesian2Polar(x - std::sin(phi), y - 1.0 + std::cos(phi));
813 double u = polar.first;
814 double t = polar.second;
815 double v = 0.0;
816 if (t >= 0.0) {
817 v = common::math::NormalizeAngle(phi - t);
818 if (v >= 0.0) {
819 param->flag = true;
820 param->u = u;
821 param->t = t;
822 param->v = v;
823 }
824 }
825}

◆ LSR()

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

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

828 {
829 std::pair<double, double> polar =
830 common::math::Cartesian2Polar(x + std::sin(phi), y - 1.0 - std::cos(phi));
831 double u1 = polar.first * polar.first;
832 double t1 = polar.second;
833 double u = 0.0;
834 double theta = 0.0;
835 double t = 0.0;
836 double v = 0.0;
837 if (u1 >= 4.0) {
838 u = std::sqrt(u1 - 4.0);
839 theta = std::atan2(2.0, u);
840 t = common::math::NormalizeAngle(t1 + theta);
841 v = common::math::NormalizeAngle(t - phi);
842 if (t >= 0.0 && v >= 0.0) {
843 param->flag = true;
844 param->u = u;
845 param->t = t;
846 param->v = v;
847 }
848 }
849}

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

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

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

1044 {
1045 ReedSheppPath path;
1046 std::vector<double> length_vec(lengths, lengths + size);
1047 std::vector<char> type_vec(types, types + size);
1048 path.segs_lengths = length_vec;
1049 path.segs_types = type_vec;
1050 path.radius = radius;
1051 double sum = 0.0;
1052 for (int i = 0; i < size; ++i) {
1053 sum += std::abs(lengths[i]);
1054 }
1055 path.total_length = sum;
1056 if (path.total_length <= 0.0) {
1057 AERROR << "total length smaller than 0";
1058 return false;
1059 }
1060 all_possible_paths->emplace_back(path);
1061 return true;
1062}

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

1189 {
1190 ReedSheppPath path;
1191 std::vector<double> length_vec(lengths, lengths + size);
1192 std::vector<char> type_vec(types.begin(), types.begin() + size);
1193 path.segs_lengths = length_vec;
1194 path.segs_types = type_vec;
1195 double sum = 0.0;
1196 for (int i = 0; i < size; ++i) {
1197 sum += std::abs(lengths[i]);
1198 }
1199 path.total_length = sum;
1200 if (path.total_length <= 0.0) {
1201 AERROR << "total length smaller than 0";
1202 return false;
1203 }
1204
1205 all_possible_paths->at(idx) = path;
1206 return true;
1207}

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

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

◆ SLS()

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

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

899 {
900 double phi_mod = common::math::NormalizeAngle(phi);
901 double xd = 0.0;
902 double u = 0.0;
903 double t = 0.0;
904 double v = 0.0;
905 double epsilon = 1e-1;
906 if (y > 0.0 && phi_mod > epsilon && phi_mod < M_PI) {
907 xd = -y / std::tan(phi_mod) + x;
908 t = xd - std::tan(phi_mod / 2.0);
909 u = phi_mod;
910 v = std::sqrt((x - xd) * (x - xd) + y * y) - tan(phi_mod / 2.0);
911 param->flag = true;
912 param->u = u;
913 param->t = t;
914 param->v = v;
915 } else if (y < 0.0 && phi_mod > epsilon && phi_mod < M_PI) {
916 xd = -y / std::tan(phi_mod) + x;
917 t = xd - std::tan(phi_mod / 2.0);
918 u = phi_mod;
919 v = -std::sqrt((x - xd) * (x - xd) + y * y) - std::tan(phi_mod / 2.0);
920 param->flag = true;
921 param->u = u;
922 param->t = t;
923 param->v = v;
924 }
925}
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 第 163 行定义.

◆ planner_open_space_config_

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

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

◆ planner_warm_start_config_

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

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

◆ reeds_sheep_last_straight_

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

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

◆ traj_back_penalty_

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

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

◆ traj_expected_shortest_length_

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

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

◆ traj_forward_penalty_

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

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

◆ traj_gear_switch_penalty_

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

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

◆ traj_short_length_penalty_

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

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

◆ traj_steer_change_penalty_

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

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

◆ traj_steer_penalty_

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

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

◆ vehicle_param_

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

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

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