util.cpp
Go to the documentation of this file.
1 // *****************************************************************************
2 //
3 // Copyright (c) 2016, Southwest Research Institute® (SwRI®)
4 // All rights reserved.
5 //
6 // Redistribution and use in source and binary forms, with or without
7 // modification, are permitted provided that the following conditions are met:
8 // * Redistributions of source code must retain the above copyright
9 // notice, this list of conditions and the following disclaimer.
10 // * Redistributions in binary form must reproduce the above copyright
11 // notice, this list of conditions and the following disclaimer in the
12 // documentation and/or other materials provided with the distribution.
13 // * Neither the name of Southwest Research Institute® (SwRI®) nor the
14 // names of its contributors may be used to endorse or promote products
15 // derived from this software without specific prior written permission.
16 //
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20 // ARE DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
21 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
22 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
23 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
24 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
26 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
27 //
28 // *****************************************************************************
29 #include <swri_route_util/util.h>
30 #include <swri_route_util/route.h>
32 
35 
36 namespace mnm = marti_nav_msgs;
37 namespace stu = swri_transform_util;
38 
39 namespace swri_route_util
40 {
41 void transform(Route &route,
43  const std::string &target_frame)
44 {
45  for (auto &point : route.points) {
46  point.setPosition(transform*point.position());
47  point.setOrientation(transform*point.orientation());
48  }
49  route.header.frame_id = target_frame;
50 }
51 
52 void projectToXY(Route &route)
53 {
54  for (auto &point : route.points) {
55  point.position().setZ(0.0);
56  // todo(exjohnson): if orientation is valid, project it to a
57  // rotation around the Z axis only.
58  }
59 }
60 
61 void fillOrientations(Route &route, const tf::Vector3 &up)
62 {
63  // We can't estimate any orientations for 0 or 1 points.
64  if (route.points.size() < 2) {
65  return;
66  }
67 
68  std::vector<size_t> degenerate_orientations;
69 
70  for (size_t i = 0; i < route.points.size(); ++i) {
71  // We're going to estimate the orientation by the center difference using
72  // the vector from the previous point to the next point in the route. This
73  // assumes that the points are evenly spaced, but it is a reasonable
74  // estimate even if they are not.
75  tf::Vector3 v_forward;
76  if (i == 0) {
77  // For the first point, we use the forward difference
78  v_forward = route.points[i+1].position() - route.points[i+0].position();
79  } else if (i+1 == route.points.size()) {
80  // For the last point, we use the backward difference
81  v_forward = route.points[i+0].position() - route.points[i-1].position();
82  } else {
83  v_forward = route.points[i+1].position() - route.points[i-1].position();
84  }
85 
86  v_forward.normalize();
87 
88  // Y = Z x X
89  tf::Vector3 v_left = up.cross(v_forward);
90  // Since Z and X are not necessarily orthogonal, we need to normalize this
91  // to get a unit vector. This is where we'll have problems if our
92  // v_forward happens to be really closely aligned with the up
93  // axis. We ignore that.
94  v_left.normalize();
95 
96  if (std::isnan(v_left.x()))
97  {
98  // This should catch issues with repeated route points, and co-linear
99  // v_forward and up vectors (though the latter should never happen)
100  degenerate_orientations.push_back(i);
101  }
102 
103 
104  // We now have left unit vector that is perpendicular to the
105  // forward unit vector, so we can find our actual up vector, which
106  // should be in the plane spanned by v_forward and the user
107  // provided up direction.
108  tf::Vector3 v_up = v_forward.cross(v_left);
109  // We shouldn't need to normalize v_up, but it's good practice
110  // since I don't know if the Matrix3x3 handles errors well.
111  v_up.normalize();
112 
113  // Don't understand why Matrix3x3 doesn't have a constructor for 3
114  // vectors.
115  tf::Matrix3x3 rotation(
116  v_forward.x(), v_left.x(), v_up.x(),
117  v_forward.y(), v_left.y(), v_up.y(),
118  v_forward.z(), v_left.z(), v_up.z());
119 
120  // Finally we can extract the orientation as a quaternion from the
121  // matrix.
122  tf::Quaternion orientation;
123  rotation.getRotation(orientation);
124  route.points[i].setOrientation(orientation);
125 
126  // There is probably a simpler, more elegant way to do this.
127  }
128 
129  // If there are any degenerate orientations, assign the same orientation as
130  // the nearest neighbor. There is probably a better way to do this.
131  for (size_t i = 0; i < degenerate_orientations.size(); ++i)
132  {
133  size_t d_idx = degenerate_orientations[i];
134  bool repaired_orientation = false;
135  for (size_t j = 1; j < route.points.size(); ++j)
136  {
137  size_t up_idx = d_idx + j;
138  int64_t down_idx = (int64_t)d_idx - (int64_t)j;
139  if (up_idx < route.points.size())
140  {
141  if (std::find(degenerate_orientations.begin(), degenerate_orientations.end(), up_idx) == degenerate_orientations.end())
142  {
143  // Found a neighboring point with valid orientation.
144  route.points[d_idx].setOrientation(route.points[up_idx].orientation());
145  repaired_orientation = true;
146  break;
147  }
148  }
149 
150  if (down_idx >= 0)
151  {
152  if (std::find(degenerate_orientations.begin(), degenerate_orientations.end(), down_idx) == degenerate_orientations.end())
153  {
154  // Found a neighboring point with valid orientation.
155  route.points[d_idx].setOrientation(route.points[down_idx].orientation());
156  repaired_orientation = true;
157  break;
158  }
159  }
160  }
161 
162  if (!repaired_orientation)
163  {
164  ROS_ERROR_THROTTLE(1.0, "fillOrientations was unable to repair an invalid "
165  "orientation. The route may be malformed.");
166  }
167  else
168  {
169  ROS_WARN_THROTTLE(1.0, "fillOrientations found and repaired an invalid "
170  "orientation. Note that the source route may contain "
171  "repeated points.");
172  }
173  }
174 }
175 
176 // This is a private helper function that finds the nearest distance
177 // between a point p and the line segment defined by p0 ad p1. It
178 // also returns the distance along the segment to the point that is
179 // nearest to p. If extrapolate_start/extrapolate_end are true, the
180 // calculation considers the line segment to extend infinitely in the
181 // corresponding directions.
182 static
184  double &min_distance_from_line,
185  double &min_distance_on_line,
186  const tf::Vector3 &p0,
187  const tf::Vector3 &p1,
188  const tf::Vector3 &p,
189  bool extrapolate_start,
190  bool extrapolate_end)
191 {
192  tf::Vector3 v = p1 - p0;
193  const double v_len_sq = v.dot(v);
194 
195  // s will be the normalized distance along v that is closest to the
196  // desired point.
197  double s = 0.0;
198  if (v_len_sq > 1e-6) {
199  s = v.dot(p - p0) / v_len_sq;
200  } else {
201  // The two points are too close to define a reasonable line
202  // segment, so just pick p1 as the closest point.
203  s = 1.0;
204  }
205 
206  // If we don't allow extrapolation and the nearest point is beyond
207  // the line segment boundaries, we need to clamp to the boundary.
208  if (!extrapolate_start && s < 0.0) {
209  s = 0.0;
210  } else if (!extrapolate_end && s > 1.0) {
211  s = 1.0;
212  }
213 
214  tf::Vector3 x_nearest = p0 + s*v;
215 
216  min_distance_from_line = x_nearest.distance(p);
217  min_distance_on_line = s*std::sqrt(v_len_sq);
218 }
219 
220 bool projectOntoRoute(mnm::RoutePosition &position,
221  const Route &route,
222  const tf::Vector3 &point,
223  bool extrapolate_before_start,
224  bool extrapolate_past_end)
225 {
226  if (route.points.size() == 0) {
227  // We can't do anything with this.
228  return false;
229  }
230 
231  if (route.points.size() == 1) {
232  // We can't do much with this.
233  position.route_id = route.guid();
234  position.id = route.points[0].id();
235  position.distance = 0.0;
236  return true;
237  }
238 
239  // First we find the nearest point on the route, without allowing
240  // extrapolation.
241  double min_distance_from_line = std::numeric_limits<double>::infinity();
242  double min_distance_on_line = std::numeric_limits<double>::infinity();
243  size_t min_segment_index = 0;
244 
245  for (size_t i = 0; i+1 < route.points.size(); ++i) {
246  double distance_from_line;
247  double distance_on_line;
248 
249  nearestDistanceToLineSegment(distance_from_line,
250  distance_on_line,
251  route.points[i+0].position(),
252  route.points[i+1].position(),
253  point,
254  false, false);
255 
256  if (distance_from_line <= min_distance_from_line) {
257  min_segment_index = i;
258  min_distance_on_line = distance_on_line;
259  min_distance_from_line = distance_from_line;
260  }
261  }
262 
263  // If the nearest segment is the first or last segment, we redo
264  // the search while allowing the segment to be extrapolated
265  // backward or forward, respectively. This allows graceful
266  // operation if the vehicle is past the boundary of the route.
267 
268  if (extrapolate_before_start && min_segment_index == 0) {
269  size_t i = 0;
270  nearestDistanceToLineSegment(min_distance_from_line,
271  min_distance_on_line,
272  route.points[i+0].position(),
273  route.points[i+1].position(),
274  point,
275  true, false);
276  } else if (min_segment_index + 2 == route.points.size()) {
277  // The end of the route is a special case. If we go past the end,
278  // we want to return a position with the id of the last point and
279  // the distance past it. This annoying complicates things in a
280  // number of places, but makes it easy to check if a point is past
281  // the end of a route.
282  size_t i = min_segment_index;
283  nearestDistanceToLineSegment(min_distance_from_line,
284  min_distance_on_line,
285  route.points[i+0].position(),
286  route.points[i+1].position(),
287  point,
288  false, true);
289 
290  double last_length = (route.points[i+1].position() - route.points[i+0].position()).length();
291  if (min_distance_on_line > last_length) {
292  min_segment_index++;
293  min_distance_on_line -= last_length;
294  }
295 
296  if (!extrapolate_past_end) {
297  min_distance_on_line = 0.0;
298  }
299  }
300 
301  position.route_id = route.guid();
302  position.id = route.points[min_segment_index].id();
303  position.distance = min_distance_on_line;
304  return true;
305 }
306 
308  mnm::RoutePosition &position,
309  const Route &route,
310  const tf::Vector3 &point,
311  const mnm::RoutePosition &window_start,
312  const mnm::RoutePosition &window_end)
313 {
314  if (route.points.size() < 2) {
315  // We can't do anything with this.
316  return false;
317  }
318 
319  // First we normalize the window boundaries.
320  mnm::RoutePosition start;
321  if (!normalizeRoutePosition(start, route, window_start)) {
322  return false;
323  }
324  mnm::RoutePosition end;
325  if (!normalizeRoutePosition(end, route, window_end)) {
326  return false;
327  }
328 
329  // Handle the special case where the start and end points are
330  // identical.
331  if (start.id == end.id && start.distance == end.distance) {
332  position.route_id = route.guid();
333  position = start;
334  return true;
335  }
336 
337  // Find the indices of the start and end points. Since we have
338  // normalized positions, we know they exist in the route.
339  size_t start_index;
340  route.findPointId(start_index, start.id);
341  size_t end_index;
342  route.findPointId(end_index, end.id);
343 
344  // Fix the ordering so that start comes before end.
345  if ((end_index < start_index) ||
346  (start_index == end_index && end.distance < start.distance)) {
347  std::swap(end, start);
348  std::swap(start_index, end_index);
349  }
350 
351  // If either of the points are past the end of the route, we want to
352  // back them up to the previous segment to reduce the number of
353  // special cases we have to handle.
354  if (start_index+1 == route.points.size()) {
355  start_index -= 1;
356  start.id = route.points[start_index].id();
357  start.distance += (route.points[start_index+1].position() -
358  route.points[start_index+0].position()).length();
359  }
360  if (end_index+1 == route.points.size()) {
361  end_index -= 1;
362  end.id = route.points[end_index].id();
363  end.distance += (route.points[end_index+1].position() -
364  route.points[end_index+0].position()).length();
365  }
366 
367  // Although it causes a little duplication, it's easier over all to
368  // explicitly handle the special case where the window is over a
369  // single segment.
370  if (start_index == end_index) {
371  double distance_from_line;
372  double distance_on_line;
373 
374  nearestDistanceToLineSegment(distance_from_line,
375  distance_on_line,
376  route.points[start_index+0].position(),
377  route.points[start_index+1].position(),
378  point,
379  true, true);
380 
381  if (distance_on_line < start.distance) {
382  distance_on_line = start.distance;
383  } else if (distance_on_line > end.distance) {
384  distance_on_line = end.distance;
385  }
386 
387  mnm::RoutePosition denormal_position;
388  denormal_position.id = start.id;
389  denormal_position.distance = distance_on_line;
390  if (!normalizeRoutePosition(position, route, denormal_position)) {
391  return false;
392  }
393 
394  position.route_id = route.guid();
395  return true;
396  }
397 
398  // Find the nearest point on the route, without allowing
399  // extrapolation.
400  double min_distance_from_line = std::numeric_limits<double>::infinity();
401  double min_distance_on_line = std::numeric_limits<double>::infinity();
402  size_t min_segment_index = 0;
403 
404  for (size_t i = start_index; i <= end_index; ++i) {
405  double distance_from_line;
406  double distance_on_line;
407 
408  nearestDistanceToLineSegment(distance_from_line,
409  distance_on_line,
410  route.points[i+0].position(),
411  route.points[i+1].position(),
412  point,
413  false, false);
414 
415  if (distance_from_line <= min_distance_from_line) {
416  min_segment_index = i;
417  min_distance_on_line = distance_on_line;
418  min_distance_from_line = distance_from_line;
419  }
420  }
421 
422  // We have identified the closest segment. We need to clamp it
423  // to the window boundaries.
424  if (min_segment_index == start_index) {
425  nearestDistanceToLineSegment(min_distance_from_line,
426  min_distance_on_line,
427  route.points[min_segment_index+0].position(),
428  route.points[min_segment_index+1].position(),
429  point,
430  true, false);
431  if (min_distance_on_line < start.distance) {
432  min_distance_on_line = start.distance;
433  }
434  } else if (min_segment_index == end_index) {
435  nearestDistanceToLineSegment(min_distance_from_line,
436  min_distance_on_line,
437  route.points[min_segment_index+0].position(),
438  route.points[min_segment_index+1].position(),
439  point,
440  false, true);
441  if (min_distance_on_line > end.distance) {
442  min_distance_on_line = end.distance;
443  }
444  }
445 
446  mnm::RoutePosition denormal_position;
447  denormal_position.id = route.points[min_segment_index].id();
448  denormal_position.distance = min_distance_on_line;
449  if (!normalizeRoutePosition(position, route, denormal_position)) {
450  return false;
451  }
452 
453  position.route_id = route.guid();
454  return true;
455 }
456 
457 static
459  RoutePoint &dst,
460  const RoutePoint &p0,
461  const RoutePoint &p1,
462  double distance)
463 {
464  double len = (p0.position()-p1.position()).length();
465 
466  double s;
467  if (len > 1e-6) {
468  s = distance / len;
469  } else {
470  // This is a degenerate case where the points are too close
471  // together to define a numerically stable route point.
472  if (distance < 0) {
473  // If the distance is negative, the interpolated value will be
474  // the first route point.
475  s = 0.0;
476  } else if (distance > 1) {
477  // If the distance is positive, the interpolated value will be
478  // the second route point.
479  s = 1.0;
480  } else {
481  // Otherwise just take the center of the two points.
482  s = 0.5;
483  }
484  }
485 
486  dst.setPosition((1.0-s)*p0.position() + s*p1.position());
487  dst.setOrientation(p0.orientation().slerp(p1.orientation(), s));
488 
489  // Interpolate other known properties here.
490 }
491 
492 bool normalizeRoutePosition(mnm::RoutePosition &normalized_position,
493  const Route &route,
494  const mnm::RoutePosition &position)
495 {
496  size_t index;
497  if (!route.findPointId(index, position.id)) {
498  return false;
499  }
500 
501  double distance = position.distance;
502  while (distance < 0.0) {
503  // We can't go past the start of the route.
504  if (index == 0) {
505  break;
506  }
507 
508  // The distance is still negative, so we can't be on this
509  // segment. Move to the preceding segment.
510  distance += (route.points[index-0].position() -
511  route.points[index-1].position()).length();
512  index--;
513  }
514 
515  while (distance > 0.0) {
516  // We can't go past the end of the route.
517  if (index+1 == route.points.size()) {
518  break;
519  }
520 
521  double segment_length = (route.points[index+0].position() -
522  route.points[index+1].position()).length();
523  if (distance > segment_length) {
524  // The distance is greater than this segment length, so we're
525  // not on this segment. Move to the following segment.
526  distance -= segment_length;
527  index++;
528  } else {
529  // The distance is within the length of this segment, so the
530  // point is on this segment.
531  break;
532  }
533  }
534 
535  normalized_position.route_id = position.route_id;
536  normalized_position.distance = distance;
537  normalized_position.id = route.points[index].id();
538  return true;
539 }
540 
541 
543  const Route &route,
544  const mnm::RoutePosition &position,
545  bool allow_extrapolation)
546 {
547  mnm::RoutePosition norm_position;
548  if (!normalizeRoutePosition(norm_position, route, position)) {
549  return false;
550  }
551 
552  // Since we have a normalized position, we know it exists in the route.
553  size_t index;
554  route.findPointId(index, norm_position.id);
555 
556  // Special case when the point is before the start of the route.
557  if (index == 0 && norm_position.distance < 0.0) {
558  if (!allow_extrapolation) {
559  return false;
560  }
561 
562  if (route.points.size() < 2) {
563  // This route point is before the start of the route and we
564  // don't have enough information to extrapolate.
565  return false;
566  }
567 
569  route.points[0],
570  route.points[1],
571  norm_position.distance);
572  return true;
573  }
574 
575  // Special case when the point is after the end of the route.
576  if (index+1 == route.points.size() && norm_position.distance > 0.0) {
577  if (!allow_extrapolation) {
578  return false;
579  }
580  if (route.points.size() < 2) {
581  // This route point is after the end of the route and we don't
582  // have enough information to extrapolate.
583  return false;
584  }
585 
586  const RoutePoint &p0 = route.points[index-1];
587  const RoutePoint &p1 = route.points[index-0];
588  double extra_dist = (p1.position() - p0.position()).length();
589 
591  p0,
592  p1,
593  norm_position.distance + extra_dist);
594  return true;
595  }
596 
598  route.points[index+0],
599  route.points[index+1],
600  norm_position.distance);
601  return true;
602 }
603 
605  double &distance,
606  const mnm::RoutePosition &start,
607  const mnm::RoutePosition &end,
608  const Route &route)
609 {
610  size_t start_index;
611  if (!route.findPointId(start_index, start.id)) {
612  return false;
613  }
614 
615  size_t end_index;
616  if (!route.findPointId(end_index, end.id)) {
617  return false;
618  }
619 
620  size_t min_index = std::min(start_index, end_index);
621  size_t max_index = std::max(start_index, end_index);
622 
623  double d = 0.0;
624  if (route.header.frame_id == stu::_wgs84_frame) {
625  for (size_t i = min_index; i < max_index; i++) {
626  d += stu::GreatCircleDistance(route.points[i+1].position(), route.points[i].position());
627  }
628  } else {
629  for (size_t i = min_index; i < max_index; i++) {
630  d += (route.points[i+1].position() - route.points[i].position()).length();
631  }
632  }
633 
634  if (end_index < start_index) {
635  d = -d;
636  }
637 
638  distance = d + end.distance - start.distance;
639  return true;
640 }
641 
643  std::vector<double> &distances,
644  const mnm::RoutePosition &start,
645  const std::vector<mnm::RoutePosition> &ends,
646  const Route &route)
647 {
648  size_t start_index;
649  if (!route.findPointId(start_index, start.id)) {
650  // Without a start index, we can't calculate anything.
651  return false;
652  }
653 
654  // Find the indices for every point in the route, and the minimum
655  // and maximum indices that we need to work over (the ROI).
656  size_t min_index = start_index;
657  size_t max_index = start_index;
658  std::vector<int> indices;
659  indices.resize(ends.size());
660  for (size_t i = 0; i < ends.size(); ++i) {
661  size_t pt_index;
662  if (route.findPointId(pt_index, ends[i].id)) {
663  indices[i] = pt_index;
664  min_index = std::min(min_index, pt_index);
665  max_index = std::max(max_index, pt_index);
666  } else {
667  indices[i] = -1;
668  }
669  }
670 
671  // This is the index of the start point in the ROI.
672  const size_t roi_start_index = start_index - min_index;
673 
674  // We calculate the arc length of each point in the ROI relative to
675  // the start point. This vector covers the ROI (so it corresponds
676  // from min_index to max_index)
677  std::vector<double> arc_lengths;
678  arc_lengths.resize(max_index-min_index+1);
679 
680  arc_lengths[roi_start_index] = 0.0;
681  if (route.header.frame_id == stu::_wgs84_frame) {
682  // Calculate the lengths before the start point.
683  for (size_t rev_i = 1; rev_i <= roi_start_index; ++rev_i) {
684  const size_t i = roi_start_index - rev_i;
685  const tf::Vector3 pt1 = route.points[min_index+i].position();
686  const tf::Vector3 pt2 = route.points[min_index+i+1].position();
687  arc_lengths[i] = arc_lengths[i+1] - stu::GreatCircleDistance(pt1, pt2);
688  }
689  // Calculate the lengths after the start point.
690  for (size_t i = roi_start_index+1; i < arc_lengths.size(); ++i) {
691  const tf::Vector3 pt1 = route.points[min_index+i].position();
692  const tf::Vector3 pt2 = route.points[min_index+i-1].position();
693  arc_lengths[i] = arc_lengths[i-1] + stu::GreatCircleDistance(pt1, pt2);
694  }
695  } else {
696  // Assume Euclidean coordinates.
697  // Calculate the lengths before the start point.
698  for (size_t rev_i = 1; rev_i <= roi_start_index; ++rev_i) {
699  const size_t i = roi_start_index - rev_i;
700  const tf::Vector3 pt1 = route.points[min_index+i].position();
701  const tf::Vector3 pt2 = route.points[min_index+i+1].position();
702  arc_lengths[i] = arc_lengths[i+1] - (pt2-pt1).length();
703  }
704  // Calculate the lengths after the start point.
705  for (size_t i = roi_start_index+1; i < arc_lengths.size(); ++i) {
706  const tf::Vector3 pt1 = route.points[min_index+i].position();
707  const tf::Vector3 pt2 = route.points[min_index+i-1].position();
708  arc_lengths[i] = arc_lengths[i-1] + (pt2-pt1).length();
709  }
710  }
711 
712  // Now we can calculate the distances.
713  distances.resize(ends.size());
714  for (size_t i = 0; i < distances.size(); ++i) {
715  if (indices[i] < 0) {
716  distances[i] = std::numeric_limits<double>::quiet_NaN();
717  continue;
718  }
719 
720  const size_t cache_index = indices[i]-min_index;
721  distances[i] = arc_lengths[cache_index] + ends[i].distance - start.distance;
722  }
723 
724  return true;
725 }
726 
728  Route &sub_route,
729  const Route &route,
730  const marti_nav_msgs::RoutePosition &start,
731  const marti_nav_msgs::RoutePosition &end)
732 {
733  sub_route.header = route.header;
734  sub_route.properties_ = route.properties_;
735  sub_route.guid_ = route.guid_;
736  sub_route.name_ = route.name_;
737 
738  mnm::RoutePosition norm_start;
739  if (!normalizeRoutePosition(norm_start, route, start)) {
740  return false;
741  }
742 
743  mnm::RoutePosition norm_end;
744  if (!normalizeRoutePosition(norm_end, route, end)) {
745  return false;
746  }
747 
748  // Since we have a normalized position, we know it exists in the route.
749  size_t start_index;
750  route.findPointId(start_index, norm_start.id);
751 
752  size_t end_index;
753  route.findPointId(end_index, norm_end.id);
754 
755  // If the end distance is after the id point, round up to the next
756  // point.
757  if (norm_end.distance > 0.0) {
758  end_index += 1;
759  }
760 
761  // Increment the end_index so that we can iterate from [start, end),
762  // and make sure we stay within the array bounds.
763  end_index++;
764  end_index = std::min(end_index, route.points.size());
765 
766  if (end_index <= start_index)
767  {
768  sub_route.points.clear();
769  sub_route.rebuildPointIndex();
770  return true;
771  }
772 
773  sub_route.points.reserve(end_index - start_index);
774  for (size_t i = start_index; i < end_index; i++) {
775  sub_route.points.push_back(route.points[i]);
776  }
777  sub_route.rebuildPointIndex();
778 
779  return true;
780 }
781 } // namespace swri_route_util
d
void rebuildPointIndex() const
Definition: route.cpp:236
std::string guid() const
Definition: route.cpp:226
bool normalizeRoutePosition(marti_nav_msgs::RoutePosition &normalized_position, const Route &route, const marti_nav_msgs::RoutePosition &position)
std_msgs::Header header
Definition: route.h:77
std::map< std::string, std::string > properties_
Definition: route.h:161
void transform(Route &route, const swri_transform_util::Transform &transform, const std::string &target_frame)
Definition: util.cpp:41
bool routeDistances(std::vector< double > &distances, const marti_nav_msgs::RoutePosition &start, const std::vector< marti_nav_msgs::RoutePosition > &ends, const Route &route)
XmlRpcServer s
bool interpolateRoutePosition(RoutePoint &point, const Route &route, const marti_nav_msgs::RoutePosition &position, bool allow_extrapolation)
bool projectOntoRoute(marti_nav_msgs::RoutePosition &position, const Route &route, const tf::Vector3 &point, bool extrapolate_before_start, bool extrapolate_past_end)
TFSIMD_FORCE_INLINE tfScalar distance(const Vector3 &v) const
TFSIMD_FORCE_INLINE tfScalar distance(const Vector3 &v) const
Quaternion slerp(const Quaternion &q, const tfScalar &t) const
TFSIMD_FORCE_INLINE Vector3 cross(const Vector3 &v) const
void setPosition(const tf::Vector3 &position)
std::string name_
Definition: route.h:165
void fillOrientations(Route &route, const tf::Vector3 &up=tf::Vector3(0.0, 0.0, 1.0))
Definition: util.cpp:61
TFSIMD_FORCE_INLINE const tfScalar & x() const
TFSIMD_FORCE_INLINE tfScalar dot(const Vector3 &v) const
TFSIMD_FORCE_INLINE const tfScalar & z() const
#define ROS_WARN_THROTTLE(period,...)
std::string guid_
Definition: route.h:164
#define ROS_ERROR_THROTTLE(period,...)
bool projectOntoRouteWindow(marti_nav_msgs::RoutePosition &position, const Route &route, const tf::Vector3 &point, const marti_nav_msgs::RoutePosition &window_start, const marti_nav_msgs::RoutePosition &window_end)
TFSIMD_FORCE_INLINE const tfScalar & y() const
bool findPointId(size_t &index, const std::string &id) const
Definition: route.cpp:113
std::vector< RoutePoint > points
Definition: route.h:82
TFSIMD_FORCE_INLINE Vector3 & normalize()
static void nearestDistanceToLineSegment(double &min_distance_from_line, double &min_distance_on_line, const tf::Vector3 &p0, const tf::Vector3 &p1, const tf::Vector3 &p, bool extrapolate_start, bool extrapolate_end)
Definition: util.cpp:183
bool routeDistance(double &distance, const marti_nav_msgs::RoutePosition &start, const marti_nav_msgs::RoutePosition &end, const Route &route)
static void interpolateRouteSegment(RoutePoint &dst, const RoutePoint &p0, const RoutePoint &p1, double distance)
Definition: util.cpp:458
void setOrientation(const tf::Quaternion &orientation)
TFSIMD_FORCE_INLINE tfScalar length(const Quaternion &q)
const tf::Vector3 & position() const
void projectToXY(Route &route)
Definition: util.cpp:52
bool extractSubroute(Route &sub_route, const Route &route, const marti_nav_msgs::RoutePosition &start, const marti_nav_msgs::RoutePosition &end)
Definition: util.cpp:727
const tf::Quaternion & orientation() const


swri_route_util
Author(s):
autogenerated on Fri Jun 7 2019 22:06:09