PassiveDecisionMaker.cpp 5.0 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153
  1. /// \file PassiveDecisionMaker.cpp
  2. /// \brief Decision Maker for surrounding vehicle, to be integrated with particle filter for intention prediction
  3. /// \author Hatem Darweesh
  4. /// \date Jan 10, 2018
  5. #include "op_planner/PassiveDecisionMaker.h"
  6. #include "op_utility/UtilityH.h"
  7. #include "op_planner/PlanningHelpers.h"
  8. #include "op_planner/MappingHelpers.h"
  9. #include "op_planner/MatrixOperations.h"
  10. namespace PlannerHNS
  11. {
  12. PassiveDecisionMaker::PassiveDecisionMaker()
  13. {
  14. }
  15. PassiveDecisionMaker& PassiveDecisionMaker::operator=(const PassiveDecisionMaker& obj)
  16. {
  17. return *this;
  18. }
  19. PassiveDecisionMaker::PassiveDecisionMaker(const PassiveDecisionMaker& obj)
  20. {
  21. }
  22. PassiveDecisionMaker::~PassiveDecisionMaker()
  23. {
  24. }
  25. double PassiveDecisionMaker::GetVelocity(PlannerHNS::WayPoint& currPose, const std::vector<WayPoint>& path, const CAR_BASIC_INFO& carInfo, const RelativeInfo& info)
  26. {
  27. double average_braking_distance = -pow(currPose.v, 2)/(carInfo.max_deceleration) + 1.0;
  28. int prev_index = 0;
  29. double velocity = PlannerHNS::PlanningHelpers::GetVelocityAhead(path, info, prev_index, average_braking_distance);
  30. if(velocity > carInfo.max_speed_forward)
  31. velocity = carInfo.max_speed_forward;
  32. else if(velocity < carInfo.min_speed_forward)
  33. velocity = carInfo.min_speed_forward;
  34. return velocity;
  35. }
  36. double PassiveDecisionMaker::GetSteerAngle(PlannerHNS::WayPoint& currPose, const std::vector<WayPoint>& path, const RelativeInfo& info)
  37. {
  38. unsigned int point_index = 0;
  39. PlannerHNS::WayPoint pursuite_point = PlanningHelpers::GetFollowPointOnTrajectory(path, info, 2, point_index);
  40. double current_a = UtilityHNS::UtilityH::SplitPositiveAngle(currPose.pos.a);
  41. double target_a = atan2(pursuite_point.pos.y - currPose.pos.y, pursuite_point.pos.x - currPose.pos.x);
  42. double e = UtilityHNS::UtilityH::SplitPositiveAngle(target_a - current_a);
  43. double before_lowpass = e;//m_pidSteer.getPID(e);
  44. //std::cout << "CurrA: " << current_a << ", targetA: " << target_a << ", e: " << e << std::endl;
  45. return before_lowpass;
  46. }
  47. bool PassiveDecisionMaker::CheckForStopLine(PlannerHNS::WayPoint& currPose, const std::vector<WayPoint>& path, const CAR_BASIC_INFO& carInfo)
  48. {
  49. double minStoppingDistance = -pow(currPose.v, 2)/(carInfo.max_deceleration);
  50. double critical_long_front_distance = carInfo.wheel_base/2.0 + carInfo.length/2.0;
  51. int stopLineID = -1;
  52. int stopSignID = -1;
  53. int trafficLightID = -1;
  54. double distanceToClosestStopLine = 0;
  55. distanceToClosestStopLine = PlanningHelpers::GetDistanceToClosestStopLineAndCheck(path, currPose, 0, stopLineID, stopSignID, trafficLightID) - critical_long_front_distance;
  56. if(distanceToClosestStopLine > -2 && distanceToClosestStopLine < minStoppingDistance)
  57. {
  58. return true;
  59. }
  60. return false;
  61. }
  62. PlannerHNS::BehaviorState PassiveDecisionMaker::MoveStep(const double& dt, PlannerHNS::WayPoint& currPose, const std::vector<WayPoint>& path, const CAR_BASIC_INFO& carInfo)
  63. {
  64. PlannerHNS::BehaviorState beh;
  65. if(path.size() == 0) return beh;
  66. RelativeInfo info;
  67. PlanningHelpers::GetRelativeInfo(path, currPose, info);
  68. bool bStopLine = CheckForStopLine(currPose, path, carInfo);
  69. if(bStopLine)
  70. beh.state = PlannerHNS::STOPPING_STATE;
  71. else
  72. beh.state = PlannerHNS::FORWARD_STATE;
  73. double average_braking_distance = -pow(currPose.v, 2)/(carInfo.max_deceleration) + 1.0;
  74. if(average_braking_distance < 10)
  75. average_braking_distance = 10;
  76. beh.indicator = PlanningHelpers::GetIndicatorsFromPath(path, currPose, average_braking_distance);
  77. currPose.v = beh.maxVelocity = GetVelocity(currPose, path, carInfo, info);
  78. double steer = GetSteerAngle(currPose, path, info);
  79. currPose.pos.x += currPose.v * dt * cos(currPose.pos.a);
  80. currPose.pos.y += currPose.v * dt * sin(currPose.pos.a);
  81. currPose.pos.a += currPose.v * dt * tan(steer) / carInfo.wheel_base;
  82. return beh;
  83. }
  84. PlannerHNS::ParticleInfo PassiveDecisionMaker::MoveStepSimple(const double& dt, PlannerHNS::WayPoint& currPose, const std::vector<WayPoint>& path, const CAR_BASIC_INFO& carInfo)
  85. {
  86. PlannerHNS::ParticleInfo beh;
  87. if(path.size() == 0) return beh;
  88. RelativeInfo info;
  89. PlanningHelpers::GetRelativeInfo(path, currPose, info);
  90. bool bStopLine = CheckForStopLine(currPose, path, carInfo);
  91. if(bStopLine)
  92. beh.state = PlannerHNS::STOPPING_STATE;
  93. else
  94. beh.state = PlannerHNS::FORWARD_STATE;
  95. double average_braking_distance = -pow(currPose.v, 2)/(carInfo.max_deceleration) + 15.0;
  96. PlannerHNS::WayPoint startPose = path.at(0);
  97. beh.indicator = PlanningHelpers::GetIndicatorsFromPath(path, startPose, average_braking_distance);
  98. double speed = 0;
  99. if(info.iFront < path.size())
  100. {
  101. beh.vel = path.at(info.iFront).v;
  102. }
  103. else
  104. beh.vel = 0;
  105. double steer = GetSteerAngle(currPose, path, info);
  106. currPose.pos.x += currPose.v * dt * cos(currPose.pos.a);
  107. currPose.pos.y += currPose.v * dt * sin(currPose.pos.a);
  108. currPose.pos.a += currPose.v * dt * tan(steer) / carInfo.wheel_base;
  109. return beh;
  110. }
  111. } /* namespace PlannerHNS */