ADORe
ADORe is a modular open source software library and toolkit for decision making, planning, control and simulation of automated vehicles
p_longitudinal_planner.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  * Jan Lauermann - initial API and implementation
13  ********************************************************************************/
14 
15 #pragma once
16 
17 #include <ros/ros.h>
20 
21 namespace adore
22 {
23  namespace if_ROS
24  {
25  namespace params
26  {
28  {
29  public:
30  PLongitudinalPlanner(ros::NodeHandle n,std::string prefix)
31  :ROSParam(n,prefix + "longitudinal_planner/")
32  {
33  }
34  //getWeightPos returns cost function weight for quadratic position error term
35  virtual double getWeightPos() const override
36  {
37  double value = 2.0;
38  static const std::string name = prefix_ + "weight_pos";
39  get(name,value);
40  return value;
41  }
42  //getWeightVel returns cost function weight for quadratic velocity error term
43  virtual double getWeightVel() const override
44  {
45  double value = 5.0;
46  static const std::string name = prefix_ + "weight_vel";
47  get(name,value);
48  return value;
49  }
50  //getWeightAcc returns cost function weight for quadratic acceleration term
51  virtual double getWeightAcc() const override
52  {
53  double value = 5.0;
54  static const std::string name = prefix_ + "weight_acc";
55  get(name,value);
56  return value;
57  }
58  //getWeightJerk returns cost function weight for quadratic jerk term
59  virtual double getWeightJerk() const override
60  {
61  double value = 1.0;
62  static const std::string name = prefix_ + "weight_jerk";
63  get(name,value);
64  return value;
65  }
66  //getWeightEndPos returns cost function weight for quadratic position error term at end point
67  virtual double getWeightEndPos() const override
68  {
69  double value = 0.0;
70  static const std::string name = prefix_ + "weight_end_pos";
71  get(name,value);
72  return value;
73  }
74  //getWeightEndVel returns cost function weight for quadratic velocity error term at end point
75  virtual double getWeightEndVel() const override
76  {
77  double value = 0.0;
78  static const std::string name = prefix_ + "weight_end_vel";
79  get(name,value);
80  return value;
81  }
82  //getWeightEndAcc returns cost function weight for quadratic acceleration term at end point
83  virtual double getWeightEndAcc() const override
84  {
85  double value = 0.0;
86  static const std::string name = prefix_ + "weight_end_acc";
87  get(name,value);
88  return value;
89  }
90  //getSlackPos returns maximum slack of soft-constraints for position
91  virtual double getSlackPos() const override
92  {
93  double value = 0.5;
94  static const std::string name = prefix_ + "slack_pos";
95  get(name,value);
96  return value;
97  }
98  //getSlackVel returns maximum slack of soft-constraints for velocity
99  virtual double getSlackVel() const override
100  {
101  double value = 0.01;
102  static const std::string name = prefix_ + "slack_vel";
103  get(name,value);
104  return value;
105  }
106  //getSlackAcc returns maximum slack of soft-constraints for acceleration
107  virtual double getSlackAcc() const override
108  {
109  double value = 5.0;
110  static const std::string name = prefix_ + "slack_acc";
111  get(name,value);
112  return value;
113  }
114  //getAccLB returns longitudinal acceleration lower bound
115  virtual double getAccLB() const override
116  {
117  double value = -3.0;
118  static const std::string name = prefix_ + "acc_lb";
119  get(name,value);
120  return value;
121  }
122  //getAccUB returns longitudinal acceleration upper bound
123  virtual double getAccUB() const override
124  {
125  double value = 2.0;
126  static const std::string name = prefix_ + "acc_ub";
127  get(name,value);
128  return value;
129  }
130 
132  virtual double getAccUBSlow() const override
133  {
134  double value = 2.0;
135  static const std::string name = prefix_ + "acc_ub_slow";
136  get(name,value);
137  return value;
138  }
139 
141  virtual double getVAccUBSlow() const override
142  {
143  double value = 5.0;
144  static const std::string name = prefix_ + "v_acc_ub_slow";
145  get(name,value);
146  return value;
147  }
148 
150  virtual double getComfortAccLB() const override
151  {
152  double value = -1.0;
153  static const std::string name = prefix_ + "comf_acc_lb";
154  get(name,value);
155  return value;
156  }
157 
159  virtual double getComfortAccUB() const override
160  {
161  double value = 1.0;
162  static const std::string name = prefix_ + "comf_acc_ub";
163  get(name,value);
164  return value;
165  }
166 
167  //getJerkLB returns longitudinal jerk lower bound
168  virtual double getJerkLB() const override
169  {
170  double value = -100.0;
171  static const std::string name = prefix_ + "jerk_lb";
172  get(name,value);
173  return value;
174  }
175  //getJerkLB returns longitudinal jerk upper bound
176  virtual double getJerkUB() const override
177  {
178  double value = 100.0;
179  static const std::string name = prefix_ + "jerk_ub";
180  get(name,value);
181  return value;
182  }
183  //getAccLatUB returns the absolute lateral acceleration bound which has to be maintained by reducing speed
184  virtual double getAccLatUB() const override
185  {
186  double value = 2.0;
187  static const std::string name = prefix_ + "acc_lat_ub";
188  get(name,value);
189  return value;
190  }
191  //getAccLatUB_minVelocity returns the minimum velocity, which is always feasible despite getAccLatUB
192  virtual double getAccLatUB_minVelocity() const override
193  {
194  double value = 1.0;
195  static const std::string name = prefix_ + "acc_lat_ub_min_velocity";
196  get(name,value);
197  return value;
198  }
199  //getConstraintClearancePos returns the offset of the reference from the position constraints
200  virtual double getConstraintClearancePos() const override
201  {
202  double value = 0.1;
203  static const std::string name = prefix_ + "constraint_clearance_pos";
204  get(name,value);
205  return value;
206  }
207  //getConstraintClearanceVel returns the offset of the reference from the velocity constraints
208  virtual double getConstraintClearanceVel() const override
209  {
210  double value = 0.05;
211  static const std::string name = prefix_ + "constraint_clearance_vel";
212  get(name,value);
213  return value;
214  }
215 
216 
218  virtual double getMinWidthStop() const override
219  {
220  double value = 2.2;
221  static const std::string name = prefix_ + "min_width_stop";
222  get(name,value);
223  return value;
224  }
225 
227  virtual double getMinWidthSlow() const override
228  {
229  double value = 2.5;
230  static const std::string name = prefix_ + "min_width_slow";
231  get(name,value);
232  return value;
233  }
234 
236  virtual double getMinWidthSlowSpeed() const override
237  {
238  double value = 1.0;
239  static const std::string name = prefix_ + "min_width_slow_speed";
240  get(name,value);
241  return value;
242  }
243 
245  virtual double getMinWidthFast() const override
246  {
247  double value = 3.7;
248  static const std::string name = prefix_ + "min_width_fast";
249  get(name,value);
250  return value;
251  }
252 
254  virtual double getMinWidthFastSpeed() const override
255  {
256  double value = 40.0/3.6;
257  static const std::string name = prefix_ + "min_width_fast_speed";
258  get(name,value);
259  return value;
260  }
261 
262 
263  //getMaxCPUTime returns the maximum cpu time for one plan computation
264  virtual double getMaxCPUTime() const override
265  {
266  double value = 0.05;
267  static const std::string name = prefix_ + "max_cpu_time";
268  get(name,value);
269  return value;
270  }
271 
273  virtual bool stopAtRedLights_always_before()const override
274  {
275  bool value = true;
276  static const std::string name = prefix_ + "stopAtRedLights_always_before";
277  get(name,value);
278  return value;
279  }
281  virtual double stopAtRedLights_max_connection_length() const override
282  {
283  double value = 50.0;
284  static const std::string name = prefix_ + "stopAtRedLights_max_connection_length";
285  get(name,value);
286  return value;
287  }
291  virtual double getStopDistanceToConflictPoint()const override
292  {
293  double value = 5.0;
294  static const std::string name = prefix_ + "stop_distance_to_conflict_point";
295  get(name,value);
296  return value;
297  }
298  };
299  }
300  }
301 }
Definition: ros_com_patterns.h:179
std::string prefix_
Definition: ros_com_patterns.h:181
void get(const std::string &name, T &result) const
Definition: ros_com_patterns.h:186
Definition: p_longitudinal_planner.h:28
virtual double getWeightEndAcc() const override
getWeightEndAcc returns cost function weight for quadratic acceleration term at end point
Definition: p_longitudinal_planner.h:83
virtual double getMinWidthStop() const override
getMinWidthStop returns the minimum lane width, below/at which vehicle stops: Should be greater or eq...
Definition: p_longitudinal_planner.h:218
virtual double getJerkLB() const override
getJerkLB returns longitudinal jerk lower bound
Definition: p_longitudinal_planner.h:168
virtual double getComfortAccLB() const override
getAccLB returns longitudinal acceleration lower bound
Definition: p_longitudinal_planner.h:150
virtual double getAccUBSlow() const override
getAccUBSlow returns acceleration upper bound at low speeds
Definition: p_longitudinal_planner.h:132
virtual double getMinWidthSlowSpeed() const override
getMinWidthSlowSpeed returns the slow speed to be applied, if lane width equals minWidthSlow: Should ...
Definition: p_longitudinal_planner.h:236
virtual double getJerkUB() const override
getJerkLB returns longitudinal jerk upper bound
Definition: p_longitudinal_planner.h:176
virtual double getMinWidthFast() const override
getMinWidthFast returns the minimum lane width, below/at which vehicle moves fast: Should be greater ...
Definition: p_longitudinal_planner.h:245
virtual double getConstraintClearanceVel() const override
getConstraintClearanceVel returns the offset of the reference from the velocity constraints
Definition: p_longitudinal_planner.h:208
virtual double getComfortAccUB() const override
getAccUB returns longitudinal acceleration upper bound
Definition: p_longitudinal_planner.h:159
virtual double getWeightEndPos() const override
getWeightEndPos returns cost function weight for quadratic position error term at end point
Definition: p_longitudinal_planner.h:67
virtual double getWeightJerk() const override
getWeightJerk returns cost function weight for quadratic jerk term
Definition: p_longitudinal_planner.h:59
virtual double getStopDistanceToConflictPoint() const override
distance between stop position and conflict point
Definition: p_longitudinal_planner.h:291
virtual double getWeightAcc() const override
getWeightAcc returns cost function weight for quadratic acceleration term
Definition: p_longitudinal_planner.h:51
virtual double getMinWidthFastSpeed() const override
getMinWidthFastSpeed returns the fast speed to be applied, if lane width equals minWidthFast: Should ...
Definition: p_longitudinal_planner.h:254
virtual double getWeightPos() const override
getWeightPos returns cost function weight for quadratic position error term
Definition: p_longitudinal_planner.h:35
virtual double getAccUB() const override
getAccUB returns longitudinal acceleration upper bound
Definition: p_longitudinal_planner.h:123
virtual double getMinWidthSlow() const override
getMinWidthSlow returns the minimum lane width, below/at which vehicle moves slowly: Should be greate...
Definition: p_longitudinal_planner.h:227
PLongitudinalPlanner(ros::NodeHandle n, std::string prefix)
Definition: p_longitudinal_planner.h:30
virtual bool stopAtRedLights_always_before() const override
determin stop mode for red lights: true - always before red light, continue driving after; false - ba...
Definition: p_longitudinal_planner.h:273
virtual double getAccLatUB_minVelocity() const override
getAccLatUB_minVelocity returns the minimum velocity, which is always feasible despite getAccLatUB
Definition: p_longitudinal_planner.h:192
virtual double getVAccUBSlow() const override
getVAccUBSlow returns speed up to which slow speed acceleration is used
Definition: p_longitudinal_planner.h:141
virtual double getWeightVel() const override
getWeightVel returns cost function weight for quadratic velocity error term
Definition: p_longitudinal_planner.h:43
virtual double getMaxCPUTime() const override
getMaxCPUTime returns the maximum cpu time for one plan computation
Definition: p_longitudinal_planner.h:264
virtual double getSlackVel() const override
getSlackVel returns maximum slack of soft-constraints for velocity
Definition: p_longitudinal_planner.h:99
virtual double getConstraintClearancePos() const override
getConstraintClearancePos returns the longitudinal offset of the reference from the position constrai...
Definition: p_longitudinal_planner.h:200
virtual double getSlackAcc() const override
getSlackAcc returns maximum slack of soft-constraints for acceleration
Definition: p_longitudinal_planner.h:107
virtual double getAccLatUB() const override
getAccLatUB returns the absolute lateral acceleration bound which has to be maintained by reducing sp...
Definition: p_longitudinal_planner.h:184
virtual double getWeightEndVel() const override
getWeightEndVel returns cost function weight for quadratic velocity error term at end point
Definition: p_longitudinal_planner.h:75
virtual double getSlackPos() const override
getSlackPos returns maximum slack of soft-constraints for position
Definition: p_longitudinal_planner.h:91
virtual double getAccLB() const override
getAccLB returns longitudinal acceleration lower bound
Definition: p_longitudinal_planner.h:115
virtual double stopAtRedLights_max_connection_length() const override
stopAtRedLights_max_connection_length returns the maximum length for which clearance based on tcd sta...
Definition: p_longitudinal_planner.h:281
abstract class containing parameters related to configuring the longitudinal planner
Definition: ap_longitudinal_planner.h:26
Definition: areaofeffectconverter.h:20