ADORe
ADORe is a modular open source software library and toolkit for decision making, planning, control and simulation of automated vehicles
p_lateral_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  PLateralPlanner(ros::NodeHandle n,std::string prefix)
31  :ROSParam(n,prefix + "lateral_planner/")
32  {
33  }
34  //getWeightPos returns cost function weight for quadratic position error term
35  virtual double getWeightPos() const override
36  {
37  double value = 6.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 = 1.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 = 1.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.1;
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.1;
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 = 0.1;
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 = -10.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 = 10.0;
126  static const std::string name = prefix_ + "acc_ub";
127  get(name,value);
128  return value;
129  }
130  //getJerkLB returns longitudinal jerk lower bound
131  virtual double getJerkLB() const override
132  {
133  double value = -100.0;
134  static const std::string name = prefix_ + "jerk_lb";
135  get(name,value);
136  return value;
137  }
138  //getJerkUB returns longitudinal jerk upper bound
139  virtual double getJerkUB() const override
140  {
141  double value = 100.0;
142  static const std::string name = prefix_ + "jerk_ub";
143  get(name,value);
144  return value;
145  }
146  //getCurvatureUB returns maximum curvature of path (relevant at low speeds)
147  virtual double getCurvatureUB() const override
148  {
149  double value = 0.14;
150  static const std::string name = prefix_ + "curvature_ub";
151  get(name,value);
152  return value;
153  }
154  //getCurvatureLB returns minimum curvature of path (relevant at low speeds)
155  virtual double getCurvatureLB() const override
156  {
157  double value = -0.14;
158  static const std::string name = prefix_ + "curvature_lb";
159  get(name,value);
160  return value;
161  }
162  //getRelativeHeadingUB returns upper bound on heading deviation from current lane's coordinate system
163  virtual double getRelativeHeadingUB() const override
164  {
165  double value = 0.7;
166  static const std::string name = prefix_ + "relative_heading_ub";
167  get(name,value);
168  return value;
169  }
170  //getRelativeHeadingLB returns lower bound on heading deviation from current lane's coordinate system
171  virtual double getRelativeHeadingLB() const override
172  {
173  double value = -0.7;
174  static const std::string name = prefix_ + "relative_heading_lb";
175  get(name,value);
176  return value;
177  }
178  //getMergeConstraintDelay returns a time-delay after which lateral position constraints are activated, if they are initially violated
179  virtual double getMergeConstraintDelay() const override
180  {
181  double value = 3.0;
182  static const std::string name = prefix_ + "merge_constraint_delay";
183  get(name,value);
184  return value;
185  }
186 
188  virtual double getHardSafetyDistanceToLaneBoundary() const override
189  {
190  double value = 0.2;
191  static const std::string name = prefix_ + "hard_safety_distance_to_lane_boundary";
192  get(name,value);
193  return value;
194  }
195 
197  virtual double getSoftSafetyDistanceToLaneBoundary() const override
198  {
199  double value = 1.0;
200  static const std::string name = prefix_ + "soft_safety_distance_to_lane_boundary";
201  get(name,value);
202  return value;
203  }
204 
206  virtual double getMinimumLateralControlSpace() const override
207  {
208  double value = 1.0;
209  static const std::string name = prefix_ + "minimum_lateral_control_space";
210  get(name,value);
211  return value;
212  }
213 
214  //getMaxCPUTime returns the maximum cpu time for one plan computation
215  virtual double getMaxCPUTime() const override
216  {
217  double value = 0.05;
218  static const std::string name = prefix_ + "max_cpu_time";
219  get(name,value);
220  return value;
221  }
223  virtual double getLateralGridScale() const override
224  {
225  double value = 0.2;
226  static const std::string name = prefix_ + "lateral_grid_scale";
227  get(name,value);
228  return value;
229  }
230 
231  };
232  }
233  }
234 }
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_lateral_planner.h:28
virtual double getJerkLB() const override
getJerkLB returns lateral jerk lower bound
Definition: p_lateral_planner.h:131
virtual double getHardSafetyDistanceToLaneBoundary() const override
getHardSafetyDistanceToLaneBoundary returns the minimum distance between lane boundary and vehicle si...
Definition: p_lateral_planner.h:188
PLateralPlanner(ros::NodeHandle n, std::string prefix)
Definition: p_lateral_planner.h:30
virtual double getMinimumLateralControlSpace() const override
getMinimumLateralControlSpace returns the minimum desired lateral control space: If vehicle has more ...
Definition: p_lateral_planner.h:206
virtual double getMaxCPUTime() const override
getMaxCPUTime returns the maximum cpu time for one plan computation
Definition: p_lateral_planner.h:215
virtual double getWeightAcc() const override
getWeightAcc returns cost function weight for quadratic acceleration term
Definition: p_lateral_planner.h:51
virtual double getLateralGridScale() const override
getLateralGridScale returns the size of a grid step d for lateral variations of a maneuver: maneuver ...
Definition: p_lateral_planner.h:223
virtual double getCurvatureUB() const override
getCurvatureUB returns maximum curvature of path (relevant at low speeds)
Definition: p_lateral_planner.h:147
virtual double getAccLB() const override
getAccLB returns lateral acceleration lower bound
Definition: p_lateral_planner.h:115
virtual double getMergeConstraintDelay() const override
getMergeConstraintDelay returns a time-delay after which lateral position constraints are activated,...
Definition: p_lateral_planner.h:179
virtual double getWeightEndVel() const override
getWeightEndVel returns cost function weight for quadratic velocity error term at end point
Definition: p_lateral_planner.h:75
virtual double getWeightEndPos() const override
getWeightEndPos returns cost function weight for quadratic position error term at end point
Definition: p_lateral_planner.h:67
virtual double getRelativeHeadingLB() const override
getRelativeHeadingLB returns lower bound on heading deviation from current lane's coordinate system
Definition: p_lateral_planner.h:171
virtual double getWeightVel() const override
getWeightVel returns cost function weight for quadratic velocity error term
Definition: p_lateral_planner.h:43
virtual double getJerkUB() const override
getJerkLB returns lateral jerk upper bound
Definition: p_lateral_planner.h:139
virtual double getSoftSafetyDistanceToLaneBoundary() const override
getSoftSafetyDistanceToLaneBoundary returns the minimum distance between lane boundary and vehicle si...
Definition: p_lateral_planner.h:197
virtual double getWeightJerk() const override
getWeightJerk returns cost function weight for quadratic jerk term
Definition: p_lateral_planner.h:59
virtual double getSlackPos() const override
getSlackPos returns maximum slack of soft-constraints for position
Definition: p_lateral_planner.h:91
virtual double getSlackAcc() const override
getSlackAcc returns maximum slack of soft-constraints for acceleration
Definition: p_lateral_planner.h:107
virtual double getWeightPos() const override
getWeightPos returns cost function weight for quadratic position error term
Definition: p_lateral_planner.h:35
virtual double getWeightEndAcc() const override
getWeightEndAcc returns cost function weight for quadratic acceleration term at end point
Definition: p_lateral_planner.h:83
virtual double getCurvatureLB() const override
getCurvatureLB returns minimum curvature of path (relevant at low speeds)
Definition: p_lateral_planner.h:155
virtual double getAccUB() const override
getAccUB returns lateral acceleration upper bound
Definition: p_lateral_planner.h:123
virtual double getRelativeHeadingUB() const override
getRelativeHeadingUB returns upper bound on heading deviation from current lane's coordinate system
Definition: p_lateral_planner.h:163
virtual double getSlackVel() const override
getSlackVel returns maximum slack of soft-constraints for velocity
Definition: p_lateral_planner.h:99
abstract class containing parameters related to configuring the lateral planner
Definition: ap_lateral_planner.h:26
Definition: areaofeffectconverter.h:20