20 #include <adore/fun/nlpqlp_planner/colors.h>
105 sort(egoStates,list);
133 double marginTau = 1.;
159 std::vector<double> s;
164 double x_ego_front_side, y_ego_front_side;
165 double x_ego_rear_side, y_ego_rear_side;
173 for(
int i=0; i<list.size(); i++)
179 if(list[i].currentTrajectory.x.size() > 1)
182 double x = list[i].currentTrajectory.x[0];
183 double y = list[i].currentTrajectory.y[0];
190 double *s_sorted =
new double [s.size()];
191 int *index =
new int [s.size()];
195 for(
int i=0; i<s.size(); i++)
198 if(s_sorted[i]<ego_s)
200 tmp.
MCM = list[index[i]];
219 tmp.
MCM = list[index[i]];
295 double tieBreaker = 0.5;
296 double safeTau = 3.0 - tieBreaker;
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
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