ADORe
ADORe is a modular open source software library and toolkit for decision making, planning, control and simulation of automated vehicles
cooperativeusersprocess.h
Go to the documentation of this file.
1 /********************************************************************************
2  * Copyright (C) 2017-2020 German Aerospace Center (DLR).
3  * Eclipse ADORe, Automated Driving Open Research https://eclipse.org/adore
4  *
5  * This program and the accompanying materials are made available under the
6  * terms of the Eclipse Public License 2.0 which is available at
7  * http://www.eclipse.org/legal/epl-2.0.
8  *
9  * SPDX-License-Identifier: EPL-2.0
10  *
11  * Contributors:
12  * Reza Dariani- initial API and implementation
13  ********************************************************************************/
14 #pragma once
20 #include <adore/fun/nlpqlp_planner/colors.h>
21 namespace adore
22 {
23  namespace env
24  {
25 
27  {
28  private:
32  {
35  bool inPlatooningRange = false;
38  };
39  typedef std::vector<CooperativeUser> ListOfCooperativeUsers;
40 
41 
60 
61  public:
62 
69  bool leaderExists() {return leader_exist;}
77  double getLeaderTimeHeadway() {return leader_tau;}
83  {
84  this->three_lanes_ = three_lanes_;
85  this->pveh = pveh;
88  this->possiblePlatooningTimeHeadwayRange = possiblePlatooningTimeHeadwayRange;
89  platooningDistance = 50.;
90  this->DEBUG_LEVEL = DEBUG_LEVEL;
91 
92 
93  }
95  {
96  delete [] three_lanes_;
97 
98  }
100  {
101  preceding_exist = false;
102  leader_exist = false;
103  following_exist = false;
104  precedingIsLeader = false;
105  sort(egoStates,list);
106  compare(frontMan,behindMan);
107 
108 
109 
110  }
111  private:
112  // void addAcceleration(CooperativeUser user)
113  // {
114  // std::vector<double> w;
115  // for(int i=0; i<user.MCM.currentTrajectory.v.size(); i++) w.push_back(1.);
116  // double *v = new double [user.MCM.currentTrajectory.v.size()];
117  // double *a = new double [user.MCM.currentTrajectory.v.size()];
118  // adore::mad::CubicPiecewiseFunction::fit(&pp_v, &user.MCM.currentTrajectory.t0[0],&user.MCM.currentTrajectory.v[0],&w[0],user.MCM.currentTrajectory.v.size(), 0.9);
119  // adore::mad::CubicPiecewiseFunction::CubicSplineEvaluation(&v[0],&a[0],&user.MCM.currentTrajectory.t0[0],user.MCM.currentTrajectory.t0.size(),pp_v);
120  // for(int i=0; i<user.MCM.currentTrajectory.v.size(); i++)
121  // {
122  // user.MCM.currentTrajectory.a.push_back(a[i]);
123  // }
124  // std::cout<<"\n"<<a[0]<<"\t"<<a[1];
125  // delete [] a;
126  // delete [] v;
127 
128  // }
129  //check is cooperative preceding is the same as front man detected by ego sensor
131  {
132  //preceding_progress = 0.;
133  double marginTau = 1.;
134  frontManIsCooperative = false;
135  followingIsCooperative = false;
136  double s_fr = frontMan.getCurrentProgress();
137  double s_bm = behindMan.getCurrentProgress();
138  // std::cout<<"\n"<<s_fr <<"\t"<< preceding.progress_rear_side;
139  if(std::abs(s_fr - preceding.progress_rear_side) < std::max(frontMan.getCurrentSpeed()*marginTau,5.))
140  {
141  frontManIsCooperative = true;
142  }
143 
144  if(std::abs(s_bm - following.progress_front_side) < std::max(behindMan.getCurrentSpeed()*marginTau,5.))
145  {
146  followingIsCooperative = true;
147  }
148  }
149 
150 
152  {
153  Cooperative_behindMans.clear();
154  Cooperative_frontMans.clear();
155  preceding.MCM.clear();
156  leader.MCM.clear();
157  following.MCM.clear();
158  CooperativeUser tmp;
159  std::vector<double> s;
160  if(three_lanes_->getCurrentLane()->isValid() && list.size())
161  {
162  double ego_n, ego_s;
163  three_lanes_->getCurrentLane()->toRelativeCoordinates(egoStates.getX() ,egoStates.getY() ,ego_s,ego_n);
164  double x_ego_front_side, y_ego_front_side;
165  double x_ego_rear_side, y_ego_rear_side;
166  x_ego_front_side = egoStates.getX() + std::cos(egoStates.getPSI())*(pveh->get_a()+pveh->get_b()+pveh->get_c()) ;
167  y_ego_front_side = egoStates.getY() + std::sin(egoStates.getPSI())*(pveh->get_a()+pveh->get_b()+pveh->get_c()) ;
168  three_lanes_->getCurrentLane()->toRelativeCoordinates(x_ego_front_side,y_ego_front_side,ego_progress_front_side,ego_n);
169 
170  x_ego_rear_side = egoStates.getX() + std::cos(egoStates.getPSI())*(-pveh->get_d()) ;
171  y_ego_rear_side = egoStates.getY() + std::sin(egoStates.getPSI())*(-pveh->get_d()) ;
172  three_lanes_->getCurrentLane()->toRelativeCoordinates(x_ego_front_side,y_ego_front_side,ego_progress_rear_side,ego_n); for(int i=0; i<list.size(); i++)
173  for(int i=0; i<list.size(); i++)
174  {
175 
176  //TO DO : check if they are in the same lane as ego
177  //TO DO: if data is still valid
178 
179  if(list[i].currentTrajectory.x.size() > 1)
180  {
181  double _s, _n;
182  double x = list[i].currentTrajectory.x[0];
183  double y = list[i].currentTrajectory.y[0];
185  s.push_back(_s);
186  }
187  }
188  if(s.size())
189  {
190  double *s_sorted = new double [s.size()];
191  int *index = new int [s.size()];
192  adore::mad::ArrayMatrixTools::sort(&s_sorted[0],&index[0],&s[0],s.size());
193 
194 
195  for(int i=0; i<s.size(); i++)
196  {
197 
198  if(s_sorted[i]<ego_s)
199  {
200  tmp.MCM = list[index[i]];
201  //addAcceleration(tmp);
202  tmp.MCM.communicationDelay = egoStates.getTime() - tmp.MCM.currentTrajectory.t0[0];
203  tmp.progress_front_side = s_sorted[i]; //MCM is based on front side
204  double x, y, _n;
205  x = tmp.MCM.currentTrajectory.x[0];
206  y = tmp.MCM.currentTrajectory.y[0];
208  //processIntention(&tmp);
210 
211  Cooperative_behindMans.push_back(tmp);
212  tmp.MCM.clear();
213  //std::cout<<"\nbehindMans: "<<Cooperative_behindMans.back().MCM.id<<"\t"<<Cooperative_behindMans.back().MCM.currentTrajectory.x[0]<<"\t"<<Cooperative_behindMans.back().MCM.currentTrajectory.y[0];
214 
215  }
216  else
217  {
218 
219  tmp.MCM = list[index[i]];
220  // addAcceleration(tmp);
221  tmp.MCM.communicationDelay = egoStates.getTime() - tmp.MCM.currentTrajectory.t0[0];
222  tmp.progress_front_side = s_sorted[i]; //MCM is based on front side
223  double x, y, _n;
224  x = tmp.MCM.currentTrajectory.x[0];
225  y = tmp.MCM.currentTrajectory.y[0];
226 
227  //std::cout<<"\n"<<tmp.MCM.currentTrajectory.psi[0]<<"\t"<<tmp.MCM.vehicleLength ;
228 
230 
231  //processIntention(&tmp);
233 
234  //processIntention(&tmp);
235  Cooperative_frontMans.push_back(tmp);
236  //std::cout<<"\nfrontMans: "<<Cooperative_frontMans.back().MCM.id<<"\t"<<Cooperative_frontMans.back().MCM.currentTrajectory.x[0]<<"\t"<<Cooperative_frontMans.back().MCM.currentTrajectory.y[0];
237 
238  }
239 
240  }
241  delete [] s_sorted;
242  delete [] index;
243  if(Cooperative_frontMans.size())
244  {
245  preceding_exist = true;
248  preceding_tau = (preceding_dist_to_ego)/std::max(egoStates.getvx(),0.10);
252  if(Cooperative_frontMans.size()==1)
253  {
254  leader = preceding;
255  leader_exist = true;
256  precedingIsLeader = true;
259 
260 
261 
262 
263  }
264  else
265  {
266 
267  leader = Cooperative_frontMans.back();
268  leader_exist = true;
270  leader_tau = (leader_dist_to_ego)/std::max(egoStates.getvx(),0.10);
271  leader.inPlatooningRange = false;
274  }
275  }
276  if(Cooperative_behindMans.size())
277  {
278  following_exist = true;
285  }
286  }
287 
288  }
289  //std::cout<<"\n-*-*-*-*-*-*LANE IS NOT VALID";
290 
291  }
293  {
294  print_debug(cu);
295  double tieBreaker = 0.5;
296  double safeTau = 3.0 - tieBreaker; //safe time headway
297  cu->PlatooningState = logic::NotAble; //initialization
298  if(cu->MCM.currentTrajectory.x.size() == 0) //No trajectory included
299  {
301  // std::cout<<"\n"<<cu->MCM.id<<"------------------------NOT ABLE";
302  }
303  else
304  {
305  double safeDistance = safeTau*std::max(cu->MCM.currentTrajectory.v[0],3.);
306 
308  cu->MCM.toletated_distance_ahead <= (safeDistance) &&
309  cu->MCM.toletated_distance_behind <= (safeDistance))
310  {
312  // std::cout<<"\n"<<cu->MCM.id<<"------------------------WANT TO FORM";
313 
314 
315  }
317  cu->MCM.toletated_distance_ahead > (safeDistance) &&
318  cu->MCM.toletated_distance_behind > (safeDistance))
319  {
320  std::cout<<"\n"<<cu->MCM.toletated_distance_ahead<<"\t"<<cu->MCM.toletated_distance_behind <<"\t"<<safeDistance<<"\t"<<cu->MCM.currentTrajectory.v[0];
322  // std::cout<<"\n"<<cu->MCM.id<<"------------------------WANT TO FORM";
323 
324 
325  }
326  }
327 
328  }
330  {
331  if(cu->MCM.currentTrajectory.x.size() == 0) BOLD(FRED("MCM.NoTrajectory"));
332  std::cout<<"\n"<< BOLD(FRED("MCM.toletated_distance_ahead: "))<<cu->MCM.toletated_distance_ahead ;
333  std::cout<<"\n"<<BOLD(FRED("MCM.toletated_distance_behind: "))<<cu->MCM.toletated_distance_behind ;
334  std::cout<<"\n"<<BOLD(FRED("MCM.target_automation_level: "))<<unsigned(cu->MCM.target_automation_level) ;
335  //printf("\n*****%i",(cu->MCM.target_automation_level) );
336 
337  }
338 
340  {
341  //std::cout<<"\n"<<DEBUG_LEVEL;
342  if(DEBUG_LEVEL)
343  {
345 
346  }
347 
348  }
349  void toCenterOfTheRearSide(double &x, double &y, double psi, double vehicleLength)
350  {
351  x = x + std::cos(psi)*(-vehicleLength) ;
352  y = y + std::sin(psi)*(-vehicleLength) ;
353  }
354 
355 
356  };
357  }
358 }
Definition: cooperativeusersprocess.h:27
void intentionPredictionPrint(CooperativeUser *cu)
Definition: cooperativeusersprocess.h:329
bool precedingExists()
Definition: cooperativeusersprocess.h:68
bool followingIsCooperative
Definition: cooperativeusersprocess.h:48
double leader_tau
Definition: cooperativeusersprocess.h:55
double getEgoProgressRearSide()
Definition: cooperativeusersprocess.h:76
double following_dist_to_ego
Definition: cooperativeusersprocess.h:53
~CooperativeUsersProcess()
Definition: cooperativeusersprocess.h:94
bool following_exist
Definition: cooperativeusersprocess.h:47
CooperativeUser getFollowing()
Definition: cooperativeusersprocess.h:67
std::vector< CooperativeUser > ListOfCooperativeUsers
Definition: cooperativeusersprocess.h:39
double getEgoProgressFrontSide()
Definition: cooperativeusersprocess.h:75
double leader_dist_to_ego
Definition: cooperativeusersprocess.h:54
CooperativeUser leader
Definition: cooperativeusersprocess.h:45
void toCenterOfTheRearSide(double &x, double &y, double psi, double vehicleLength)
Definition: cooperativeusersprocess.h:349
ListOfCooperativeUsers getFrontMansList()
Definition: cooperativeusersprocess.h:63
CooperativeUsersProcess(adore::env::ThreeLaneViewDecoupled *three_lanes_, adore::params::APVehicle *pveh, int DEBUG_LEVEL=0, double possiblePlatooningTimeHeadwayRange=6.)
Definition: cooperativeusersprocess.h:82
bool precedingIsLeader
Definition: cooperativeusersprocess.h:47
double getLeaderTimeHeadway()
Definition: cooperativeusersprocess.h:77
double platooningDistance
Definition: cooperativeusersprocess.h:58
void sort(adore::fun::VehicleMotionState9d &egoStates, CooperativeUsersList &list)
Definition: cooperativeusersprocess.h:151
adore::mad::CubicPiecewiseFunction::PieceweisePolynomial pp_v
Definition: cooperativeusersprocess.h:56
bool noVehicleBetweenEgoAndFollowing()
Definition: cooperativeusersprocess.h:81
void print_debug(CooperativeUser *cu)
Definition: cooperativeusersprocess.h:339
adore::params::APVehicle * pveh
Definition: cooperativeusersprocess.h:29
ListOfCooperativeUsers Cooperative_frontMans
Definition: cooperativeusersprocess.h:42
bool preceding_exist
Definition: cooperativeusersprocess.h:47
double possiblePlatooningTimeHeadwayRange
Definition: cooperativeusersprocess.h:57
double getDistanceToFollowing()
Definition: cooperativeusersprocess.h:74
ListOfCooperativeUsers Cooperative_behindMans
Definition: cooperativeusersprocess.h:43
void process(adore::fun::VehicleMotionState9d &egoStates, CooperativeUsersList &list, adore::view::TrafficObject &frontMan, adore::view::TrafficObject &behindMan)
Definition: cooperativeusersprocess.h:99
double ego_progress_front_side
Definition: cooperativeusersprocess.h:49
double getDistanceToPreceding()
Definition: cooperativeusersprocess.h:73
CooperativeUser getLeader()
Definition: cooperativeusersprocess.h:66
void processIntention(CooperativeUser *cu)
Definition: cooperativeusersprocess.h:292
double preceding_tau
Definition: cooperativeusersprocess.h:55
double following_tau
Definition: cooperativeusersprocess.h:55
double getDistanceToLeader()
Definition: cooperativeusersprocess.h:72
double preceding_dist_to_ego
Definition: cooperativeusersprocess.h:52
bool frontManIsCooperative
Definition: cooperativeusersprocess.h:48
bool leader_exist
Definition: cooperativeusersprocess.h:47
bool leaderExists()
Definition: cooperativeusersprocess.h:69
int DEBUG_LEVEL
Definition: cooperativeusersprocess.h:59
double getFollowingTimeHeadway()
Definition: cooperativeusersprocess.h:79
double getPrecedingTimeHeadway()
Definition: cooperativeusersprocess.h:78
bool followingExists()
Definition: cooperativeusersprocess.h:70
CooperativeUser getPreceding()
Definition: cooperativeusersprocess.h:65
CooperativeUser preceding
Definition: cooperativeusersprocess.h:44
ListOfCooperativeUsers getBehindMansList()
Definition: cooperativeusersprocess.h:64
bool leaderIsPreceding()
Definition: cooperativeusersprocess.h:71
bool noVehicleBetweenEgoAndPreceding()
Definition: cooperativeusersprocess.h:80
CooperativeUser following
Definition: cooperativeusersprocess.h:46
adore::env::ThreeLaneViewDecoupled * three_lanes_
Definition: cooperativeusersprocess.h:51
void compare(adore::view::TrafficObject &frontMan, adore::view::TrafficObject &behindMan)
Definition: cooperativeusersprocess.h:130
double ego_progress_rear_side
Definition: cooperativeusersprocess.h:50
Definition: threelaneviewdecoupled.h:32
virtual adore::view::ALane * getCurrentLane()
Definition: threelaneviewdecoupled.h:405
static void sort(T *output, int *outputIndex, T *input, int inputSize)
Definition: arraymatrixtools.h:89
abstract class for vehicle configuration related paremeters
Definition: ap_vehicle.h:29
virtual double get_b() const =0
rear axle to cog
virtual double get_d() const =0
rear border to rear axle
virtual double get_a() const =0
cog to front axle
virtual double get_c() const =0
front axle to front border
virtual void toRelativeCoordinates(double xe, double ye, double &s, double &n)=0
virtual bool isValid() const =0
std::vector< CooperativeUserPrediction > CooperativeUsersList
Definition: cooperativeusersprediction.h:70
@ NotAble
Definition: platoonStateMachine.h:42
@ Leaving
Definition: platoonStateMachine.h:42
@ WantToForm
Definition: platoonStateMachine.h:42
@ SAE_LEVEL3
Definition: platoonStateMachine.h:43
interval< T > cos(interval< T > x)
Definition: intervalarithmetic.h:225
interval< T > sin(interval< T > x)
Definition: intervalarithmetic.h:204
adoreMatrix< T, N, M > max(adoreMatrix< T, N, M > a, const adoreMatrix< T, N, M > &b)
Definition: adoremath.h:686
x
Definition: adore_set_goal.py:30
y
Definition: adore_set_goal.py:31
Definition: areaofeffectconverter.h:20
Definition: cooperativeusersprediction.h:24
std::vector< double > v
Definition: cooperativeusersprediction.h:39
struct adore::env::CooperativeUserPrediction::@0 currentTrajectory
std::vector< double > psi
Definition: cooperativeusersprediction.h:42
double toletated_distance_behind
Definition: cooperativeusersprediction.h:28
double vehicleLength
Definition: cooperativeusersprediction.h:32
unsigned int target_automation_level
Definition: cooperativeusersprediction.h:30
double toletated_distance_ahead
Definition: cooperativeusersprediction.h:29
void clear()
Definition: cooperativeusersprediction.h:52
double communicationDelay
Definition: cooperativeusersprediction.h:34
std::vector< double > t0
Definition: cooperativeusersprediction.h:40
std::vector< double > x
Definition: cooperativeusersprediction.h:37
std::vector< double > y
Definition: cooperativeusersprediction.h:38
prediction plus CACC(Platoon)
Definition: cooperativeusersprocess.h:32
CooperativeUserPrediction MCM
Definition: cooperativeusersprocess.h:33
int PlatooningState
Definition: cooperativeusersprocess.h:34
double progress_front_side
Definition: cooperativeusersprocess.h:36
bool inPlatooningRange
Definition: cooperativeusersprocess.h:35
double progress_rear_side
Definition: cooperativeusersprocess.h:37
This struct holds the motion state of the vehicle in 9d.
Definition: vehiclemotionstate9d.h:39
double getX() const
Get the x-coordinate.
Definition: vehiclemotionstate9d.h:54
double getvx() const
Get the longitudinal velocity.
Definition: vehiclemotionstate9d.h:78
double getY() const
Get the y-coordinate.
Definition: vehiclemotionstate9d.h:60
double getPSI() const
Get the heading.
Definition: vehiclemotionstate9d.h:72
double getTime() const
Get the time.
Definition: vehiclemotionstate9d.h:48
Definition: cubicpiecewisefunction.h:51
Definition: trafficobject.h:27
double getCurrentSpeed() const
Definition: trafficobject.h:69
double getCurrentProgress() const
Definition: trafficobject.h:62