17 #include <adore_if_ros_msg/PlanningResultSet.h>
62 adore_if_ros_msg::PlanningResult msg;
65 msg.name = result.
name;
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);
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);
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;
96 const auto& cylinder = pair.second;
97 adore_if_ros_msg::OccupancyCylinder msgi;
98 msgi.rxy = cylinder.rxy_;
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);
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;
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);
128 const int NC_lon = longitudinal_plan.nc();
129 const int NR_lon = longitudinal_plan.nr();
130 if (NC_lon > 0 && NR_lon > 1)
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;
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)
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 "
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;
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)
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 "
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;
192 const int NC_lat = lateral_plan.nc();
193 const int NR_lat = lateral_plan.nr();
194 if (NC_lat > 0 && NR_lat > 1)
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;
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)
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;
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)
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;
249 template <
typename Tmsg>
254 result.
name = msg->name;
264 for (
auto [name, value] : msg->objective_values)
268 for (
auto [name, value] : msg->performance_values)
273 for (
auto& msgi : msg->nominal_maneuver_swath.occupancy)
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;
286 for (
auto& msgi : msg->combined_maneuver_swath.occupancy)
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;
300 if (!msg->nominal_maneuver_longitudinal_plan.fcn.data.empty() &&
301 !msg->nominal_maneuver_longitudinal_plan.fcn.layout.dim.empty())
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);
309 if (!msg->nominal_maneuver_longitudinal_plan_lbx.fcn.data.empty() &&
310 !msg->nominal_maneuver_longitudinal_plan_lbx.fcn.layout.dim.empty())
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);
318 if (!msg->nominal_maneuver_longitudinal_plan_ubx.fcn.data.empty() &&
319 !msg->nominal_maneuver_longitudinal_plan_ubx.fcn.layout.dim.empty())
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);
328 if (!msg->nominal_maneuver_lateral_plan.fcn.data.empty() &&
329 !msg->nominal_maneuver_lateral_plan.fcn.layout.dim.empty())
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);
337 if (!msg->nominal_maneuver_lateral_plan_lbx.fcn.data.empty() &&
338 !msg->nominal_maneuver_lateral_plan_lbx.fcn.layout.dim.empty())
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);
346 if (!msg->nominal_maneuver_lateral_plan_ubx.fcn.data.empty() &&
347 !msg->nominal_maneuver_lateral_plan_ubx.fcn.layout.dim.empty())
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);
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