110 if (spr->isActive(t))
112 auto x_ref = spr->interpolateReference(t,
pvehicle_);
113 double dx =
x_.
getX() - x_ref.getX();
114 double dy =
x_.
getY() - x_ref.getY();
115 if (dx * dx + dy * dy < pTacticalPlanner_->getResetRadius())
117 x_replan.setX(x_ref.getX());
118 x_replan.setY(x_ref.getY());
119 x_replan.setPSI(x_ref.getPSI());
120 x_replan.setvx(x_ref.getvx());
121 x_replan.setvy(x_ref.getvy());
122 x_replan.setOmega(x_ref.getOmega());
123 x_replan.setAx(x_ref.getAx());
124 x_replan.setDelta(x_ref.getDelta());
130 std::cout <<
"TestTrajectoryPlanner: Reset initial state.\n";
136 for (
auto &obj : olt)
148 for (
auto &obj : oltr)
150 if (obj.getCurrentProgress() < s+35 && obj.getCurrentProgress() > s-2 && obj.getCurrentSpeed() < 0.95 *
lfv_.
getSpeedLimit(s))
158 if (
last_n_ > 0.5 && n < 0.0 || last_n_ < -0.5 && n > 0.0)
236 std::stringstream ss;
237 std::string options_point =
"LineColor=1.0,0.0,0.0;LineWidth=0;LineStyle=none;PointSize=5";
244 std::cout <<
"cornerpoitns size" << cornerpoints->size() << std::endl;
245 double xc[cornerpoints->size()];
246 double yc[cornerpoints->size()];
247 double zc[cornerpoints->size()];
249 for (
auto i = cornerpoints->begin(); i != cornerpoints->end(); ++i)
255 options_point =
"LineColor=1.0,0.0,1.0;LineWidth=0;LineStyle=none;PointSize=6";
256 fig->
plot(
"cornerpoints", xc, yc, 2.0, c, options_point);
257 std::cout <<
"now:conflictset" << std::endl;
260 std::string options_normal =
"LineColor=0.0,1.0,0.0;LineWidth=3";
261 std::string options_highlighted =
"LineColor=1.0,1.0,0.0;LineWidth=6";
262 std::string cz_str =
"cz_subgroup";
263 std::string bo_str =
"bo_number";
266 std::cout <<
"\n plot evaluated borders";
267 for (
auto it = plotdata->begin(); it != plotdata->end(); ++it)
270 std::cout <<
"number of cz:" << plotdata->size() << std::endl;
271 for (
auto ij = it->second.begin(); ij != it->second.end(); ++ij)
273 std::cout <<
" path of this cz" << std::endl;
274 options = (bo_int == 0 ? options_highlighted : options_normal);
283 static const int nplot = 100;
288 int n =
right->m_path->getData().nc();
289 right->m_path->writePointsToArray(0, n - 1, 0, X);
290 right->m_path->writePointsToArray(0, n - 1, 1, Y);
293 int m =
left->m_path->getData().nc();
294 left->m_path->writePointsToArray(0, m - 1, 0, Xn);
295 left->m_path->writePointsToArray(0, m - 1, 1, Yn);
298 for (
int i = 0; i < m; i++)
300 X[n + m - i - 1] = Xn[i];
301 Y[n + m - i - 1] = Yn[i];
306 for (
int i = 0; i < m; i++)
312 figure_->
patch(name +
"/h", X, Y,
z, n + m, options);
314 figure_->
plot(name, X, Y,
z + 0.1, n, options);
322 std::cout <<
"plot border overlaps" << std::endl;
326 for (
auto overlapset = overlapsets->begin(); overlapset != overlapsets->end(); ++overlapset)
330 for (
auto overlap = overlapset->borderoverlaps_.begin(); overlap != overlapset->borderoverlaps_.end(); ++overlap)
333 auto cpv = (*overlap)->getCornerPointVector();
334 count += cpv->size();
335 double x[cpv->size()];
336 double y[cpv->size()];
337 double z[cpv->size()];
339 for (
auto coordinate = cpv->begin(); coordinate != cpv->end(); ++coordinate)
341 x[i] = coordinate->m_X;
342 y[i] = coordinate->m_Y;
345 double red = 1.0 - os / 6.1;
346 double green = os / 6.1;
347 std::string options_point =
"LineColor=" + std::to_string(red) +
"," + std::to_string(green) +
",1.0;LineWidth=0;LineStyle=none;PointSize=11";
348 fig->
plot(
"cornerpointsos" + std::to_string(p) + std::to_string(os),
x,
y, 2.0 + os, i, options_point);
349 std::string optionsy =
"LineColor=1.0,1.0,0.0;LineWidth=2;PointSize=1";
350 std::string options =
"LineColor=1.0,0.0,0.0;LineWidth=2;PointSize=1";
351 std::string optionsg =
"LineColor=0.0,1.0,0.0;LineWidth=2;PointSize=1";
352 plotBorder(
"namelfvl" + std::to_string(p) + std::to_string(os), (*overlap)->m_first_left, 0, 5.0, optionsy, fig);
353 plotBorder(
"namelfvr" + std::to_string(p) + std::to_string(os), (*overlap)->m_first_right, 0, 5.0, optionsy, fig);
354 plotBorder(
"namesl" + std::to_string(p) + std::to_string(os), (*overlap)->m_second_left, 0, 10.0, optionsg, fig);
355 plotBorder(
"namesr" + std::to_string(p) + std::to_string(os), (*overlap)->m_second_right, 0, 10.0, options, fig);
358 std::vector<env::BorderBased::Border *> borderlist;
359 std::vector<int> ids;
361 std::string optionsmagenta =
"LineColor=1.0,0.0,1.0;LineWidth=2;PointSize=2";
362 std::string optionsmagenta2 =
"LineColor=1.0,0.0,1.0;LineWidth=8;PointSize=8";
363 for (
auto &b : borderlist)
365 if (std::find(ids.begin(), ids.end(), counteri) != ids.end())
367 plotBorder(
"borderpathC" + std::to_string(counteri), b, 0, 30.0, optionsmagenta2, fig);
371 plotBorder(
"borderpathC" + std::to_string(counteri), b, 0, 30.0, optionsmagenta, fig);
381 if (i > plotdata->size() - 1)
384 std::string options_normal =
"LineColor=0.0,1.0,0.0;LineWidth=3";
385 std::string options_highlighted =
"LineColor=1.0,1.0,0.0;LineWidth=6";
386 std::string cz_str =
"cz_subgroup";
387 std::string bo_str =
"bo_number";
390 std::cout <<
"\n plot evaluated borders";
392 for (
auto it = plotdata->begin(); it != plotdata->end(); ++it)
394 plot = i == cz_int ? true :
false;
396 std::cout <<
"number of cz:" << plotdata->size() << std::endl;
397 for (
auto ij = it->second.begin(); ij != it->second.end(); ++ij)
399 std::cout <<
" path of this cz" << std::endl;
400 options = (bo_int == 0 ? options_highlighted : options_normal);
404 fig->
erase(cz_str + std::to_string(cz_int) + bo_str + std::to_string(bo_int));
test implementation of a lane change trajectory planner
Definition: test_lc_trajectory_planner.h:40
adore::env::BorderBased::LaneChangeView lcvl_
Definition: test_lc_trajectory_planner.h:50
void plotCZ(DLR_TS::PlotLab::AFigureStub *fig, int &i)
Definition: test_lc_trajectory_planner.h:378
adore::fun::BasicLaneFollowingPlanner< 20, 5 > TPlanner
Definition: test_lc_trajectory_planner.h:42
bool perform_lc_right_
Definition: test_lc_trajectory_planner.h:61
adore::fun::AFactory * funFactory_
Definition: test_lc_trajectory_planner.h:44
bool hasValidPlan()
Definition: test_lc_trajectory_planner.h:198
adore::fun::DecoupledLFLCPlanner< 20, 5 > * getPlanner()
Definition: test_lc_trajectory_planner.h:220
adore::env::traffic::TrafficMap trafficMap_
Definition: test_lc_trajectory_planner.h:53
void plotConflictSet(DLR_TS::PlotLab::AFigureStub *fig)
Definition: test_lc_trajectory_planner.h:233
bool perform_lc_left_
Definition: test_lc_trajectory_planner.h:61
adore::env::BorderBased::LaneFollowingView lfv_
Definition: test_lc_trajectory_planner.h:48
adore::params::APTacticalPlanner * pTacticalPlanner_
Definition: test_lc_trajectory_planner.h:46
adore::params::AFactory * paramsFactory_
Definition: test_lc_trajectory_planner.h:45
adore::params::APVehicle * pvehicle_
Definition: test_lc_trajectory_planner.h:59
TPlanner * lfplanner_
Definition: test_lc_trajectory_planner.h:55
adore::fun::VehicleMotionState9d x_
Definition: test_lc_trajectory_planner.h:54
TestLCTrajectoryPlanner()
Definition: test_lc_trajectory_planner.h:64
adore::fun::DecoupledLFLCPlanner< 20, 5 >::TProgressSolver & getProgressSolver()
Definition: test_lc_trajectory_planner.h:212
void performLCRight()
Definition: test_lc_trajectory_planner.h:207
adore::fun::DecoupledLFLCPlanner< 20, 5 > * active_planner_
Definition: test_lc_trajectory_planner.h:58
double last_n_
Definition: test_lc_trajectory_planner.h:60
adore::fun::BasicLaneChangePlanner< 20, 5 > * lcplanner_l_
Definition: test_lc_trajectory_planner.h:56
adore::fun::RoadCoordinateConverter & getRoadCoordinateConverter()
Definition: test_lc_trajectory_planner.h:228
adore::env::BorderBased::LocalRoadMap roadmap_
Definition: test_lc_trajectory_planner.h:47
adore::mad::AWriter< adore::fun::SetPointRequest > * wwriter_
Definition: test_lc_trajectory_planner.h:52
adore::env::BorderBased::LaneChangeView lcvr_
Definition: test_lc_trajectory_planner.h:49
void plotBorder(std::string name, adore::env::BorderBased::Border *right, adore::env::BorderBased::Border *left, double z, std::string options, DLR_TS::PlotLab::AFigureStub *figure_)
Definition: test_lc_trajectory_planner.h:281
adore::mad::AReader< adore::fun::VehicleMotionState9d > * xreader_
Definition: test_lc_trajectory_planner.h:51
void run()
Definition: test_lc_trajectory_planner.h:96
void performLCLeft()
Definition: test_lc_trajectory_planner.h:202
const adore::fun::SetPointRequest * getSetPointRequest()
Definition: test_lc_trajectory_planner.h:224
adore::fun::DecoupledLFLCPlanner< 20, 5 >::TOffsetSolver & getOffsetSolver()
Definition: test_lc_trajectory_planner.h:216
adore::fun::BasicLaneChangePlanner< 20, 5 > * lcplanner_r_
Definition: test_lc_trajectory_planner.h:57
void plotBorderOverlapSet(DLR_TS::PlotLab::AFigureStub *fig)
Definition: test_lc_trajectory_planner.h:320
adore::env::AFactory * envFactory_
Definition: test_lc_trajectory_planner.h:43
abstract factory for adore::env communication
Definition: afactory.h:41
virtual TVehicleMotionStateReader * getVehicleMotionStateReader()=0
adore::mad::LLinearPiecewiseFunctionM< double, 4 > getCenterline(const BorderID &id)
get the linear piecewise description of the centerline:
Definition: borderset.h:647
LaneChangeView provides traffic related information for an adjacent lane.
Definition: lanechangeview.h:182
virtual adore::view::ALane * getTargetLane() override
Definition: lanechangeview.h:243
void update(adore::view::ALaneChangeView::direction direction)
Update the LaneChangeView.
Definition: lanechangeview.h:222
LaneFollowingview provides traffic related information for the current lane.
Definition: lanefollowingview.h:42
virtual const adore::view::TrafficQueue & getOnLaneTraffic() const override
Definition: lanefollowingview.h:140
virtual double getSpeedLimit(double s) override
Definition: lanefollowingview.h:157
void update()
update the LaneFollowingView
Definition: lanefollowingview.h:87
void getBordersToPrint(std::vector< Border * > &result, std::vector< int > &ids)
Definition: lanefollowingview.h:99
std::unordered_multimap< view::ConflictZone *, std::vector< Border * > > * getConflictSetPlotData()
Definition: lanefollowingview.h:78
std::vector< BorderOverlapSet > * getOverlapSet()
Definition: lanefollowingview.h:233
virtual void toRelativeCoordinates(double xe, double ye, double &s, double &n) override
Definition: lanefollowingview.h:217
std::vector< Coordinate > * getCornerPoints()
Definition: lanefollowingview.h:232
Definition: localroadmap.h:38
void update(bool matched_lane_proposal_valid=false, BorderID matched_lane_proposal_id=BorderID())
update the local road map
Definition: localroadmap.h:256
BorderSet * getBorderSet()
Get the BorderSet object.
Definition: localroadmap.h:123
Definition: trafficmap.h:36
void update()
Update the TrafficMap.
Definition: trafficmap.h:126
Definition: basiclanefollowingplanner.h:37
virtual const SetPointRequest * getSetPointRequest() const override
Definition: decoupled_lflc_planner.h:491
TProgressSolver & getProgressSolver()
Definition: decoupled_lflc_planner.h:335
virtual bool hasValidPlan() const override
Definition: decoupled_lflc_planner.h:484
TOffsetSolver & getOffsetSolver()
Definition: decoupled_lflc_planner.h:339
RoadCoordinateConverter & getRoadCoordinateConverter()
Definition: decoupled_lflc_planner.h:343
virtual void compute(const VehicleMotionState9d &initial_state) override
Definition: decoupled_lflc_planner.h:372
Definition: roadcoordinates.h:46
Definition: setpointrequest.h:35
virtual void getData(T &value)=0
virtual void write(const T &value)=0
adoreMatrix< T, n+1, 0 > & getData()
Definition: llinearpiecewisefunction.h:147
Definition: lq_oc_single_shooting.h:61
abstract factory for adore::params classes
Definition: afactory.h:54
virtual APLateralPlanner * getLateralPlanner() const =0
virtual APTacticalPlanner * getTacticalPlanner() const =0
virtual APTrajectoryGeneration * getTrajectoryGeneration() const =0
virtual APVehicle * getVehicle() const =0
virtual APLongitudinalPlanner * getLongitudinalPlanner() const =0
abstract classs containing parameters to configure aspects and constraints of the tactical planner
Definition: ap_tactical_planner.h:25
abstract class for vehicle configuration related paremeters
Definition: ap_vehicle.h:29
@ LEFT
Definition: alanechangeview.h:43
@ RIGHT
Definition: alanechangeview.h:43
virtual const TrafficQueue & getOnLaneTraffic() const =0
virtual bool isValid() const =0
void plotPath(std::string name, const adoreMatrix< double > &data, double z, std::string options, DLR_TS::PlotLab::AFigureStub *figure)
Definition: plot_shape.h:262
@ right
Definition: indicator_hint.h:36
@ left
Definition: indicator_hint.h:35
x
Definition: adore_set_goal.py:30
y
Definition: adore_set_goal.py:31
z
Definition: adore_set_goal.py:32
Definition: areaofeffectconverter.h:20
The border struct contains data of the smallest.
Definition: border.h:62
@ SAME_DIRECTION
Definition: border.h:507
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 getY() const
Get the y-coordinate.
Definition: vehiclemotionstate9d.h:60
double getTime() const
Get the time.
Definition: vehiclemotionstate9d.h:48
void plot(adore::fun::DecoupledLFLCPlanner< 20, 5 > *testplanner)
Definition: test_lc_trajectory_planner_node.cpp:97