LocalPlannerH.cpp 30 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797
  1. /// \file LocalPlannerH.cpp
  2. /// \brief OpenPlanner's local planing functions combines in one process, used in simulation vehicle and OpenPlanner old implementation like dp_planner node.
  3. /// \author Hatem Darweesh
  4. /// \date Dec 14, 2016
  5. #include "op_planner/LocalPlannerH.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. #include "op_planner/PlannerH.h"
  11. using namespace UtilityHNS;
  12. namespace PlannerHNS
  13. {
  14. LocalPlannerH::LocalPlannerH()
  15. {
  16. m_PrevBrakingWayPoint = 0;
  17. m_iSafeTrajectory = 0;
  18. m_iCurrentTotalPathId = 0;
  19. pLane = 0;
  20. m_CurrentVelocity = m_CurrentVelocityD =0;
  21. m_CurrentSteering = m_CurrentSteeringD =0;
  22. m_CurrentShift = m_CurrentShiftD = SHIFT_POS_NN;
  23. m_CurrentAccSteerAngle = m_CurrentAccVelocity = 0;
  24. m_pCurrentBehaviorState = 0;
  25. m_pGoToGoalState = 0;
  26. m_pStopState= 0;
  27. m_pWaitState= 0;
  28. m_pMissionCompleteState= 0;
  29. m_pAvoidObstacleState = 0;
  30. m_pTrafficLightStopState = 0;
  31. m_pTrafficLightWaitState = 0;
  32. m_pStopSignStopState = 0;
  33. m_pStopSignWaitState = 0;
  34. m_pFollowState = 0;
  35. m_SimulationSteeringDelayFactor = 0.1;
  36. UtilityH::GetTickCount(m_SteerDelayTimer);
  37. m_PredictionTime = 0;
  38. InitBehaviorStates();
  39. }
  40. LocalPlannerH::~LocalPlannerH()
  41. {
  42. delete m_pStopState;
  43. delete m_pMissionCompleteState ;
  44. delete m_pGoalState ;
  45. delete m_pGoToGoalState ;
  46. delete m_pWaitState ;
  47. delete m_pInitState ;
  48. delete m_pFollowState ;
  49. delete m_pAvoidObstacleState ;
  50. delete m_pTrafficLightStopState;
  51. delete m_pTrafficLightWaitState;
  52. delete m_pStopSignWaitState ;
  53. delete m_pStopSignStopState;
  54. }
  55. void LocalPlannerH::Init(const ControllerParams& ctrlParams, const PlannerHNS::PlanningParams& params,const CAR_BASIC_INFO& carInfo)
  56. {
  57. m_CarInfo = carInfo;
  58. m_ControlParams = ctrlParams;
  59. m_CurrentVelocity = m_CurrentVelocityD =0;
  60. m_CurrentSteering = m_CurrentSteeringD =0;
  61. m_CurrentShift = m_CurrentShiftD = SHIFT_POS_NN;
  62. m_CurrentAccSteerAngle = m_CurrentAccVelocity = 0;
  63. m_params = params;
  64. m_InitialFollowingDistance = m_params.minFollowingDistance;
  65. m_pidVelocity.Init(0.005, 0.005, 0.05);
  66. m_pidVelocity.Setlimit(m_params.maxSpeed, 0);
  67. m_pidStopping.Init(0.1, 0.05, 0.1);
  68. m_pidStopping.Setlimit(m_params.horizonDistance, 0);
  69. if(m_pCurrentBehaviorState)
  70. m_pCurrentBehaviorState->SetBehaviorsParams(&m_params);
  71. }
  72. void LocalPlannerH::InitBehaviorStates()
  73. {
  74. m_pStopState = new StopState(&m_params, 0, 0);
  75. m_pMissionCompleteState = new MissionAccomplishedState(m_pStopState->m_pParams, m_pStopState->GetCalcParams(), 0);
  76. m_pGoalState = new GoalState(m_pStopState->m_pParams, m_pStopState->GetCalcParams(), m_pMissionCompleteState);
  77. m_pGoToGoalState = new ForwardState(m_pStopState->m_pParams, m_pStopState->GetCalcParams(), m_pGoalState);
  78. m_pWaitState = new WaitState(m_pStopState->m_pParams, m_pStopState->GetCalcParams(), m_pGoToGoalState);
  79. m_pInitState = new InitState(m_pStopState->m_pParams, m_pStopState->GetCalcParams(), m_pGoToGoalState);
  80. m_pFollowState = new FollowState(m_pStopState->m_pParams, m_pStopState->GetCalcParams(), m_pGoToGoalState);
  81. m_pAvoidObstacleState = new SwerveState(m_pStopState->m_pParams, m_pStopState->GetCalcParams(), m_pGoToGoalState);
  82. m_pTrafficLightStopState = new TrafficLightStopState(m_pStopState->m_pParams, m_pStopState->GetCalcParams(), m_pGoToGoalState);
  83. m_pTrafficLightWaitState = new TrafficLightWaitState(m_pStopState->m_pParams, m_pStopState->GetCalcParams(), m_pGoToGoalState);
  84. m_pStopSignWaitState = new StopSignWaitState(m_pStopState->m_pParams, m_pStopState->GetCalcParams(), m_pGoToGoalState);
  85. m_pStopSignStopState = new StopSignStopState(m_pStopState->m_pParams, m_pStopState->GetCalcParams(), m_pStopSignWaitState);
  86. m_pGoToGoalState->InsertNextState(m_pStopState);
  87. m_pGoToGoalState->InsertNextState(m_pWaitState);
  88. m_pGoToGoalState->InsertNextState(m_pFollowState);
  89. m_pGoToGoalState->InsertNextState(m_pAvoidObstacleState);
  90. m_pGoToGoalState->InsertNextState(m_pTrafficLightStopState);
  91. m_pGoToGoalState->InsertNextState(m_pStopSignStopState);
  92. m_pStopState->InsertNextState(m_pGoToGoalState);
  93. m_pTrafficLightStopState->InsertNextState(m_pTrafficLightWaitState);
  94. m_pTrafficLightWaitState->InsertNextState(m_pTrafficLightStopState);
  95. m_pStopSignWaitState->decisionMakingTime = 5.0;
  96. m_pStopSignWaitState->InsertNextState(m_pStopSignStopState);
  97. m_pFollowState->InsertNextState(m_pStopState);
  98. m_pFollowState->InsertNextState(m_pTrafficLightStopState);
  99. m_pFollowState->InsertNextState(m_pStopSignStopState);
  100. m_pAvoidObstacleState->decisionMakingTime = 0.1;
  101. m_pCurrentBehaviorState = m_pInitState;
  102. }
  103. void LocalPlannerH::InitPolygons()
  104. {
  105. double l2 = m_CarInfo.length/2.0;
  106. double w2 = m_CarInfo.width/2.0;
  107. m_CarShapePolygon.push_back(GPSPoint(-w2, -l2, 0,0));
  108. m_CarShapePolygon.push_back(GPSPoint(w2, -l2, 0,0));
  109. m_CarShapePolygon.push_back(GPSPoint(w2, l2, 0,0));
  110. m_CarShapePolygon.push_back(GPSPoint(-w2, l2, 0,0));
  111. }
  112. void LocalPlannerH::ReInitializePlanner(const WayPoint& start_pose)
  113. {
  114. m_pidVelocity.Init(0.005, 0.005, 0.05);
  115. m_pidVelocity.Setlimit(m_params.maxSpeed, 0);
  116. m_pidStopping.Init(0.1, 0.05, 0.1);
  117. m_pidStopping.Setlimit(m_params.horizonDistance, 0);
  118. m_PrevBrakingWayPoint = 0;
  119. m_iSafeTrajectory = 0;
  120. m_iCurrentTotalPathId = 0;
  121. m_CurrentVelocity = m_CurrentVelocityD =0;
  122. m_CurrentSteering = m_CurrentSteeringD =0;
  123. m_CurrentShift = m_CurrentShiftD = SHIFT_POS_NN;
  124. m_CurrentAccSteerAngle = m_CurrentAccVelocity = 0;
  125. m_pCurrentBehaviorState = m_pFollowState;
  126. m_TotalPath.clear();
  127. m_OriginalLocalPath.clear();
  128. m_TotalOriginalPath.clear();
  129. m_Path.clear();
  130. m_RollOuts.clear();
  131. m_pCurrentBehaviorState->m_Behavior = PlannerHNS::FORWARD_STATE;
  132. m_pCurrentBehaviorState->GetCalcParams()->bOutsideControl = 1;
  133. FirstLocalizeMe(start_pose);
  134. LocalizeMe(0);
  135. }
  136. void LocalPlannerH::FirstLocalizeMe(const WayPoint& initCarPos)
  137. {
  138. pLane = initCarPos.pLane;
  139. state = initCarPos;
  140. m_OdometryState.pos.a = initCarPos.pos.a;
  141. m_OdometryState.pos.x = initCarPos.pos.x + (m_CarInfo.wheel_base/2.0 * cos(initCarPos.pos.a));
  142. m_OdometryState.pos.y = initCarPos.pos.y + (m_CarInfo.wheel_base/2.0 * sin(initCarPos.pos.a));
  143. }
  144. void LocalPlannerH::LocalizeMe(const double& dt)
  145. {
  146. //calculate the new x, y ,
  147. WayPoint currPose = state;
  148. if(m_CurrentShift == SHIFT_POS_DD)
  149. {
  150. m_OdometryState.pos.x += m_CurrentVelocity * dt * cos(currPose.pos.a);
  151. m_OdometryState.pos.y += m_CurrentVelocity * dt * sin(currPose.pos.a);
  152. m_OdometryState.pos.a += m_CurrentVelocity * dt * tan(m_CurrentSteering) / m_CarInfo.wheel_base;
  153. }
  154. else if(m_CurrentShift == SHIFT_POS_RR )
  155. {
  156. m_OdometryState.pos.x += -m_CurrentVelocity * dt * cos(currPose.pos.a);
  157. m_OdometryState.pos.y += -m_CurrentVelocity * dt * sin(currPose.pos.a);
  158. m_OdometryState.pos.a += -m_CurrentVelocity * dt * tan(m_CurrentSteering);
  159. }
  160. m_OdometryState.pos.a = atan2(sin(m_OdometryState.pos.a), cos(m_OdometryState.pos.a));
  161. m_OdometryState.pos.a = UtilityH::FixNegativeAngle(m_OdometryState.pos.a);
  162. state.pos.a = m_OdometryState.pos.a;
  163. state.pos.x = m_OdometryState.pos.x - (m_CurrentVelocity*dt* (m_CarInfo.wheel_base) * cos (m_OdometryState.pos.a));
  164. state.pos.y = m_OdometryState.pos.y - (m_CurrentVelocity*dt* (m_CarInfo.wheel_base/2.0) * sin (m_OdometryState.pos.a));
  165. }
  166. void LocalPlannerH::UpdateState(const PlannerHNS::VehicleState& state, const bool& bUseDelay)
  167. {
  168. if(!bUseDelay)
  169. {
  170. m_CurrentSteering = m_CurrentSteeringD;
  171. // std::cout << " No Delay " << std::endl;
  172. }
  173. else
  174. {
  175. double currSteerDeg = RAD2DEG * m_CurrentSteering;
  176. double desiredSteerDeg = RAD2DEG * m_CurrentSteeringD;
  177. double mFact = UtilityH::GetMomentumScaleFactor(state.speed);
  178. double diff = desiredSteerDeg - currSteerDeg;
  179. double diffSign = UtilityH::GetSign(diff);
  180. double inc = 1.0*diffSign;
  181. if(fabs(diff) < 1.0 )
  182. inc = diff;
  183. // std::cout << "Delay: " << m_SimulationSteeringDelayFactor
  184. // << ", Fact: " << mFact
  185. // << ", Diff: " << diff
  186. // << ", inc: " << inc << std::endl;
  187. if(UtilityH::GetTimeDiffNow(m_SteerDelayTimer) > m_SimulationSteeringDelayFactor*mFact)
  188. {
  189. UtilityH::GetTickCount(m_SteerDelayTimer);
  190. currSteerDeg += inc;
  191. }
  192. m_CurrentSteering = DEG2RAD * currSteerDeg;
  193. }
  194. m_CurrentShift = m_CurrentShiftD;
  195. m_CurrentVelocity = m_CurrentVelocityD;
  196. }
  197. void LocalPlannerH::AddAndTransformContourPoints(const PlannerHNS::DetectedObject& obj, std::vector<PlannerHNS::WayPoint>& contourPoints)
  198. {
  199. contourPoints.clear();
  200. WayPoint p, p_center = obj.center;
  201. p_center.pos.a += M_PI_2;
  202. for(unsigned int i=0; i< obj.contour.size(); i++)
  203. {
  204. p.pos = obj.contour.at(i);
  205. //TransformPoint(p_center, p.pos);
  206. contourPoints.push_back(p);
  207. }
  208. contourPoints.push_back(obj.center);
  209. }
  210. void LocalPlannerH::TransformPoint(const PlannerHNS::WayPoint& refPose, PlannerHNS::GPSPoint& p)
  211. {
  212. PlannerHNS::Mat3 rotationMat(refPose.pos.a);
  213. PlannerHNS::Mat3 translationMat(refPose.pos.x, refPose.pos.y);
  214. p = rotationMat*p;
  215. p = translationMat*p;
  216. }
  217. bool LocalPlannerH::GetNextTrafficLight(const int& prevTrafficLightId, const std::vector<PlannerHNS::TrafficLight>& trafficLights, PlannerHNS::TrafficLight& trafficL)
  218. {
  219. for(unsigned int i = 0; i < trafficLights.size(); i++)
  220. {
  221. double d = hypot(trafficLights.at(i).pos.y - state.pos.y, trafficLights.at(i).pos.x - state.pos.x);
  222. if(d <= trafficLights.at(i).stoppingDistance)
  223. {
  224. double a_diff = UtilityH::AngleBetweenTwoAnglesPositive(UtilityH::FixNegativeAngle(trafficLights.at(i).pos.a) , UtilityH::FixNegativeAngle(state.pos.a));
  225. if(a_diff < M_PI_2 && trafficLights.at(i).id != prevTrafficLightId)
  226. {
  227. //std::cout << "Detected Light, ID = " << trafficLights.at(i).id << ", Distance = " << d << ", Angle = " << trafficLights.at(i).pos.a*RAD2DEG << ", Car Heading = " << state.pos.a*RAD2DEG << ", Diff = " << a_diff*RAD2DEG << std::endl;
  228. trafficL = trafficLights.at(i);
  229. return true;
  230. }
  231. }
  232. }
  233. return false;
  234. }
  235. void LocalPlannerH::CalculateImportantParameterForDecisionMaking(const PlannerHNS::VehicleState& car_state,
  236. const int& goalID, const bool& bEmergencyStop, const vector<TrafficLight>& detectedLights,
  237. const TrajectoryCost& bestTrajectory)
  238. {
  239. PreCalculatedConditions* pValues = m_pCurrentBehaviorState->GetCalcParams();
  240. double critical_long_front_distance = m_CarInfo.wheel_base/2.0 + m_CarInfo.length/2.0 + m_params.verticalSafetyDistance;
  241. pValues->minStoppingDistance = -pow(car_state.speed, 2)/(m_CarInfo.max_deceleration);
  242. pValues->iCentralTrajectory = m_pCurrentBehaviorState->m_pParams->rollOutNumber/2;
  243. if(pValues->iCurrSafeTrajectory < 0)
  244. pValues->iCurrSafeTrajectory = pValues->iCentralTrajectory;
  245. if(pValues->iPrevSafeTrajectory < 0)
  246. pValues->iPrevSafeTrajectory = pValues->iCentralTrajectory;
  247. pValues->stoppingDistances.clear();
  248. pValues->currentVelocity = car_state.speed;
  249. pValues->bTrafficIsRed = false;
  250. pValues->currentTrafficLightID = -1;
  251. pValues->bRePlan = false;
  252. pValues->bFullyBlock = false;
  253. pValues->distanceToNext = bestTrajectory.closest_obj_distance;
  254. pValues->velocityOfNext = bestTrajectory.closest_obj_velocity;
  255. if(pValues->distanceToNext > m_params.minDistanceToAvoid)
  256. pValues->iCurrSafeTrajectory = pValues->iCentralTrajectory;
  257. else if(bestTrajectory.index>=0)
  258. pValues->iCurrSafeTrajectory = bestTrajectory.index;
  259. pValues->bFullyBlock = bestTrajectory.bBlocked;
  260. if(bestTrajectory.lane_index >=0)
  261. pValues->iCurrSafeLane = bestTrajectory.lane_index;
  262. else
  263. {
  264. PlannerHNS::RelativeInfo info;
  265. PlannerHNS::PlanningHelpers::GetRelativeInfoRange(m_TotalPath, state, m_params.rollOutDensity*m_params.rollOutNumber/2.0 + 0.1, info);
  266. pValues->iCurrSafeLane = info.iGlobalPath;
  267. }
  268. if(NoWayTest(pValues->minStoppingDistance, pValues->iCurrSafeLane))
  269. pValues->currentGoalID = -1;
  270. else
  271. pValues->currentGoalID = goalID;
  272. m_iSafeTrajectory = pValues->iCurrSafeTrajectory;
  273. m_iCurrentTotalPathId = pValues->iCurrSafeLane;
  274. int stopLineID = -1;
  275. int stopSignID = -1;
  276. int trafficLightID = -1;
  277. double distanceToClosestStopLine = 0;
  278. bool bGreenTrafficLight = true;
  279. if(m_TotalPath.size()>0)
  280. distanceToClosestStopLine = PlanningHelpers::GetDistanceToClosestStopLineAndCheck(m_TotalPath.at(pValues->iCurrSafeLane), state, 0, stopLineID, stopSignID, trafficLightID) - critical_long_front_distance;
  281. if(distanceToClosestStopLine > 0 && distanceToClosestStopLine < pValues->minStoppingDistance)
  282. {
  283. if(m_pCurrentBehaviorState->m_pParams->enableTrafficLightBehavior)
  284. {
  285. pValues->currentTrafficLightID = trafficLightID;
  286. //cout << "Detected Traffic Light: " << trafficLightID << endl;
  287. for(unsigned int i=0; i< detectedLights.size(); i++)
  288. {
  289. if(detectedLights.at(i).id == trafficLightID)
  290. bGreenTrafficLight = (detectedLights.at(i).lightState == GREEN_LIGHT);
  291. }
  292. }
  293. if(m_pCurrentBehaviorState->m_pParams->enableStopSignBehavior)
  294. pValues->currentStopSignID = stopSignID;
  295. pValues->stoppingDistances.push_back(distanceToClosestStopLine);
  296. //std::cout << "LP => D: " << pValues->distanceToStop() << ", PrevSignID: " << pValues->prevTrafficLightID << ", CurrSignID: " << pValues->currentTrafficLightID << ", Green: " << bGreenTrafficLight << endl;
  297. }
  298. // cout << "Distance To Closest: " << distanceToClosestStopLine << ", Stop LineID: " << stopLineID << ", Stop SignID: " << stopSignID << ", TFID: " << trafficLightID << endl;
  299. pValues->bTrafficIsRed = !bGreenTrafficLight;
  300. if(bEmergencyStop)
  301. {
  302. pValues->bFullyBlock = true;
  303. pValues->distanceToNext = 1;
  304. pValues->velocityOfNext = 0;
  305. }
  306. //cout << "Distances: " << pValues->stoppingDistances.size() << ", Distance To Stop : " << pValues->distanceToStop << endl;
  307. }
  308. void LocalPlannerH::UpdateCurrentLane(PlannerHNS::RoadNetwork& map, const double& search_distance)
  309. {
  310. PlannerHNS::Lane* pMapLane = 0;
  311. PlannerHNS::Lane* pPathLane = 0;
  312. pPathLane = MappingHelpers::GetLaneFromPath(state, m_Path);
  313. if(!pPathLane)
  314. {
  315. cout << "Performance Alert: Can't Find Lane Information in Global Path, Searching the Map :( " << endl;
  316. pMapLane = MappingHelpers::GetClosestLaneFromMap(state, map, search_distance);
  317. }
  318. if(pPathLane)
  319. pLane = pPathLane;
  320. else if(pMapLane)
  321. pLane = pMapLane;
  322. else
  323. pLane = 0;
  324. }
  325. void LocalPlannerH::SimulateOdoPosition(const double& dt, const PlannerHNS::VehicleState& vehicleState)
  326. {
  327. SetSimulatedTargetOdometryReadings(vehicleState.speed, vehicleState.steer, vehicleState.shift);
  328. UpdateState(vehicleState, false);
  329. LocalizeMe(dt);
  330. }
  331. bool LocalPlannerH::NoWayTest(const double& min_distance, const int& iGlobalPathIndex)
  332. {
  333. if(m_TotalPath.size()==0) return false;
  334. PlannerHNS::RelativeInfo info;
  335. PlanningHelpers::GetRelativeInfo(m_TotalPath.at(iGlobalPathIndex), state, info);
  336. double d = 0;
  337. for(unsigned int i = info.iFront; i < m_TotalPath.at(iGlobalPathIndex).size()-1; i++)
  338. {
  339. d+= hypot(m_TotalPath.at(iGlobalPathIndex).at(i+1).pos.y - m_TotalPath.at(iGlobalPathIndex).at(i).pos.y, m_TotalPath.at(iGlobalPathIndex).at(i+1).pos.x - m_TotalPath.at(iGlobalPathIndex).at(i).pos.x);
  340. if(d > min_distance)
  341. return false;
  342. }
  343. return true;
  344. }
  345. bool LocalPlannerH::SelectSafeTrajectoryAndSpeedProfile(const PlannerHNS::VehicleState& vehicleState)
  346. {
  347. PlannerHNS::PreCalculatedConditions *preCalcPrams = m_pCurrentBehaviorState->GetCalcParams();
  348. bool bNewTrajectory = false;
  349. if(m_TotalPath.size()>0)
  350. {
  351. int currIndex = PlannerHNS::PlanningHelpers::GetClosestNextPointIndexFast(m_Path, state);
  352. int index_limit = 0;//m_Path.size() - 20;
  353. if(index_limit<=0)
  354. index_limit = m_Path.size()/2.0;
  355. if(m_RollOuts.size() == 0
  356. || currIndex > index_limit
  357. || m_pCurrentBehaviorState->GetCalcParams()->bRePlan
  358. || m_pCurrentBehaviorState->GetCalcParams()->bNewGlobalPath
  359. || m_pCurrentBehaviorState->m_Behavior == OBSTACLE_AVOIDANCE_STATE)
  360. {
  361. PlannerHNS::PlannerH planner;
  362. planner.GenerateRunoffTrajectory(m_TotalPath, state,
  363. m_pCurrentBehaviorState->m_pParams->enableLaneChange,
  364. vehicleState.speed,
  365. m_pCurrentBehaviorState->m_pParams->microPlanDistance,
  366. m_pCurrentBehaviorState->m_pParams->maxSpeed,
  367. m_pCurrentBehaviorState->m_pParams->minSpeed,
  368. m_pCurrentBehaviorState->m_pParams->carTipMargin,
  369. m_pCurrentBehaviorState->m_pParams->rollInMargin,
  370. m_pCurrentBehaviorState->m_pParams->rollInSpeedFactor,
  371. m_pCurrentBehaviorState->m_pParams->pathDensity,
  372. m_pCurrentBehaviorState->m_pParams->rollOutDensity,
  373. m_pCurrentBehaviorState->m_pParams->rollOutNumber,
  374. m_pCurrentBehaviorState->m_pParams->smoothingDataWeight,
  375. m_pCurrentBehaviorState->m_pParams->smoothingSmoothWeight,
  376. m_pCurrentBehaviorState->m_pParams->smoothingToleranceError,
  377. m_pCurrentBehaviorState->m_pParams->speedProfileFactor,
  378. m_pCurrentBehaviorState->m_pParams->enableHeadingSmoothing,
  379. preCalcPrams->iCurrSafeLane , preCalcPrams->iCurrSafeTrajectory,
  380. m_RollOuts, m_SampledPoints);
  381. m_pCurrentBehaviorState->GetCalcParams()->bRePlan = false;
  382. m_pCurrentBehaviorState->GetCalcParams()->bNewGlobalPath = false;
  383. if(m_pCurrentBehaviorState->m_Behavior == OBSTACLE_AVOIDANCE_STATE)
  384. preCalcPrams->iPrevSafeTrajectory = preCalcPrams->iCurrSafeTrajectory;
  385. else
  386. preCalcPrams->iPrevSafeTrajectory = preCalcPrams->iCentralTrajectory;
  387. preCalcPrams->iPrevSafeLane = preCalcPrams->iCurrSafeLane;
  388. if(preCalcPrams->iPrevSafeLane >= 0
  389. && preCalcPrams->iPrevSafeLane < (int)m_RollOuts.size()
  390. && preCalcPrams->iPrevSafeTrajectory >= 0
  391. && preCalcPrams->iPrevSafeTrajectory < (int)m_RollOuts.at(preCalcPrams->iPrevSafeLane).size())
  392. {
  393. m_Path = m_RollOuts.at(preCalcPrams->iPrevSafeLane).at(preCalcPrams->iPrevSafeTrajectory);
  394. m_OriginalLocalPath = m_TotalPath.at(m_iCurrentTotalPathId);
  395. bNewTrajectory = true;
  396. }
  397. }
  398. }
  399. return bNewTrajectory;
  400. }
  401. PlannerHNS::BehaviorState LocalPlannerH::GenerateBehaviorState(const PlannerHNS::VehicleState& vehicleState)
  402. {
  403. PlannerHNS::PreCalculatedConditions *preCalcPrams = m_pCurrentBehaviorState->GetCalcParams();
  404. m_pCurrentBehaviorState = m_pCurrentBehaviorState->GetNextState();
  405. if(m_pCurrentBehaviorState==0)
  406. m_pCurrentBehaviorState = m_pInitState;
  407. PlannerHNS::BehaviorState currentBehavior;
  408. currentBehavior.state = m_pCurrentBehaviorState->m_Behavior;
  409. currentBehavior.followDistance = preCalcPrams->distanceToNext;
  410. currentBehavior.minVelocity = 0;
  411. currentBehavior.stopDistance = preCalcPrams->distanceToStop();
  412. currentBehavior.followVelocity = preCalcPrams->velocityOfNext;
  413. double average_braking_distance = -pow(vehicleState.speed, 2)/(m_CarInfo.max_deceleration) + m_params.additionalBrakingDistance;
  414. if(average_braking_distance < 15)
  415. average_braking_distance = 15;
  416. currentBehavior.indicator = PlanningHelpers::GetIndicatorsFromPath(m_Path, state, average_braking_distance );
  417. return currentBehavior;
  418. }
  419. // double LocalPlannerH::UpdateVelocityDirectlyToTrajectory(const BehaviorState& beh, const VehicleState& CurrStatus, const double& dt)
  420. // {
  421. // RelativeInfo info, total_info;
  422. // PlanningHelpers::GetRelativeInfo(m_TotalPath.at(m_iCurrentTotalPathId), state, total_info);
  423. // PlanningHelpers::GetRelativeInfo(m_Path, state, info);
  424. // double average_braking_distance = -pow(CurrStatus.speed, 2)/(m_CarInfo.max_deceleration);
  425. // double max_velocity = PlannerHNS::PlanningHelpers::GetVelocityAhead(m_TotalPath.at(m_iCurrentTotalPathId), total_info, average_braking_distance);
  426. //
  427. // unsigned int point_index = 0;
  428. // double critical_long_front_distance = 2.0;
  429. //
  430. // if(m_Path.size() <= 5)
  431. // {
  432. // double target_velocity = 0;
  433. // for(unsigned int i = 0; i < m_Path.size(); i++)
  434. // m_Path.at(i).v = target_velocity;
  435. // }
  436. // else if(beh.state == TRAFFIC_LIGHT_STOP_STATE || beh.state == STOP_SIGN_STOP_STATE || beh.state == STOP_SIGN_WAIT_STATE || beh.state == TRAFFIC_LIGHT_WAIT_STATE)
  437. // {
  438. // PlanningHelpers::GetFollowPointOnTrajectory(m_Path, info, beh.stopDistance - critical_long_front_distance, point_index);
  439. //
  440. // double inc = CurrStatus.speed;
  441. // int iRange = point_index - info.iBack;
  442. // if(iRange > 0)
  443. // inc = inc / (double)iRange;
  444. // else
  445. // inc = 0;
  446. //
  447. // double target_velocity = CurrStatus.speed - inc;
  448. // for(unsigned int i = info.iBack; i < point_index; i++)
  449. // {
  450. // if(i < m_Path.size() && i >= 0)
  451. // m_Path.at(i).v = target_velocity;
  452. // target_velocity -= inc;
  453. // }
  454. // }
  455. // else if(beh.state == FOLLOW_STATE)
  456. // {
  457. // double targe_acceleration = -pow(CurrStatus.speed, 2)/(2.0*(beh.followDistance - critical_long_front_distance));
  458. // if(targe_acceleration <= 0 && targe_acceleration > m_CarInfo.max_deceleration/2.0)
  459. // {
  460. // double target_velocity = (targe_acceleration * dt) + CurrStatus.speed;
  461. // for(unsigned int i = 0; i < m_Path.size(); i++)
  462. // {
  463. // if(m_pCurrentBehaviorState->GetCalcParams()->iCurrSafeTrajectory == m_pCurrentBehaviorState->GetCalcParams()->iCentralTrajectory)
  464. // m_Path.at(i).v = target_velocity;
  465. // else
  466. // m_Path.at(i).v = target_velocity*AVOIDANCE_SPEED_FACTOR;
  467. // }
  468. //
  469. // //cout << "Accelerate -> Target V: " << target_velocity << ", Brake D: " << average_braking_distance << ", Acceleration: " << targe_acceleration << endl;
  470. // }
  471. // else
  472. // {
  473. // WayPoint pursuite_point = PlanningHelpers::GetFollowPointOnTrajectory(m_Path, info, beh.followDistance - critical_long_front_distance, point_index);
  474. // double inc = CurrStatus.speed;
  475. // int iRange = point_index - info.iBack;
  476. //
  477. // if(iRange > 0)
  478. // inc = inc / (double)iRange;
  479. // else
  480. // inc = 0;
  481. //
  482. // double target_velocity = CurrStatus.speed - inc;
  483. // for(unsigned int i = info.iBack; i < point_index; i++)
  484. // {
  485. // if(i < m_Path.size() && i >= 0)
  486. // {
  487. // target_velocity = target_velocity < 0 ? 0 : target_velocity;
  488. // if(m_pCurrentBehaviorState->GetCalcParams()->iCurrSafeTrajectory == m_pCurrentBehaviorState->GetCalcParams()->iCentralTrajectory)
  489. // m_Path.at(i).v = target_velocity;
  490. // else
  491. // m_Path.at(i).v = target_velocity*AVOIDANCE_SPEED_FACTOR;
  492. // }
  493. //
  494. // target_velocity -= inc;
  495. // }
  496. //
  497. // //cout << "Decelerate -> Target V: " << target_velocity << ", Brake D: " << average_braking_distance << ", Start I" << info.iBack << endl;
  498. // }
  499. //
  500. // }
  501. // else if(beh.state == FORWARD_STATE || beh.state == OBSTACLE_AVOIDANCE_STATE )
  502. // {
  503. // double target_velocity = max_velocity;
  504. //
  505. // for(unsigned int i = 0; i < m_Path.size(); i++)
  506. // {
  507. // if(m_pCurrentBehaviorState->GetCalcParams()->iCurrSafeTrajectory == m_pCurrentBehaviorState->GetCalcParams()->iCentralTrajectory)
  508. // m_Path.at(i).v = target_velocity;
  509. // else
  510. // m_Path.at(i).v = target_velocity*AVOIDANCE_SPEED_FACTOR;
  511. // }
  512. // }
  513. // else
  514. // {
  515. // double target_velocity = 0;
  516. // for(unsigned int i = 0; i < m_Path.size(); i++)
  517. // m_Path.at(i).v = target_velocity;
  518. // }
  519. //
  520. // return max_velocity;
  521. // }
  522. double LocalPlannerH::UpdateVelocityDirectlyToTrajectory(const BehaviorState& beh, const VehicleState& CurrStatus, const double& dt)
  523. {
  524. if(m_TotalOriginalPath.size() ==0 ) return 0;
  525. RelativeInfo info, total_info;
  526. PlanningHelpers::GetRelativeInfo(m_TotalOriginalPath.at(m_iCurrentTotalPathId), state, total_info);
  527. PlanningHelpers::GetRelativeInfo(m_Path, state, info);
  528. double average_braking_distance = -pow(CurrStatus.speed, 2)/(m_CarInfo.max_deceleration) + m_params.additionalBrakingDistance;
  529. double max_velocity = PlannerHNS::PlanningHelpers::GetVelocityAhead(m_TotalOriginalPath.at(m_iCurrentTotalPathId), total_info, m_PrevBrakingWayPoint, average_braking_distance);
  530. unsigned int point_index = 0;
  531. double critical_long_front_distance = 2.0;
  532. if(m_Path.size() <= 5)
  533. {
  534. double target_velocity = 0;
  535. for(unsigned int i = 0; i < m_Path.size(); i++)
  536. m_Path.at(i).v = target_velocity;
  537. }
  538. else if(beh.state == TRAFFIC_LIGHT_STOP_STATE || beh.state == STOP_SIGN_STOP_STATE || beh.state == STOP_SIGN_WAIT_STATE || beh.state == TRAFFIC_LIGHT_WAIT_STATE)
  539. {
  540. PlanningHelpers::GetFollowPointOnTrajectory(m_Path, info, beh.stopDistance - critical_long_front_distance, point_index);
  541. double e = -beh.stopDistance;
  542. double desiredVelocity = m_pidStopping.getPID(e);
  543. for(unsigned int i = info.iBack; i < point_index; i++)
  544. {
  545. if(i < m_Path.size() && i >= 0)
  546. m_Path.at(i).v = desiredVelocity;
  547. }
  548. return desiredVelocity;
  549. }
  550. else if(beh.state == FOLLOW_STATE)
  551. {
  552. double targe_acceleration = -pow(CurrStatus.speed, 2)/(2.0*(beh.followDistance - critical_long_front_distance));
  553. if(targe_acceleration <= 0 && targe_acceleration > m_CarInfo.max_deceleration/2.0)
  554. {
  555. double target_velocity = (targe_acceleration * dt) + CurrStatus.speed;
  556. double e = target_velocity - CurrStatus.speed;
  557. double desiredVelocity = m_pidVelocity.getPID(e);
  558. for(unsigned int i = info.iBack; i < m_Path.size(); i++)
  559. {
  560. if(i < m_Path.size() && i >= 0)
  561. m_Path.at(i).v = desiredVelocity;
  562. }
  563. return desiredVelocity;
  564. //cout << "Accelerate -> Target V: " << target_velocity << ", Brake D: " << average_braking_distance << ", Acceleration: " << targe_acceleration << endl;
  565. }
  566. else
  567. {
  568. WayPoint pursuite_point = PlanningHelpers::GetFollowPointOnTrajectory(m_Path, info, beh.followDistance - critical_long_front_distance, point_index);
  569. double e = beh.followDistance - m_params.minFollowingDistance;
  570. double desiredVelocity = m_pidStopping.getPID(e);
  571. for(unsigned int i = info.iBack; i < point_index; i++)
  572. {
  573. if(i < m_Path.size() && i >= 0)
  574. m_Path.at(i).v = desiredVelocity;
  575. }
  576. return desiredVelocity;
  577. //cout << "Decelerate -> Target V: " << target_velocity << ", Brake D: " << average_braking_distance << ", Start I" << info.iBack << endl;
  578. }
  579. }
  580. else if(beh.state == FORWARD_STATE || beh.state == OBSTACLE_AVOIDANCE_STATE )
  581. {
  582. double target_velocity = max_velocity;
  583. if(m_pCurrentBehaviorState->GetCalcParams()->iCurrSafeTrajectory != m_pCurrentBehaviorState->GetCalcParams()->iCentralTrajectory)
  584. target_velocity*=AVOIDANCE_SPEED_FACTOR;
  585. double e = target_velocity - CurrStatus.speed;
  586. double desiredVelocity = m_pidVelocity.getPID(e);
  587. for(unsigned int i = info.iBack; i < m_Path.size(); i++)
  588. {
  589. m_Path.at(i).v = desiredVelocity;
  590. }
  591. return desiredVelocity;
  592. }
  593. else
  594. {
  595. double target_velocity = 0;
  596. for(unsigned int i = info.iBack; i < m_Path.size(); i++)
  597. m_Path.at(i).v = target_velocity;
  598. return target_velocity;
  599. }
  600. return max_velocity;
  601. }
  602. void LocalPlannerH::ExtractHorizonAndCalculateRecommendedSpeed()
  603. {
  604. if(m_pCurrentBehaviorState->GetCalcParams()->bNewGlobalPath && m_TotalOriginalPath.size() > 0)
  605. {
  606. m_PrevBrakingWayPoint = 0;
  607. PlanningHelpers::FixPathDensity(m_TotalOriginalPath.at(m_iCurrentTotalPathId), m_pCurrentBehaviorState->m_pParams->pathDensity);
  608. PlanningHelpers::SmoothPath(m_TotalOriginalPath.at(m_iCurrentTotalPathId), 0.49, 0.25, 0.05);
  609. PlanningHelpers::GenerateRecommendedSpeed(m_TotalOriginalPath.at(m_iCurrentTotalPathId), m_CarInfo.max_speed_forward, m_pCurrentBehaviorState->m_pParams->speedProfileFactor);
  610. m_TotalOriginalPath.at(m_iCurrentTotalPathId).at(m_TotalOriginalPath.at(m_iCurrentTotalPathId).size()-1).v = 0;
  611. }
  612. m_TotalPath.clear();
  613. for(unsigned int i = 0; i < m_TotalOriginalPath.size(); i++)
  614. {
  615. vector<WayPoint> centerTrajectorySmoothed;
  616. PlanningHelpers::ExtractPartFromPointToDistanceFast(m_TotalOriginalPath.at(i), state,
  617. m_pCurrentBehaviorState->m_pParams->horizonDistance ,
  618. m_pCurrentBehaviorState->m_pParams->pathDensity ,
  619. centerTrajectorySmoothed,
  620. m_pCurrentBehaviorState->m_pParams->smoothingDataWeight,
  621. m_pCurrentBehaviorState->m_pParams->smoothingSmoothWeight,
  622. m_pCurrentBehaviorState->m_pParams->smoothingToleranceError);
  623. m_TotalPath.push_back(centerTrajectorySmoothed);
  624. }
  625. }
  626. PlannerHNS::BehaviorState LocalPlannerH::DoOneStep(
  627. const double& dt,
  628. const PlannerHNS::VehicleState& vehicleState,
  629. const std::vector<PlannerHNS::DetectedObject>& obj_list,
  630. const int& goalID, PlannerHNS::RoadNetwork& map ,
  631. const bool& bEmergencyStop,
  632. const std::vector<TrafficLight>& trafficLight,
  633. const bool& bLive)
  634. {
  635. PlannerHNS::BehaviorState beh;
  636. m_params.minFollowingDistance = m_InitialFollowingDistance + vehicleState.speed*1.5;
  637. if(!bLive)
  638. SimulateOdoPosition(dt, vehicleState);
  639. UpdateCurrentLane(map, 3.0);
  640. ExtractHorizonAndCalculateRecommendedSpeed();
  641. m_PredictedTrajectoryObstacles = obj_list;
  642. timespec t;
  643. UtilityH::GetTickCount(t);
  644. TrajectoryCost tc = m_TrajectoryCostsCalculatotor.DoOneStep(m_RollOuts, m_TotalPath, state,
  645. m_pCurrentBehaviorState->GetCalcParams()->iCurrSafeTrajectory, m_pCurrentBehaviorState->GetCalcParams()->iCurrSafeLane, *m_pCurrentBehaviorState->m_pParams,
  646. m_CarInfo,vehicleState, m_PredictedTrajectoryObstacles);
  647. m_CostCalculationTime = UtilityH::GetTimeDiffNow(t);
  648. UtilityH::GetTickCount(t);
  649. CalculateImportantParameterForDecisionMaking(vehicleState, goalID, bEmergencyStop, trafficLight, tc);
  650. beh = GenerateBehaviorState(vehicleState);
  651. m_BehaviorGenTime = UtilityH::GetTimeDiffNow(t);
  652. UtilityH::GetTickCount(t);
  653. beh.bNewPlan = SelectSafeTrajectoryAndSpeedProfile(vehicleState);
  654. m_RollOutsGenerationTime = UtilityH::GetTimeDiffNow(t);
  655. beh.maxVelocity = UpdateVelocityDirectlyToTrajectory(beh, vehicleState, dt);
  656. return beh;
  657. }
  658. } /* namespace PlannerHNS */