ADORe
ADORe is a modular open source software library and toolkit for decision making, planning, control and simulation of automated vehicles
p_tactical_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  PTacticalPlanner(ros::NodeHandle n,std::string prefix)
31  :ROSParam(n,prefix + "tactical_planner/")
32  {
33  }
34  virtual double getGlobalSpeedLimit()const override
35  {
36  double value = 10.0;
37  static const std::string name = prefix_ + "global_speed_limit";
38  get(name,value);
39  return value;
40  }
41  virtual double getResetRadius()const override
42  {
43  double value = 1.0;
44  static const std::string name = prefix_ + "reset_radius";
45  get(name,value);
46  return value;
47  }
48  virtual double getAccLatUB()const override
49  {
50  double value = 2.0;
51  static const std::string name = prefix_ + "acc_lat_ub";
52  get(name,value);
53  return value;
54  }
55  virtual double getAccLatUB_minVelocity()const override
56  {
57  double value = 3.0;
58  static const std::string name = prefix_ + "acc_lat_ub_min_vel";
59  get(name,value);
60  return value;
61  }
62  virtual double getAccLonUB()const override
63  {
64  double value = 2.0;
65  static const std::string name = prefix_ + "acc_lon_ub";
66  get(name,value);
67  return value;
68  }
69  virtual double getAccLonLB()const override
70  {
71  double value = -3.0;
72  static const std::string name = prefix_ + "acc_lon_lb";
73  get(name,value);
74  return value;
75  }
76  virtual double getFrontTimeGap()const override
77  {
78  double value = 0.9;
79  static const std::string name = prefix_ + "front_time_gap";
80  get(name,value);
81  return value;
82  }
83  virtual double getRearTimeGap()const override
84  {
85  double value = 0.9;
86  static const std::string name = prefix_ + "rear_time_gap";
87  get(name,value);
88  return value;
89  }
90  virtual double getFrontSGap()const override
91  {
92  double value = 6.0;
93  static const std::string name = prefix_ + "front_s_gap";
94  get(name,value);
95  return value;
96  }
97  virtual double getLowerBoundFrontSGapForLF()const override
98  {
99  double value = 7.0;
100  static const std::string name = prefix_ + "lower_bound_lf_front_s_gap";
101  get(name,value);
102  return value;
103  }
104  virtual double getRearSGap()const override
105  {
106  double value = 2.0;
107  static const std::string name = prefix_ + "rear_s_gap";
108  get(name,value);
109  return value;
110  }
111  virtual double getChaseReferenceOffset()const override
112  {
113  double value = 0.0;
114  static const std::string name = prefix_ + "chase_reference_offset";
115  get(name,value);
116  return value;
117  }
118  virtual double getLeadReferenceOffset()const override
119  {
120  double value = 0.0;
121  static const std::string name = prefix_ + "lead_reference_offset";
122  get(name,value);
123  return value;
124  }
125  virtual double getFrontReferenceOffset()const override
126  {
127  double value = 0.0;
128  static const std::string name = prefix_ + "front_reference_offset";
129  get(name,value);
130  return value;
131  }
132  virtual double getGapAlignment()const override
133  {
134  double value = 0.5;
135  static const std::string name = prefix_ + "gap_alignment";
136  get(name,value);
137  return value;
138  }
139  virtual double getAssumedNominalAccelerationMinimum()const override
140  {
141  double value = -1.0;
142  static const std::string name = prefix_ + "assumed_nominal_acceleration_minimum";
143  get(name,value);
144  return value;
145  }
146  virtual double getMaxNavcostLoss()const override
147  {
148  double value = 0.0;
149  static const std::string name = prefix_ + "max_navcost_loss";
150  get(name,value);
151  return value;
152  }
153  virtual bool getEnforceMonotonousNavigationCost()const override
154  {
155  bool value = false;
156  static const std::string name = prefix_ + "enforce_monotonous_navigation_cost";
157  get(name,value);
158  return value;
159  }
161  {
162  double value = 5.0;
163  static const std::string name = prefix_ + "preferred_lc_by_manual_indicator_timeout";
164  get(name,value);
165  return value;
166  }
167  virtual double getLVResetVelocity()const override
168  {
169  double value = 1.5;
170  static const std::string name = prefix_ + "lane_view_reset_velocity";
171  get(name,value);
172  return value;
173  }
174  virtual double getTimeoutForLangechangeSuppression()const override
175  {
176  double value = 5.0;
177  static const std::string name = prefix_ + "lanechange_suppression_timeout";
178  get(name,value);
179  return value;
180  }
181  virtual double getCollisionDetectionFrontBufferSpace()const override
182  {
183  double value = 4.0;
184  static const std::string name = prefix_ + "collision_detection_front_buffer_space";
185  get(name,value);
186  return value;
187  }
188  virtual double getCollisionDetectionLateralPrecision()const override
189  {
190  double value = 0.05;
191  static const std::string name = prefix_ + "collision_detection_lateral_precision";
192  get(name,value);
193  return value;
194  }
195  virtual double getCollisionDetectionLateralError()const override
196  {
197  double value = 0.0;
198  static const std::string name = prefix_ + "collision_detection_lateral_error";
199  get(name,value);
200  return value;
201  }
202  virtual double getCollisionDetectionLongitudinalError()const override
203  {
204  double value = 0.0;
205  static const std::string name = prefix_ + "collision_detection_longitudinal_error";
206  get(name,value);
207  return value;
208  }
209  virtual double getNominalSwathAccelerationError()const override
210  {
211  double value = 0.0;
212  static const std::string name = prefix_ + "nominal_swath_acceleration_error";
213  get(name,value);
214  return value;
215  }
217  virtual int getCoercionPreventionStrategy()const override
218  {
219  int value = 0;
220  static const std::string name = prefix_ + "coercion_prevention_strategy";
221  get(name,value);
222  return value;
223  }
224  virtual double getIndicatorLookahead()const override
225  {
226  double value = 50.0;
227  static const std::string name = prefix_ + "indicator_lookahead";
228  get(name,value);
229  return value;
230  }
231  virtual double getHorizonStopReferenceDistance()const override
232  {
233  double value = 5.0;
234  static const std::string name = prefix_ + "horizon_stop_reference_distance";
235  get(name,value);
236  return value;
237  }
238  virtual double getTerminateAfterFirstStopThresholdSpeed()const override
239  {
240  double value = 0.3;
241  static const std::string name = prefix_ + "terminate_after_first_stop_threshold_speed";
242  get(name,value);
243  return value;
244  }
245 
246  };
247  }
248  }
249 }
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_tactical_planner.h:28
virtual double getTimeoutForPreferredLCAfterManuallySetIndicator() const override
Definition: p_tactical_planner.h:160
virtual double getLeadReferenceOffset() const override
Definition: p_tactical_planner.h:118
virtual double getFrontTimeGap() const override
Definition: p_tactical_planner.h:76
PTacticalPlanner(ros::NodeHandle n, std::string prefix)
Definition: p_tactical_planner.h:30
virtual double getAccLatUB_minVelocity() const override
Definition: p_tactical_planner.h:55
virtual double getRearTimeGap() const override
Definition: p_tactical_planner.h:83
virtual double getCollisionDetectionFrontBufferSpace() const override
Definition: p_tactical_planner.h:181
virtual double getLowerBoundFrontSGapForLF() const override
Definition: p_tactical_planner.h:97
virtual double getAccLonUB() const override
Definition: p_tactical_planner.h:62
virtual double getAccLatUB() const override
Definition: p_tactical_planner.h:48
virtual double getAccLonLB() const override
Definition: p_tactical_planner.h:69
virtual double getTimeoutForLangechangeSuppression() const override
Definition: p_tactical_planner.h:174
virtual double getRearSGap() const override
Definition: p_tactical_planner.h:104
virtual double getGlobalSpeedLimit() const override
Definition: p_tactical_planner.h:34
virtual double getHorizonStopReferenceDistance() const override
Definition: p_tactical_planner.h:231
virtual double getChaseReferenceOffset() const override
Definition: p_tactical_planner.h:111
virtual double getCollisionDetectionLongitudinalError() const override
Definition: p_tactical_planner.h:202
virtual double getIndicatorLookahead() const override
Definition: p_tactical_planner.h:224
virtual double getTerminateAfterFirstStopThresholdSpeed() const override
Definition: p_tactical_planner.h:238
virtual double getCollisionDetectionLateralError() const override
Definition: p_tactical_planner.h:195
virtual double getCollisionDetectionLateralPrecision() const override
Definition: p_tactical_planner.h:188
virtual double getMaxNavcostLoss() const override
Definition: p_tactical_planner.h:146
virtual double getLVResetVelocity() const override
Definition: p_tactical_planner.h:167
virtual double getResetRadius() const override
Definition: p_tactical_planner.h:41
virtual double getGapAlignment() const override
Definition: p_tactical_planner.h:132
virtual double getFrontSGap() const override
Definition: p_tactical_planner.h:90
virtual int getCoercionPreventionStrategy() const override
getCoercionPreventionStrategy returns 0 switched off, 1 objective function, 2 constraint
Definition: p_tactical_planner.h:217
virtual double getFrontReferenceOffset() const override
Definition: p_tactical_planner.h:125
virtual double getAssumedNominalAccelerationMinimum() const override
Definition: p_tactical_planner.h:139
virtual bool getEnforceMonotonousNavigationCost() const override
Definition: p_tactical_planner.h:153
virtual double getNominalSwathAccelerationError() const override
Definition: p_tactical_planner.h:209
abstract classs containing parameters to configure aspects and constraints of the tactical planner
Definition: ap_tactical_planner.h:25
Definition: areaofeffectconverter.h:20