ADORe
ADORe is a modular open source software library and toolkit for decision making, planning, control and simulation of automated vehicles
planningresultconverter.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  * Daniel Heß - initial API and implementation
13  ********************************************************************************/
14 
15 #pragma once
16 
17 #include <adore_if_ros_msg/PlanningResultSet.h>
21 #include "occupancyconverter.h"
22 
23 namespace adore
24 {
25  namespace if_ROS
26  {
28  {
31  // /**
32  // * Conversion of TPlanningResultSet to PlanningResultSet message
33  // */
34  // adore_if_ros_msg::PlanningResultSet operator()(const adore::fun::TPlanningResultSet & result_set)
35  // {
36  // adore_if_ros_msg::PlanningResultSet msg;
37  // for(auto& result:result_set)
38  // {
39  // msg.data.push_back((*this)(result));
40  // }
41  // return msg;
42  // }
43  // /**
44  // * Conversion of PlanningResultSet message to TPlanningResultSet
45  // */
46  // void operator()(adore_if_ros_msg::PlanningResultSetConstPtr msg,adore::fun::TPlanningResultSet*
47  // result_set)
48  // {
49  // result_set->clear();
50  // for(auto& m:msg->data)
51  // {
52  // adore::fun::PlanningResult result;
53  // (*this)(&m,&result);
54  // result_set->push_back(result);
55  // }
56  // }
60  adore_if_ros_msg::PlanningResult operator()(const adore::fun::PlanningResult& result)
61  {
62  adore_if_ros_msg::PlanningResult msg;
63  msg.id = result.id;
64  msg.iteration = result.iteration;
65  msg.name = result.name;
66  msg.nominal_maneuver = spr_converter_(result.nominal_maneuver);
67  msg.combined_maneuver = spr_converter_(result.combined_maneuver);
68  msg.terminal_maneuver = tr_converter_(result.terminal_maneuver);
69  msg.nominal_maneuver_valid = result.nominal_maneuver_valid;
70  msg.combined_maneuver_valid = result.combined_maneuver_valid;
71  msg.status_string = result.status_string;
72  msg.maneuver_type = result.maneuver_type;
73  msg.indicator_left = result.indicator_left;
74  msg.indicator_right = result.indicator_right;
75  for (auto [cp_name, cp_value] : result.objective_values)
76  {
77  adore_if_ros_msg::CostPair cpmsg;
78  cpmsg.objective_name = cp_name;
79  cpmsg.objective_value = cp_value;
80  msg.objective_values.push_back(cpmsg);
81  }
82  for (auto [cp_name, cp_value] : result.performance_values)
83  {
84  adore_if_ros_msg::CostPair cpmsg;
85  cpmsg.objective_name = cp_name;
86  cpmsg.objective_value = cp_value;
87  msg.performance_values.push_back(cpmsg);
88  }
89  // copy the nominal_maneuver_swath
90  msg.nominal_maneuver_swath.trackingID = 0;
91  msg.nominal_maneuver_swath.branchID = 0;
92  msg.nominal_maneuver_swath.predecessorID = 0;
93  msg.nominal_maneuver_swath.confidence = 0;
94  for (const auto& pair : result.nominal_maneuver_swath.getLevel(0))
95  {
96  const auto& cylinder = pair.second;
97  adore_if_ros_msg::OccupancyCylinder msgi;
98  msgi.rxy = cylinder.rxy_;
99  msgi.x = cylinder.x_;
100  msgi.y = cylinder.y_;
101  msgi.t0 = cylinder.t0_;
102  msgi.t1 = cylinder.t1_;
103  msgi.z0 = cylinder.z0_;
104  msgi.z1 = cylinder.z1_;
105  msg.nominal_maneuver_swath.occupancy.push_back(msgi);
106  }
107  // copy the combined_maneuver_swath
108  msg.combined_maneuver_swath.trackingID = 0;
109  msg.combined_maneuver_swath.branchID = 0;
110  msg.combined_maneuver_swath.predecessorID = 0;
111  msg.combined_maneuver_swath.confidence = 0;
112  for (const auto& pair : result.combined_maneuver_swath.getLevel(0))
113  {
114  const auto& cylinder = pair.second;
115  adore_if_ros_msg::OccupancyCylinder msgi;
116  msgi.rxy = cylinder.rxy_;
117  msgi.x = cylinder.x_;
118  msgi.y = cylinder.y_;
119  msgi.t0 = cylinder.t0_;
120  msgi.t1 = cylinder.t1_;
121  msgi.z0 = cylinder.z0_;
122  msgi.z1 = cylinder.z1_;
123  msg.combined_maneuver_swath.occupancy.push_back(msgi);
124  }
125  // copy details on longitudinal planning
126  // plan
127  auto& longitudinal_plan = result.nominal_maneuver_longitudinal_plan.getData();
128  const int NC_lon = longitudinal_plan.nc();
129  const int NR_lon = longitudinal_plan.nr();
130  if (NC_lon > 0 && NR_lon > 1)
131  {
132  msg.nominal_maneuver_longitudinal_plan.fcn.layout.data_offset = 0;
133  msg.nominal_maneuver_longitudinal_plan.fcn.data.insert(
134  msg.nominal_maneuver_longitudinal_plan.fcn.data.end(), longitudinal_plan.begin(),
135  longitudinal_plan.end());
136  msg.nominal_maneuver_longitudinal_plan.fcn.layout.dim.push_back(std_msgs::MultiArrayDimension());
137  msg.nominal_maneuver_longitudinal_plan.fcn.layout.dim[0].label = "time";
138  msg.nominal_maneuver_longitudinal_plan.fcn.layout.dim[0].size = NC_lon;
139  msg.nominal_maneuver_longitudinal_plan.fcn.layout.dim[0].stride = NR_lon * NC_lon;
140  msg.nominal_maneuver_longitudinal_plan.fcn.layout.dim.push_back(std_msgs::MultiArrayDimension());
141  msg.nominal_maneuver_longitudinal_plan.fcn.layout.dim[1].label = "s and derivatives";
142  msg.nominal_maneuver_longitudinal_plan.fcn.layout.dim[1].size = (NR_lon - 1) * NC_lon;
143  msg.nominal_maneuver_longitudinal_plan.fcn.layout.dim[1].stride = NC_lon;
144  }
145  // upper bound
146  auto& longitudinal_ubx = result.nominal_maneuver_longitudinal_ubx.getData();
147  const int NC_lon_ubx = longitudinal_ubx.nc();
148  const int NR_lon_ubx = longitudinal_ubx.nr();
149  if (NC_lon_ubx > 0 && NR_lon_ubx > 1)
150  {
151  msg.nominal_maneuver_longitudinal_plan_ubx.fcn.layout.data_offset = 0;
152  msg.nominal_maneuver_longitudinal_plan_ubx.fcn.data.insert(
153  msg.nominal_maneuver_longitudinal_plan_ubx.fcn.data.end(), longitudinal_ubx.begin(),
154  longitudinal_ubx.end());
155  msg.nominal_maneuver_longitudinal_plan_ubx.fcn.layout.dim.push_back(
156  std_msgs::MultiArrayDimension());
157  msg.nominal_maneuver_longitudinal_plan_ubx.fcn.layout.dim[0].label = "time";
158  msg.nominal_maneuver_longitudinal_plan_ubx.fcn.layout.dim[0].size = NC_lon_ubx;
159  msg.nominal_maneuver_longitudinal_plan_ubx.fcn.layout.dim[0].stride = NR_lon_ubx * NC_lon_ubx;
160  msg.nominal_maneuver_longitudinal_plan_ubx.fcn.layout.dim.push_back(
161  std_msgs::MultiArrayDimension());
162  msg.nominal_maneuver_longitudinal_plan_ubx.fcn.layout.dim[1].label = "upper bound of s and "
163  "derivatives";
164  msg.nominal_maneuver_longitudinal_plan_ubx.fcn.layout.dim[1].size = (NR_lon_ubx - 1) * NC_lon_ubx;
165  msg.nominal_maneuver_longitudinal_plan_ubx.fcn.layout.dim[1].stride = NC_lon_ubx;
166  }
167  // lower bound
168  auto& longitudinal_lbx = result.nominal_maneuver_longitudinal_lbx.getData();
169  const int NC_lon_lbx = longitudinal_lbx.nc();
170  const int NR_lon_lbx = longitudinal_lbx.nr();
171  if (NC_lon_lbx > 0 && NR_lon_lbx > 1)
172  {
173  msg.nominal_maneuver_longitudinal_plan_lbx.fcn.layout.data_offset = 0;
174  msg.nominal_maneuver_longitudinal_plan_lbx.fcn.data.insert(
175  msg.nominal_maneuver_longitudinal_plan_lbx.fcn.data.end(), longitudinal_lbx.begin(),
176  longitudinal_lbx.end());
177  msg.nominal_maneuver_longitudinal_plan_lbx.fcn.layout.dim.push_back(
178  std_msgs::MultiArrayDimension());
179  msg.nominal_maneuver_longitudinal_plan_lbx.fcn.layout.dim[0].label = "time";
180  msg.nominal_maneuver_longitudinal_plan_lbx.fcn.layout.dim[0].size = NC_lon_lbx;
181  msg.nominal_maneuver_longitudinal_plan_lbx.fcn.layout.dim[0].stride = NR_lon_lbx * NC_lon_lbx;
182  msg.nominal_maneuver_longitudinal_plan_lbx.fcn.layout.dim.push_back(
183  std_msgs::MultiArrayDimension());
184  msg.nominal_maneuver_longitudinal_plan_lbx.fcn.layout.dim[1].label = "lowr bound of s and "
185  "derivatives";
186  msg.nominal_maneuver_longitudinal_plan_lbx.fcn.layout.dim[1].size = (NR_lon_lbx - 1) * NC_lon_lbx;
187  msg.nominal_maneuver_longitudinal_plan_lbx.fcn.layout.dim[1].stride = NC_lon_lbx;
188  }
189  // copy details on lateral planning
190  // plan
191  auto& lateral_plan = result.nominal_maneuver_lateral_plan.getData();
192  const int NC_lat = lateral_plan.nc();
193  const int NR_lat = lateral_plan.nr();
194  if (NC_lat > 0 && NR_lat > 1)
195  {
196  msg.nominal_maneuver_lateral_plan.fcn.layout.data_offset = 0;
197  msg.nominal_maneuver_lateral_plan.fcn.data.insert(msg.nominal_maneuver_lateral_plan.fcn.data.end(),
198  lateral_plan.begin(), lateral_plan.end());
199  msg.nominal_maneuver_lateral_plan.fcn.layout.dim.push_back(std_msgs::MultiArrayDimension());
200  msg.nominal_maneuver_lateral_plan.fcn.layout.dim[0].label = "time";
201  msg.nominal_maneuver_lateral_plan.fcn.layout.dim[0].size = NC_lat;
202  msg.nominal_maneuver_lateral_plan.fcn.layout.dim[0].stride = NR_lat * NC_lat;
203  msg.nominal_maneuver_lateral_plan.fcn.layout.dim.push_back(std_msgs::MultiArrayDimension());
204  msg.nominal_maneuver_lateral_plan.fcn.layout.dim[1].label = "n and derivatives";
205  msg.nominal_maneuver_lateral_plan.fcn.layout.dim[1].size = (NR_lat - 1) * NC_lat;
206  msg.nominal_maneuver_lateral_plan.fcn.layout.dim[1].stride = NC_lat;
207  }
208  // upper bound
209  auto& lateral_ubx = result.nominal_maneuver_lateral_ubx.getData();
210  const int NC_lat_ubx = lateral_ubx.nc();
211  const int NR_lat_ubx = lateral_ubx.nr();
212  if (NC_lat_ubx > 0 && NR_lat_ubx > 1)
213  {
214  msg.nominal_maneuver_lateral_plan_ubx.fcn.layout.data_offset = 0;
215  msg.nominal_maneuver_lateral_plan_ubx.fcn.data.insert(
216  msg.nominal_maneuver_lateral_plan_ubx.fcn.data.end(), lateral_ubx.begin(), lateral_ubx.end());
217  msg.nominal_maneuver_lateral_plan_ubx.fcn.layout.dim.push_back(std_msgs::MultiArrayDimension());
218  msg.nominal_maneuver_lateral_plan_ubx.fcn.layout.dim[0].label = "time";
219  msg.nominal_maneuver_lateral_plan_ubx.fcn.layout.dim[0].size = NC_lat_ubx;
220  msg.nominal_maneuver_lateral_plan_ubx.fcn.layout.dim[0].stride = NR_lat_ubx * NC_lat_ubx;
221  msg.nominal_maneuver_lateral_plan_ubx.fcn.layout.dim.push_back(std_msgs::MultiArrayDimension());
222  msg.nominal_maneuver_lateral_plan_ubx.fcn.layout.dim[1].label = "upper bound of n and derivatives";
223  msg.nominal_maneuver_lateral_plan_ubx.fcn.layout.dim[1].size = (NR_lat_ubx - 1) * NC_lat_ubx;
224  msg.nominal_maneuver_lateral_plan_ubx.fcn.layout.dim[1].stride = NC_lat_ubx;
225  }
226  // lower bound
227  auto& lateral_lbx = result.nominal_maneuver_lateral_lbx.getData();
228  const int NC_lat_lbx = lateral_lbx.nc();
229  const int NR_lat_lbx = lateral_lbx.nr();
230  if (NC_lat_lbx > 0 && NR_lat_lbx > 1)
231  {
232  msg.nominal_maneuver_lateral_plan_lbx.fcn.layout.data_offset = 0;
233  msg.nominal_maneuver_lateral_plan_lbx.fcn.data.insert(
234  msg.nominal_maneuver_lateral_plan_lbx.fcn.data.end(), lateral_lbx.begin(), lateral_lbx.end());
235  msg.nominal_maneuver_lateral_plan_lbx.fcn.layout.dim.push_back(std_msgs::MultiArrayDimension());
236  msg.nominal_maneuver_lateral_plan_lbx.fcn.layout.dim[0].label = "time";
237  msg.nominal_maneuver_lateral_plan_lbx.fcn.layout.dim[0].size = NC_lat_lbx;
238  msg.nominal_maneuver_lateral_plan_lbx.fcn.layout.dim[0].stride = NR_lat_lbx * NC_lat_lbx;
239  msg.nominal_maneuver_lateral_plan_lbx.fcn.layout.dim.push_back(std_msgs::MultiArrayDimension());
240  msg.nominal_maneuver_lateral_plan_lbx.fcn.layout.dim[1].label = "lower bound of n and derivatives";
241  msg.nominal_maneuver_lateral_plan_lbx.fcn.layout.dim[1].size = (NR_lat_lbx - 1) * NC_lat_lbx;
242  msg.nominal_maneuver_lateral_plan_lbx.fcn.layout.dim[1].stride = NC_lat_lbx;
243  }
244  return msg;
245  }
249  template <typename Tmsg>
250  void operator()(Tmsg msg, adore::fun::PlanningResult& result)
251  {
252  result.id = msg->id;
253  result.iteration = msg->iteration;
254  result.name = msg->name;
255  spr_converter_(&msg->nominal_maneuver, &result.nominal_maneuver);
256  spr_converter_(&msg->combined_maneuver, &result.combined_maneuver);
257  tr_converter_(&msg->terminal_maneuver, &result.terminal_maneuver);
258  result.nominal_maneuver_valid = msg->nominal_maneuver_valid;
259  result.combined_maneuver_valid = msg->combined_maneuver_valid;
260  result.status_string = msg->status_string;
261  result.maneuver_type = msg->maneuver_type;
262  result.indicator_left = msg->indicator_left;
263  result.indicator_right = msg->indicator_right;
264  for (auto [name, value] : msg->objective_values)
265  {
266  result.objective_values.insert({ name, value });
267  }
268  for (auto [name, value] : msg->performance_values)
269  {
270  result.performance_values.insert({ name, value });
271  }
272  // copy nominal_maneuver_swath
273  for (auto& msgi : msg->nominal_maneuver_swath.occupancy)
274  {
276  cylinder.rxy_ = msgi.rxy;
277  cylinder.x_ = msgi.x;
278  cylinder.y_ = msgi.y;
279  cylinder.t0_ = msgi.t0;
280  cylinder.t1_ = msgi.t1;
281  cylinder.z0_ = msgi.z0;
282  cylinder.z1_ = msgi.z1;
283  result.nominal_maneuver_swath.insert(cylinder);
284  }
285  // copy combined_maneuver_swath
286  for (auto& msgi : msg->combined_maneuver_swath.occupancy)
287  {
289  cylinder.rxy_ = msgi.rxy;
290  cylinder.x_ = msgi.x;
291  cylinder.y_ = msgi.y;
292  cylinder.t0_ = msgi.t0;
293  cylinder.t1_ = msgi.t1;
294  cylinder.z0_ = msgi.z0;
295  cylinder.z1_ = msgi.z1;
296  result.combined_maneuver_swath.insert(cylinder);
297  }
298 
299  // copy details on longitudinal planning
300  if (!msg->nominal_maneuver_longitudinal_plan.fcn.data.empty() &&
301  !msg->nominal_maneuver_longitudinal_plan.fcn.layout.dim.empty())
302  {
304  dlib::mat(&*msg->nominal_maneuver_longitudinal_plan.fcn.data.begin(),
305  msg->nominal_maneuver_longitudinal_plan.fcn.layout.dim[0].stride /
306  msg->nominal_maneuver_longitudinal_plan.fcn.layout.dim[0].size,
307  msg->nominal_maneuver_longitudinal_plan.fcn.layout.dim[0].size);
308  }
309  if (!msg->nominal_maneuver_longitudinal_plan_lbx.fcn.data.empty() &&
310  !msg->nominal_maneuver_longitudinal_plan_lbx.fcn.layout.dim.empty())
311  {
313  dlib::mat(&*msg->nominal_maneuver_longitudinal_plan_lbx.fcn.data.begin(),
314  msg->nominal_maneuver_longitudinal_plan_lbx.fcn.layout.dim[0].stride /
315  msg->nominal_maneuver_longitudinal_plan_lbx.fcn.layout.dim[0].size,
316  msg->nominal_maneuver_longitudinal_plan_lbx.fcn.layout.dim[0].size);
317  }
318  if (!msg->nominal_maneuver_longitudinal_plan_ubx.fcn.data.empty() &&
319  !msg->nominal_maneuver_longitudinal_plan_ubx.fcn.layout.dim.empty())
320  {
322  dlib::mat(&*msg->nominal_maneuver_longitudinal_plan_ubx.fcn.data.begin(),
323  msg->nominal_maneuver_longitudinal_plan_ubx.fcn.layout.dim[0].stride /
324  msg->nominal_maneuver_longitudinal_plan_ubx.fcn.layout.dim[0].size,
325  msg->nominal_maneuver_longitudinal_plan_ubx.fcn.layout.dim[0].size);
326  }
327  // copy details on lateral planning
328  if (!msg->nominal_maneuver_lateral_plan.fcn.data.empty() &&
329  !msg->nominal_maneuver_lateral_plan.fcn.layout.dim.empty())
330  {
332  dlib::mat(&*msg->nominal_maneuver_lateral_plan.fcn.data.begin(),
333  msg->nominal_maneuver_lateral_plan.fcn.layout.dim[0].stride /
334  msg->nominal_maneuver_lateral_plan.fcn.layout.dim[0].size,
335  msg->nominal_maneuver_lateral_plan.fcn.layout.dim[0].size);
336  }
337  if (!msg->nominal_maneuver_lateral_plan_lbx.fcn.data.empty() &&
338  !msg->nominal_maneuver_lateral_plan_lbx.fcn.layout.dim.empty())
339  {
341  dlib::mat(&*msg->nominal_maneuver_lateral_plan_lbx.fcn.data.begin(),
342  msg->nominal_maneuver_lateral_plan_lbx.fcn.layout.dim[0].stride /
343  msg->nominal_maneuver_lateral_plan_lbx.fcn.layout.dim[0].size,
344  msg->nominal_maneuver_lateral_plan_lbx.fcn.layout.dim[0].size);
345  }
346  if (!msg->nominal_maneuver_lateral_plan_ubx.fcn.data.empty() &&
347  !msg->nominal_maneuver_lateral_plan_ubx.fcn.layout.dim.empty())
348  {
350  dlib::mat(&*msg->nominal_maneuver_lateral_plan_ubx.fcn.data.begin(),
351  msg->nominal_maneuver_lateral_plan_ubx.fcn.layout.dim[0].stride /
352  msg->nominal_maneuver_lateral_plan_ubx.fcn.layout.dim[0].size,
353  msg->nominal_maneuver_lateral_plan_ubx.fcn.layout.dim[0].size);
354  }
355  }
356  };
357  } // namespace if_ROS
358 } // namespace adore
adoreMatrix< T, n+1, 0 > & getData()
Definition: llinearpiecewisefunction.h:147
void insert(const VolumeType &volume)
Definition: vectorbasedvolumetree.h:90
VolumeVector & getLevel(int i)
Definition: vectorbasedvolumetree.h:60
Definition: areaofeffectconverter.h:20
Definition: planning_result.h:29
std::unordered_map< std::string, double > performance_values
Definition: planning_result.h:71
adore::mad::LLinearPiecewiseFunctionM< double, 3 > nominal_maneuver_lateral_lbx
Definition: planning_result.h:56
bool combined_maneuver_valid
Definition: planning_result.h:64
adore::mad::LLinearPiecewiseFunctionM< double, 4 > nominal_maneuver_longitudinal_plan
Definition: planning_result.h:43
std::string name
Definition: planning_result.h:32
adore::mad::LLinearPiecewiseFunctionM< double, 4 > nominal_maneuver_lateral_plan
Definition: planning_result.h:54
adore::mad::LLinearPiecewiseFunctionM< double, 3 > nominal_maneuver_longitudinal_lbx
Definition: planning_result.h:46
std::string status_string
Definition: planning_result.h:65
adore::mad::LLinearPiecewiseFunctionM< double, 3 > nominal_maneuver_lateral_ubx
Definition: planning_result.h:59
std::unordered_map< std::string, double > objective_values
Definition: planning_result.h:68
int id
Definition: planning_result.h:30
int maneuver_type
Definition: planning_result.h:72
adore::mad::OccupancyCylinderTree combined_maneuver_swath
Definition: planning_result.h:40
bool indicator_right
Definition: planning_result.h:77
bool nominal_maneuver_valid
Definition: planning_result.h:63
int iteration
Definition: planning_result.h:31
adore::mad::OccupancyCylinderTree nominal_maneuver_swath
Definition: planning_result.h:38
TerminalRequest terminal_maneuver
Definition: planning_result.h:37
bool indicator_left
Definition: planning_result.h:76
SetPointRequest nominal_maneuver
Definition: planning_result.h:33
SetPointRequest combined_maneuver
Definition: planning_result.h:35
adore::mad::LLinearPiecewiseFunctionM< double, 3 > nominal_maneuver_longitudinal_ubx
Definition: planning_result.h:50
Definition: planningresultconverter.h:28
void operator()(Tmsg msg, adore::fun::PlanningResult &result)
Definition: planningresultconverter.h:250
adore_if_ros_msg::PlanningResult operator()(const adore::fun::PlanningResult &result)
Definition: planningresultconverter.h:60
TerminalRequestConverter tr_converter_
Definition: planningresultconverter.h:30
SetPointRequestConverter spr_converter_
Definition: planningresultconverter.h:29
Definition: setpointrequestconverter.h:39
Definition: terminalrequestconverter.h:39
Definition: occupancycylinder.h:25
double rxy_
Definition: occupancycylinder.h:26
double t0_
Definition: occupancycylinder.h:28
double y_
Definition: occupancycylinder.h:27
double z1_
Definition: occupancycylinder.h:29
double t1_
Definition: occupancycylinder.h:28
double z0_
Definition: occupancycylinder.h:29
double x_
Definition: occupancycylinder.h:27