BehaviorPrediction.cpp 25 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762
  1. /// \file BehaviorPrediction.cpp
  2. /// \brief Predict detected vehicles's possible trajectories, these trajectories extracted from the vector map.
  3. /// \author Hatem Darweesh
  4. /// \date Jul 6, 2017
  5. #include "op_planner/BehaviorPrediction.h"
  6. #include "op_planner/MappingHelpers.h"
  7. #include "op_planner/PlanningHelpers.h"
  8. #include "op_planner/MatrixOperations.h"
  9. namespace PlannerHNS
  10. {
  11. BehaviorPrediction::BehaviorPrediction()
  12. {
  13. m_MaxLaneDetectionDistance = 0.5;
  14. m_PredictionDistance = 20.0;
  15. m_bGenerateBranches = false;
  16. m_bUseFixedPrediction = true;
  17. m_bStepByStep = false;
  18. m_bCanDecide = true;
  19. m_bParticleFilter = false;
  20. UtilityHNS::UtilityH::GetTickCount(m_GenerationTimer);
  21. UtilityHNS::UtilityH::GetTickCount(m_ResamplingTimer);
  22. m_bFirstMove = true;
  23. m_bDebugOut = false;
  24. }
  25. BehaviorPrediction::~BehaviorPrediction()
  26. {
  27. }
  28. void BehaviorPrediction::FilterObservations(const std::vector<DetectedObject>& obj_list, RoadNetwork& map, std::vector<DetectedObject>& filtered_list)
  29. {
  30. for(unsigned int i=0; i < obj_list.size(); i++)
  31. {
  32. if(obj_list.at(i).t == SIDEWALK || obj_list.at(i).center.v < 1.0)
  33. continue;
  34. bool bFound = false;
  35. int found_index = 0;
  36. for(unsigned int ip=0; ip < filtered_list.size(); ip++)
  37. {
  38. if(filtered_list.at(ip).id == obj_list.at(i).id)
  39. {
  40. found_index = ip;
  41. bFound = true;
  42. break;
  43. }
  44. }
  45. if(bFound)
  46. filtered_list.at(found_index) = obj_list.at(i);
  47. else
  48. filtered_list.push_back(obj_list.at(i));
  49. }
  50. for(int ip=0; ip < (int)filtered_list.size(); ip++)
  51. {
  52. //check for cleaning
  53. bool bRevFound = false;
  54. for(unsigned int ic=0; ic < obj_list.size(); ic++)
  55. {
  56. if(filtered_list.at(ip).id == obj_list.at(ic).id)
  57. {
  58. bRevFound = true;
  59. break;
  60. }
  61. }
  62. if(!bRevFound)
  63. {
  64. filtered_list.erase(filtered_list.begin()+ip);
  65. ip--;
  66. }
  67. }
  68. }
  69. void BehaviorPrediction::DoOneStep(const std::vector<DetectedObject>& obj_list, const WayPoint& currPose, const double& minSpeed, const double& maxDeceleration, RoadNetwork& map)
  70. {
  71. if(!m_bUseFixedPrediction && maxDeceleration !=0)
  72. m_PredictionDistance = -pow(currPose.v, 2)/(maxDeceleration);
  73. ExtractTrajectoriesFromMap(obj_list, map, m_ParticleInfo_II);
  74. CalculateCollisionTimes(minSpeed);
  75. if(m_bParticleFilter)
  76. {
  77. ParticleFilterSteps(m_ParticleInfo_II);
  78. }
  79. }
  80. void BehaviorPrediction::CalculateCollisionTimes(const double& minSpeed)
  81. {
  82. for(unsigned int i=0; i < m_ParticleInfo_II.size(); i++)
  83. {
  84. for(unsigned int j=0; j < m_ParticleInfo_II.at(i)->obj.predTrajectories.size(); j++)
  85. {
  86. PlannerHNS::PlanningHelpers::PredictConstantTimeCostForTrajectory(m_ParticleInfo_II.at(i)->obj.predTrajectories.at(j), m_ParticleInfo_II.at(i)->obj.center, minSpeed, m_PredictionDistance);
  87. // PlannerHNS::PlanningHelpers::CalcAngleAndCost(m_PredictedObjects.at(i).predTrajectories.at(j));
  88. }
  89. }
  90. }
  91. void BehaviorPrediction::ExtractTrajectoriesFromMap(const std::vector<DetectedObject>& curr_obj_list,RoadNetwork& map, std::vector<ObjParticles*>& old_obj_list)
  92. {
  93. PlannerH planner;
  94. m_temp_list_ii.clear();
  95. std::vector<ObjParticles*> delete_me_list = old_obj_list;
  96. for(unsigned int i=0; i < curr_obj_list.size(); i++)
  97. {
  98. bool bMatch = false;
  99. for(unsigned int ip=0; ip < old_obj_list.size(); ip++)
  100. {
  101. if(old_obj_list.at(ip)->obj.id == curr_obj_list.at(i).id)
  102. {
  103. bool bFound = false;
  104. for(unsigned int k=0; k < m_temp_list_ii.size(); k++)
  105. {
  106. if(m_temp_list_ii.at(k) == old_obj_list.at(ip))
  107. {
  108. bFound = true;
  109. break;
  110. }
  111. }
  112. if(!bFound)
  113. {
  114. old_obj_list.at(ip)->obj = curr_obj_list.at(i);
  115. m_temp_list_ii.push_back(old_obj_list.at(ip));
  116. }
  117. DeleteFromList(delete_me_list, old_obj_list.at(ip));
  118. old_obj_list.erase(old_obj_list.begin()+ip);
  119. bMatch = true;
  120. break;
  121. }
  122. }
  123. if(!bMatch)
  124. {
  125. ObjParticles* pNewObj = new ObjParticles();
  126. pNewObj->obj = curr_obj_list.at(i);
  127. m_temp_list_ii.push_back(pNewObj);
  128. }
  129. }
  130. DeleteTheRest(delete_me_list);
  131. old_obj_list.clear();
  132. old_obj_list = m_temp_list_ii;
  133. //m_PredictedObjects.clear();
  134. for(unsigned int ip=0; ip < old_obj_list.size(); ip++)
  135. {
  136. PredictCurrentTrajectory(map, old_obj_list.at(ip));
  137. //m_PredictedObjects.push_back(old_obj_list.at(ip)->obj);
  138. old_obj_list.at(ip)->MatchTrajectories();
  139. }
  140. }
  141. void BehaviorPrediction::CalPredictionTimeForObject(ObjParticles* pCarPart)
  142. {
  143. if(pCarPart->obj.center.v > 0 )
  144. pCarPart->m_PredictionTime = (MIN_PREDICTION_TIME + 1.5*pCarPart->obj.center.v) / pCarPart->obj.center.v;
  145. else
  146. pCarPart->m_PredictionTime = MIN_PREDICTION_TIME;
  147. }
  148. void BehaviorPrediction::PredictCurrentTrajectory(RoadNetwork& map, ObjParticles* pCarPart)
  149. {
  150. pCarPart->obj.predTrajectories.clear();
  151. PlannerH planner;
  152. if(pCarPart->obj.bDirection && pCarPart->obj.bVelocity)
  153. {
  154. PlannerHNS::WayPoint fake_pose = pCarPart->obj.center;
  155. pCarPart->obj.pClosestWaypoints = MappingHelpers::GetClosestWaypointsListFromMap(fake_pose, map, m_MaxLaneDetectionDistance, pCarPart->obj.bDirection);
  156. planner.PredictTrajectoriesUsingDP(fake_pose, pCarPart->obj.pClosestWaypoints, m_PredictionDistance, pCarPart->obj.predTrajectories, m_bGenerateBranches, pCarPart->obj.bDirection);
  157. }
  158. else
  159. {
  160. bool bLocalDirectionSearch = false;
  161. pCarPart->obj.pClosestWaypoints = MappingHelpers::GetClosestWaypointsListFromMap(pCarPart->obj.center, map, m_MaxLaneDetectionDistance, pCarPart->obj.bDirection);
  162. if(pCarPart->obj.pClosestWaypoints.size()>0)
  163. {
  164. pCarPart->obj.center.pos.a = pCarPart->obj.pClosestWaypoints.at(0)->pos.a;
  165. bLocalDirectionSearch = true;
  166. }
  167. planner.PredictTrajectoriesUsingDP(pCarPart->obj.center, pCarPart->obj.pClosestWaypoints, m_PredictionDistance, pCarPart->obj.predTrajectories, m_bGenerateBranches, pCarPart->obj.bDirection);
  168. }
  169. for(unsigned int t = 0; t < pCarPart->obj.predTrajectories.size(); t ++)
  170. {
  171. // std::ostringstream path_name;
  172. // path_name << "/home/hatem/autoware_openplanner_logs/TempPredLog/";
  173. // path_name << t ;
  174. // path_name << "_";
  175. // PlanningHelpers::GenerateRecommendedSpeed(pCarPart->obj.predTrajectories.at(t), 10, 1.0);
  176. // PlannerHNS::PlanningHelpers::WritePathToFile(path_name.str(), pCarPart->obj.predTrajectories.at(t));
  177. if(pCarPart->obj.predTrajectories.at(t).size() > 0)
  178. pCarPart->obj.predTrajectories.at(t).at(0).collisionCost = 0;
  179. }
  180. }
  181. void BehaviorPrediction::ParticleFilterSteps(std::vector<ObjParticles*>& part_info)
  182. {
  183. for(unsigned int i=0; i < part_info.size(); i++)
  184. {
  185. SamplesFreshParticles(part_info.at(i));
  186. CollectParticles(part_info.at(i));
  187. MoveParticles(part_info.at(i));
  188. CalculateWeights(part_info.at(i));
  189. RemoveWeakParticles(part_info.at(i));
  190. CalculateAveragesAndProbabilities(part_info.at(i));
  191. FindBest(part_info.at(i));
  192. }
  193. }
  194. int BehaviorPrediction::FromIndicatorToNumber(const PlannerHNS::LIGHT_INDICATOR& indi)
  195. {
  196. if(indi == PlannerHNS::INDICATOR_NONE)
  197. return 0;
  198. else if(indi == PlannerHNS::INDICATOR_LEFT)
  199. {
  200. return 1;
  201. }
  202. else if(indi == PlannerHNS::INDICATOR_RIGHT)
  203. return 2;
  204. else if(indi == PlannerHNS::INDICATOR_BOTH)
  205. return 3;
  206. else
  207. return 0;
  208. }
  209. PlannerHNS::LIGHT_INDICATOR BehaviorPrediction::FromNumbertoIndicator(const int& num)
  210. {
  211. if(num == 0)
  212. return PlannerHNS::INDICATOR_NONE;
  213. else if(num == 1)
  214. return PlannerHNS::INDICATOR_LEFT;
  215. else if(num == 2)
  216. return PlannerHNS::INDICATOR_RIGHT;
  217. else if(num == 3)
  218. return PlannerHNS::INDICATOR_BOTH;
  219. else
  220. return PlannerHNS::INDICATOR_NONE;
  221. }
  222. double BehaviorPrediction::CalcIndicatorWeight(PlannerHNS::LIGHT_INDICATOR p_ind, PlannerHNS::LIGHT_INDICATOR obj_ind)
  223. {
  224. if((obj_ind == PlannerHNS::INDICATOR_LEFT || obj_ind == PlannerHNS::INDICATOR_RIGHT || obj_ind == PlannerHNS::INDICATOR_NONE) && p_ind == obj_ind)
  225. return 0.99;
  226. else
  227. return 0.01;
  228. }
  229. double BehaviorPrediction::CalcAccelerationWeight(int p_acl, int obj_acl)
  230. {
  231. if((p_acl > 0 && obj_acl > 0) || (p_acl < 0 && obj_acl < 0))
  232. return 0.99;
  233. else
  234. return 0.01;
  235. }
  236. void BehaviorPrediction::CalOnePartWeight(ObjParticles* pParts,Particle& p)
  237. {
  238. if(p.bDeleted) return;
  239. //p.pose_w = exp(-(0.5*pow((p.pose.pos.x - pParts->obj.center.pos.x),2)/(2*MEASURE_POSE_ERROR*MEASURE_POSE_ERROR)+ pow((p.pose.pos.y - pParts->obj.center.pos.y),2)/(2*MEASURE_POSE_ERROR*MEASURE_POSE_ERROR)));
  240. p.pose_w = 1.0/hypot(0.5*(p.pose.pos.y - pParts->obj.center.pos.y), 0.5*(p.pose.pos.x - pParts->obj.center.pos.x));
  241. //p.dir_w = exp(-(pow(fabs(UtilityHNS::UtilityH::AngleBetweenTwoAnglesPositive(p.pose.pos.a, pParts->obj.center.pos.a)),2)/(2*MEASURE_ANGLE_ERROR*MEASURE_ANGLE_ERROR)));
  242. p.dir_w = M_PI_2 - fabs(UtilityHNS::UtilityH::AngleBetweenTwoAnglesPositive(p.pose.pos.a, pParts->obj.center.pos.a));
  243. p.vel_w = exp(-(pow((p.vel - pParts->obj.center.v),2)/(2*MEASURE_VEL_ERROR*MEASURE_VEL_ERROR)));
  244. //p.vel_w = fabs(p.vel - pParts->obj.center.v);
  245. p.ind_w = CalcIndicatorWeight(FromNumbertoIndicator(p.indicator), pParts->obj.indicator_state);
  246. p.ind_w -= p.ind_w*MEASURE_IND_ERROR;
  247. p.acl_w = CalcAccelerationWeight(p.acc, pParts->obj.acceleration_desc);
  248. //std::cout << p.beh << "|" << p.vel_w <<"|" <<p.vel << "|" << pParts->obj.center.v;
  249. pParts->pose_w_t += p.pose_w;
  250. pParts->dir_w_t += p.dir_w;
  251. pParts->vel_w_t += p.vel_w;
  252. pParts->ind_w_t += p.ind_w;
  253. pParts->acl_w_t += p.acl_w;
  254. if(p.pose_w > pParts->pose_w_max)
  255. pParts->pose_w_max = p.pose_w;
  256. if(p.dir_w > pParts->dir_w_max)
  257. pParts->dir_w_max = p.dir_w;
  258. if(p.vel_w > pParts->vel_w_max)
  259. pParts->vel_w_max = p.vel_w;
  260. if(p.ind_w > pParts->ind_w_max)
  261. pParts->ind_w_max = p.ind_w;
  262. if(p.acl_w > pParts->acl_w_max)
  263. pParts->acl_w_max = p.acl_w;
  264. if(p.pose_w < pParts->pose_w_min)
  265. pParts->pose_w_min = p.pose_w;
  266. if(p.dir_w < pParts->dir_w_min)
  267. pParts->dir_w_min = p.dir_w;
  268. if(p.vel_w < pParts->vel_w_min)
  269. pParts->vel_w_min = p.vel_w;
  270. if(p.ind_w < pParts->ind_w_min)
  271. pParts->ind_w_min = p.ind_w;
  272. if(p.acl_w < pParts->acl_w_min)
  273. pParts->acl_w_min = p.acl_w;
  274. p.w_raw = p.pose_w*POSE_FACTOR + p.dir_w*DIRECTION_FACTOR + p.vel_w*VELOCITY_FACTOR + p.ind_w*INDICATOR_FACTOR + p.acl_w*ACCELERATE_FACTOR;
  275. if(p.w_raw >= pParts->max_w_raw)
  276. pParts->max_w_raw = p.w_raw;
  277. if(p.w_raw <= pParts->min_w_raw)
  278. pParts->min_w_raw = p.w_raw;
  279. }
  280. void BehaviorPrediction::NormalizeOnePartWeight(ObjParticles* pParts,Particle& p)
  281. {
  282. if(p.bDeleted) return;
  283. double pose_diff = pParts->pose_w_max-pParts->pose_w_min;
  284. double dir_diff = pParts->dir_w_max-pParts->dir_w_min;
  285. double vel_diff = pParts->vel_w_max-pParts->vel_w_min;
  286. double ind_diff = pParts->ind_w_max-pParts->ind_w_min;
  287. double acl_diff = pParts->acl_w_max-pParts->acl_w_min;
  288. double epsilon = 0.05;
  289. if(fabs(pose_diff) > epsilon)
  290. p.pose_w = p.pose_w/pose_diff;
  291. else
  292. p.pose_w = 0;
  293. if(p.pose_w > 1.0 ) p.pose_w = 1.0;
  294. if(fabs(dir_diff) > epsilon)
  295. p.dir_w = (p.dir_w - pParts->dir_w_min)/dir_diff;
  296. else
  297. p.dir_w = 0;
  298. if(p.dir_w > 1.0 ) p.dir_w = 1.0;
  299. if(fabs(vel_diff) > epsilon)
  300. p.vel_w = (p.vel_w-pParts->vel_w_min)/vel_diff;
  301. else
  302. p.vel_w = 0;
  303. if(p.vel_w > 1.0) p.vel_w = 1.0;
  304. if(fabs(ind_diff) > epsilon)
  305. p.ind_w = (p.ind_w - pParts->ind_w_min)/ind_diff;
  306. else
  307. p.ind_w = 0;
  308. if(p.ind_w > 1.0) p.ind_w = 1.0;
  309. if(fabs(acl_diff) > epsilon)
  310. p.acl_w = (p.acl_w - pParts->acl_w_min)/acl_diff;
  311. else
  312. p.acl_w = 0;
  313. if(p.acl_w > 1.0) p.acl_w = 1.0;
  314. p.w = p.pose_w*POSE_FACTOR + p.dir_w*DIRECTION_FACTOR + p.vel_w*VELOCITY_FACTOR + p.ind_w*INDICATOR_FACTOR + p.acl_w*ACCELERATE_FACTOR;
  315. //p.w = p.pose_w*0.1 + p.vel_w*0.2 + p.dir_w * 0.2 + p.ind_w + 0.5;
  316. if(p.w >= pParts->max_w)
  317. pParts->max_w = p.w;
  318. if(p.w <= pParts->min_w)
  319. pParts->min_w = p.w;
  320. pParts->all_w += p.w;
  321. }
  322. void BehaviorPrediction::CalculateWeights(ObjParticles* pParts)
  323. {
  324. pParts->all_w = 0;
  325. pParts->pose_w_t = 0;
  326. pParts->dir_w_t = 0;
  327. pParts->vel_w_t = 0;
  328. pParts->acl_w_t = 0;
  329. pParts->pose_w_max = -99999999;
  330. pParts->dir_w_max = -99999999;
  331. pParts->vel_w_max = -99999999;
  332. pParts->acl_w_max = -99999999;
  333. pParts->pose_w_min = 99999999;
  334. pParts->dir_w_min = 99999999;
  335. pParts->vel_w_min = 99999999;
  336. pParts->acl_w_min = 99999999;
  337. pParts->max_w_raw = DBL_MIN;
  338. pParts->min_w_raw = DBL_MAX;
  339. //std::cout << "Acceleration Diff: ";
  340. for(unsigned int i = 0 ; i < pParts->m_AllParticles.size(); i++)
  341. {
  342. // std::cout << "(" << pParts->m_AllParticles.at(i)->pTraj->index <<",";
  343. CalOnePartWeight(pParts, *pParts->m_AllParticles.at(i));
  344. //std::cout << ")";
  345. }
  346. //std::cout << "Befor Normalize: Max: " << pParts->acl_w_max << ", Min: " << pParts->acl_w_min << std::endl;
  347. //std::cout << std::endl;
  348. //if((pParts->m_TrajectoryTracker.size() > 1 && pParts->min_w_raw < 0.5) || pParts->max_w_raw == 0 || fabs(pParts->max_w_raw - pParts->min_w_raw) < 0.1 )
  349. if((pParts->max_w_raw == 0 || fabs(pParts->max_w_raw - pParts->min_w_raw) < 0.1 || pParts->min_w_raw > 0.5) && pParts->m_TrajectoryTracker.size() > 1)
  350. m_bCanDecide = false;
  351. else
  352. m_bCanDecide = true;
  353. //Normalize
  354. pParts->max_w = -9999999;
  355. pParts->min_w = 9999999;
  356. pParts->all_w = 0;
  357. for(unsigned int i = 0 ; i < pParts->m_AllParticles.size(); i++)
  358. {
  359. NormalizeOnePartWeight(pParts, *pParts->m_AllParticles.at(i));
  360. }
  361. }
  362. void BehaviorPrediction::CalculateAveragesAndProbabilities(ObjParticles* pParts)
  363. {
  364. for(unsigned int t=0; t < pParts->m_TrajectoryTracker.size(); t++)
  365. {
  366. pParts->m_TrajectoryTracker.at(t)->CalcAverages();
  367. pParts->m_TrajectoryTracker.at(t)->CalcProbabilities();
  368. }
  369. }
  370. void BehaviorPrediction::RemoveWeakParticles(ObjParticles* pParts)
  371. {
  372. double critical_val = pParts->min_w + (pParts->max_w - pParts->min_w)*KEEP_PERCENTAGE;
  373. // std::cout << "Behavior Weights ------------------------------------------------ " << std::endl;
  374. // std::cout << "Max Raw: " << pParts->max_w_raw << ", Min Raw: " << pParts->min_w_raw << std::endl;
  375. // std::cout << "Keep Percent Value: " << critical_val << std::endl;
  376. //
  377. // if(pParts->obj.acceleration > 0 )
  378. // std::cout << "Accelerate vrooom vroom : " << std::endl;
  379. // else if(pParts->obj.acceleration < 0 )
  380. // std::cout << "Brake Eeeeee Eeeeeeee: " << std::endl;
  381. for(int i =0 ; i < pParts->m_AllParticles.size(); i++)
  382. {
  383. //also delete far particle
  384. double d = hypot(pParts->obj.center.pos.y - pParts->m_AllParticles.at(i)->pose.pos.y, pParts->obj.center.pos.x - pParts->m_AllParticles.at(i)->pose.pos.x);
  385. if(pParts->m_AllParticles.at(i)->w < critical_val || d > m_PredictionDistance)
  386. {
  387. pParts->m_AllParticles.at(i)->pTraj->DeleteParticle(*(pParts->m_AllParticles.at(i)), pParts->m_AllParticles.at(i)->original_index);
  388. pParts->m_AllParticles.erase(pParts->m_AllParticles.begin()+i);
  389. i--;
  390. }
  391. }
  392. }
  393. void BehaviorPrediction::FindBest(ObjParticles* pParts)
  394. {
  395. if(pParts->m_TrajectoryTracker.size() > 0)
  396. {
  397. pParts->best_beh_track = pParts->m_TrajectoryTracker.at(0);
  398. pParts->i_best_track = 0;
  399. }
  400. for(unsigned int t = 1; t < pParts->m_TrajectoryTracker.size(); t++)
  401. {
  402. if(pParts->m_TrajectoryTracker.at(t)->best_p > pParts->best_beh_track->best_p)
  403. {
  404. pParts->best_beh_track = pParts->m_TrajectoryTracker.at(t);
  405. pParts->i_best_track = t;
  406. }
  407. }
  408. if(m_bDebugOut )
  409. {
  410. std::cout << "Behavior Prob ------------------------------------------------ : " << pParts->m_TrajectoryTracker.size() << std::endl;
  411. std::cout << "Raw Weights: Max: " << pParts->max_w_raw << ", Min: " << pParts->min_w_raw << std::endl;
  412. std::cout << "Norm Weights: Max: " << pParts->max_w << ", Min: " << pParts->min_w << std::endl;
  413. }
  414. for(unsigned int t=0; t < pParts->obj.predTrajectories.size() ; t++)
  415. {
  416. if(pParts->obj.predTrajectories.at(t).size()>0)
  417. {
  418. pParts->obj.predTrajectories.at(t).at(0).collisionCost = 0.25;
  419. }
  420. }
  421. if(m_bCanDecide && pParts->best_beh_track != nullptr)
  422. {
  423. std::string str_beh = "Unknown";
  424. if(pParts->best_beh_track->best_beh == BEH_STOPPING_STATE)
  425. str_beh = "Stopping";
  426. else if(pParts->best_beh_track->best_beh == BEH_FORWARD_STATE)
  427. str_beh = "Forward";
  428. if(m_bDebugOut )
  429. std::cout << "Trajectory (" << pParts->i_best_track << "), P: " << pParts->best_beh_track->best_p << " , Beh (" << pParts->best_beh_track->best_beh << ", " << str_beh << ")" << std::endl;
  430. if(pParts->best_beh_track->index < pParts->obj.predTrajectories.size())
  431. pParts->obj.predTrajectories.at(pParts->best_beh_track->index).at(0).collisionCost = 1;
  432. }
  433. else
  434. {
  435. if(m_bDebugOut )
  436. std::cout << "Trajectory (" << -1 << "), P: " << 0 << " , Beh (" << -1 << ", " << "Can't Decide" << ")" << std::endl;
  437. }
  438. if(m_bDebugOut )
  439. {
  440. for(unsigned int t=0; t < pParts->m_TrajectoryTracker.size() ; t++)
  441. {
  442. if(pParts->m_TrajectoryTracker.at(t)->nAliveForward > 0)
  443. std::cout << t << ", Forward Particles:" << pParts->m_TrajectoryTracker.at(t)->nAliveForward << std::endl;
  444. if(pParts->m_TrajectoryTracker.at(t)->nAliveStop > 0)
  445. std::cout << t << ", Stoping Particles:" << pParts->m_TrajectoryTracker.at(t)->nAliveStop << std::endl;
  446. }
  447. std::cout << "------------------------------------------------ --------------" << std::endl<< std::endl;
  448. }
  449. }
  450. void BehaviorPrediction::SamplesFreshParticles(ObjParticles* pParts)
  451. {
  452. timespec _time;
  453. UtilityHNS::UtilityH::GetTickCount(_time);
  454. srand(_time.tv_nsec);
  455. ENG eng(_time.tv_nsec);
  456. NormalDIST dist_x(0, MOTION_POSE_ERROR);
  457. VariatGEN gen_x(eng, dist_x);
  458. NormalDIST vel(MOTION_VEL_ERROR, MOTION_VEL_ERROR);
  459. VariatGEN gen_v(eng, vel);
  460. NormalDIST ang(0, MOTION_ANGLE_ERROR);
  461. VariatGEN gen_a(eng, ang);
  462. // NormalDIST acl(0, MEASURE_ACL_ERROR);
  463. // VariatGEN gen_acl(eng, acl);
  464. Particle p;
  465. p.pose = pParts->obj.center;
  466. p.vel = 0;
  467. p.acc = 0;
  468. p.indicator = 0;
  469. bool bRegenerate = true;
  470. if(UtilityHNS::UtilityH::GetTimeDiffNow(m_GenerationTimer) > 2)
  471. {
  472. UtilityHNS::UtilityH::GetTickCount(m_GenerationTimer);
  473. bRegenerate = true;
  474. }
  475. // for(unsigned int t=0; t < pParts->m_TrajectoryTracker.size(); t++)
  476. // {
  477. // PlanningHelpers::FixPathDensity(pParts->m_TrajectoryTracker.at(t)->trajectory, 0.5);
  478. // PlanningHelpers::CalcAngleAndCost(pParts->m_TrajectoryTracker.at(t)->trajectory);
  479. // }
  480. for(unsigned int t=0; t < pParts->m_TrajectoryTracker.size(); t++)
  481. {
  482. RelativeInfo info;
  483. PlanningHelpers::GetRelativeInfo(pParts->m_TrajectoryTracker.at(t)->trajectory, pParts->obj.center, info);
  484. unsigned int point_index = 0;
  485. p.pose = PlanningHelpers::GetFollowPointOnTrajectory(pParts->m_TrajectoryTracker.at(t)->trajectory, info, PREDICTION_DISTANCE_PERCENTAGE*m_PredictionDistance, point_index);
  486. if(pParts->m_TrajectoryTracker.at(t)->beh == PlannerHNS::BEH_FORWARD_STATE && pParts->m_TrajectoryTracker.at(t)->nAliveForward < BEH_PARTICLES_NUM)
  487. {
  488. p.beh = PlannerHNS::BEH_FORWARD_STATE;
  489. int nPs = BEH_PARTICLES_NUM - pParts->m_TrajectoryTracker.at(t)->nAliveForward;
  490. for(unsigned int i=0; i < nPs; i++)
  491. {
  492. Particle p_new = p;
  493. p_new.pose.pos.x += gen_x();
  494. p_new.pose.pos.y += gen_x();
  495. p_new.pose.pos.a += gen_a();
  496. p_new.vel = pParts->obj.center.v + fabs(gen_v());
  497. p_new.pose.v = p_new.vel;
  498. pParts->m_TrajectoryTracker.at(t)->InsertNewParticle(p_new);
  499. }
  500. }
  501. if(ENABLE_STOP_BEHAVIOR_GEN == 1 && pParts->m_TrajectoryTracker.at(t)->nAliveStop < BEH_PARTICLES_NUM)
  502. {
  503. p.beh = PlannerHNS::BEH_STOPPING_STATE;
  504. int nPs = BEH_PARTICLES_NUM - pParts->m_TrajectoryTracker.at(t)->nAliveStop;
  505. for(unsigned int i=0; i < nPs; i++)
  506. {
  507. Particle p_new = p;
  508. p_new.pose.pos.x += gen_x();
  509. p_new.pose.pos.y += gen_x();
  510. p_new.pose.pos.a += gen_a();
  511. p_new.vel = 0;
  512. p_new.pose.v = pParts->obj.center.v + fabs(gen_v());
  513. pParts->m_TrajectoryTracker.at(t)->InsertNewParticle(p_new);
  514. }
  515. }
  516. }
  517. }
  518. void BehaviorPrediction::CollectParticles(ObjParticles* pParts)
  519. {
  520. pParts->m_AllParticles.clear();
  521. for(unsigned int t=0; t < pParts->m_TrajectoryTracker.size(); t++)
  522. {
  523. for(unsigned int i=0; i < pParts->m_TrajectoryTracker.at(t)->m_ForwardPart.size(); i++)
  524. {
  525. pParts->m_TrajectoryTracker.at(t)->m_ForwardPart.at(i).original_index = i;
  526. pParts->m_AllParticles.push_back(&pParts->m_TrajectoryTracker.at(t)->m_ForwardPart.at(i));
  527. }
  528. for(unsigned int i=0; i < pParts->m_TrajectoryTracker.at(t)->m_StopPart.size(); i++)
  529. {
  530. pParts->m_TrajectoryTracker.at(t)->m_StopPart.at(i).original_index = i;
  531. pParts->m_AllParticles.push_back(&pParts->m_TrajectoryTracker.at(t)->m_StopPart.at(i));
  532. }
  533. }
  534. }
  535. void BehaviorPrediction::MoveParticles(ObjParticles* pParts)
  536. {
  537. double dt = 0.01;
  538. if(m_bStepByStep)
  539. {
  540. dt = 0.08;
  541. }
  542. else
  543. {
  544. dt = UtilityHNS::UtilityH::GetTimeDiffNow(m_ResamplingTimer);
  545. UtilityHNS::UtilityH::GetTickCount(m_ResamplingTimer);
  546. if(m_bFirstMove)
  547. {
  548. m_bFirstMove = false;
  549. return;
  550. }
  551. }
  552. PlannerHNS::BehaviorState curr_behavior;
  553. PlannerHNS::ParticleInfo curr_part_info;
  554. PlannerHNS::VehicleState control_u;
  555. PassiveDecisionMaker decision_make;
  556. PlannerHNS::CAR_BASIC_INFO carInfo;
  557. carInfo.width = pParts->obj.w;
  558. carInfo.length = pParts->obj.l;
  559. carInfo.max_acceleration = 3.0;
  560. carInfo.max_deceleration = -3;
  561. carInfo.max_speed_forward = MAX_PREDICTION_SPEED;
  562. carInfo.min_speed_forward = 0;
  563. carInfo.max_steer_angle = 0.4;
  564. carInfo.min_steer_angle = -0.4;
  565. carInfo.turning_radius = 7.2;
  566. carInfo.wheel_base = carInfo.length*0.75;
  567. for(unsigned int t=0; t < pParts->m_TrajectoryTracker.size(); t++)
  568. {
  569. PlanningHelpers::GenerateRecommendedSpeed(pParts->m_TrajectoryTracker.at(t)->trajectory, carInfo.max_speed_forward, 1.0);
  570. }
  571. // std::cout << "Motion Status------ " << std::endl;
  572. // if(pParts->obj.acceleration_desc == 1)
  573. // std::cout << "Acceleration: " << pParts->obj.acceleration_raw << ", Accelerate: " << pParts->obj.acceleration_desc << std::endl;
  574. // else if(pParts->obj.acceleration_desc == -1)
  575. // std::cout << "Acceleration: " << pParts->obj.acceleration_raw << ", Braking : " << pParts->obj.acceleration_desc << std::endl;
  576. // else
  577. // std::cout << "Acceleration: " << pParts->obj.acceleration_raw << ", Cruising : " << pParts->obj.acceleration_desc << std::endl;
  578. for(unsigned int i=0; i < pParts->m_AllParticles.size(); i++)
  579. {
  580. Particle* p = pParts->m_AllParticles.at(i);
  581. if(USE_OPEN_PLANNER_MOVE == 0)
  582. {
  583. p->pose.v = pParts->obj.center.v;
  584. curr_part_info = decision_make.MoveStepSimple(dt, p->pose, p->pTraj->trajectory,carInfo);
  585. if(p->prev_time_diff > ACCELERATION_CALC_TIME)
  586. {
  587. p->acc_raw = (curr_part_info.vel - p->vel_prev_big)/p->prev_time_diff;
  588. p->vel_prev_big = curr_part_info.vel;
  589. p->prev_time_diff = 0;
  590. }
  591. else
  592. {
  593. p->prev_time_diff += dt;
  594. }
  595. if(fabs(p->acc_raw) < ACCELERATION_DECISION_VALUE)
  596. p->acc = 0;
  597. else if(p->acc_raw > ACCELERATION_DECISION_VALUE)
  598. p->acc = 1;
  599. else if(p->acc_raw < -ACCELERATION_DECISION_VALUE)
  600. p->acc = -1;
  601. p->indicator = FromIndicatorToNumber(curr_part_info.indicator);
  602. // if(curr_behavior.state == PlannerHNS::STOPPING_STATE && p->beh == PlannerHNS::BEH_YIELDING_STATE)
  603. // p->vel += 1;
  604. // else if(p->beh == PlannerHNS::BEH_YIELDING_STATE)
  605. // p->vel = p->vel/2.0;
  606. // else if(curr_behavior.state != PlannerHNS::STOPPING_STATE && p->beh == PlannerHNS::BEH_STOPPING_STATE)
  607. // p->vel += 1;
  608. // else if(p->beh == PlannerHNS::BEH_STOPPING_STATE)
  609. // p->vel = 0;
  610. // if(curr_behavior.state != PlannerHNS::STOPPING_STATE && p->beh == PlannerHNS::BEH_STOPPING_STATE)
  611. // p->vel += 1;
  612. if(p->beh == PlannerHNS::BEH_STOPPING_STATE)
  613. {
  614. p->vel = 0;
  615. if(p->acc == 0)
  616. p->acc = -1;
  617. else if(p->acc == 1)
  618. p->acc = 0;
  619. }
  620. }
  621. else
  622. {
  623. curr_behavior = decision_make.MoveStep(dt, p->pose, p->pTraj->trajectory,carInfo);
  624. p->acc = UtilityHNS::UtilityH::GetSign(curr_behavior.maxVelocity - p->vel_prev_big);
  625. p->vel = curr_behavior.maxVelocity;
  626. if(fabs(p->vel - p->vel_prev_big) > 0.5)
  627. p->vel_prev_big = p->vel;
  628. p->indicator = FromIndicatorToNumber(curr_behavior.indicator);
  629. // if(p->indicator == 0)
  630. // std::cout << ", Off";
  631. // else if(p->indicator == 1)
  632. // std::cout << ", Left";
  633. // else if(p->indicator == 2)
  634. // std::cout << ", Right";
  635. if(curr_behavior.state == PlannerHNS::STOPPING_STATE && p->beh == PlannerHNS::BEH_YIELDING_STATE)
  636. p->vel += 1;
  637. else if(p->beh == PlannerHNS::BEH_YIELDING_STATE)
  638. p->vel = p->vel/2.0;
  639. else if(curr_behavior.state != PlannerHNS::STOPPING_STATE && p->beh == PlannerHNS::BEH_STOPPING_STATE)
  640. p->vel += 1;
  641. else if(p->beh == PlannerHNS::BEH_STOPPING_STATE)
  642. {
  643. p->vel = 0;
  644. }
  645. }
  646. }
  647. //std::cout << "End Motion Status ------ " << std::endl;
  648. }
  649. } /* namespace PlannerHNS */