util.h
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 #ifndef SWRI_ROUTE_UTIL_UTIL_H_
30 #define SWRI_ROUTE_UTIL_UTIL_H_
31 
32 #include <marti_nav_msgs/RoutePosition.h>
34 
35 namespace swri_route_util
36 {
37 class Route;
38 class RoutePoint;
39 
40 // Transform a route. The route will be transformed in place using
41 // the supplied transform. Known property types that are affected by
42 // the transform are expceted to be transformed properly. The
43 // frame_id of the transformed route will be set to the required
44 // target_frame argument, because forgetting to up the frame_id has
45 // caused difficult bugs several times.
46 void transform(Route &route,
48  const std::string &target_frame);
49 
50 
51 // Project a route to the XY plane by setting the Z coordinate to zero.
52 void projectToXY(Route &route);
53 
54 
55 // Fill in the orientation of the route points using an estimate from
56 // the route geometry and desired "up" direction. This function
57 // assumes the route is in a cartesian (e.g. not WGS84) frame.
58 void fillOrientations(Route &route,
59  const tf::Vector3 &up=tf::Vector3(0.0, 0.0, 1.0));
60 
61 
62 // Find the closest point on the route (as a route position) for a
63 // given point. If extrapolate_before_start and/or
64 // extrapolate_past_end are true, the projection will consider the
65 // first and last segments to extend infinitely (ONLY if the point is
66 // nearest to either without extrapolation). This function assumes
67 // the route is in a cartesian (e.g. not WGS84) frame.
68 bool projectOntoRoute(marti_nav_msgs::RoutePosition &position,
69  const Route &route,
70  const tf::Vector3 &point,
71  bool extrapolate_before_start,
72  bool extrapolate_past_end);
73 
74 
75 // Find the closest position on a route for a given point, restricted
76 // to a subset of the route. The subset is defined by a start and end
77 // position on the route. This function assumes the route is in a
78 // cartesian (e.g. not WGS84) frame.
79 bool projectOntoRouteWindow(marti_nav_msgs::RoutePosition &position,
80  const Route &route,
81  const tf::Vector3 &point,
82  const marti_nav_msgs::RoutePosition &window_start,
83  const marti_nav_msgs::RoutePosition &window_end);
84 
85 
86 // Normalize a route position. A normalize route position is guaranteed to
87 // have:
88 // - A valid id for a point in the route.
89 //
90 // - If the position is before the start of the route, the id will be
91 // first route point and the distance will be negative.
92 
93 // - If the position is after the end of the route, the id will be the
94 // last route point and the distance will be positive.
95 //
96 // - Otherwise, the position's id will be for the point that begins
97 // the segment containing the position, and the distance will be
98 // less than that length of that segment.
99 //
100 // This function fails if the original position's id is not found in the
101 // route. This function assumes the route is in a cartesian (e.g. not
102 // WGS84) frame.
103 bool normalizeRoutePosition(marti_nav_msgs::RoutePosition &normalized_position,
104  const Route &route,
105  const marti_nav_msgs::RoutePosition &position);
106 
107 
108 // Create a route point from a route position by interpolating between
109 // the route's points as needed. This function assumes the route is
110 // in a cartesian (e.g. not WGS84) frame.
111 bool interpolateRoutePosition(RoutePoint &point,
112  const Route &route,
113  const marti_nav_msgs::RoutePosition &position,
114  bool allow_extrapolation);
115 
116 // Return the distance between two route positions. This function
117 // works for routes defined in WGS84 or Euclidean spaces.
118 bool routeDistance(
119  double &distance,
120  const marti_nav_msgs::RoutePosition &start,
121  const marti_nav_msgs::RoutePosition &end,
122  const Route &route);
123 
124 // Return the distances between a start route position and multiple
125 // end route positions. This function works for routes defined in
126 // WGS84 or Euclidean spaces. This function returns false if the
127 // start point wasn't found in the route, in which case distances is
128 // not modified. If the function is true, the start point was found
129 // and distances is guaranteed to be the same size as ends. If an end
130 // point is not found in the route, its distance is set to NaN.
131 bool routeDistances(
132  std::vector<double> &distances,
133  const marti_nav_msgs::RoutePosition &start,
134  const std::vector<marti_nav_msgs::RoutePosition> &ends,
135  const Route &route);
136 
137 // Extracts a subroute from [start, end)
138 bool extractSubroute(
139  Route &sub_route,
140  const Route &route,
141  const marti_nav_msgs::RoutePosition &start,
142  const marti_nav_msgs::RoutePosition &end);
143 } // namespace swri_route_util
144 #endif // SWRI_ROUTE_UTIL_UTIL_H_
bool normalizeRoutePosition(marti_nav_msgs::RoutePosition &normalized_position, const Route &route, const marti_nav_msgs::RoutePosition &position)
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)
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)
void fillOrientations(Route &route, const tf::Vector3 &up=tf::Vector3(0.0, 0.0, 1.0))
Definition: util.cpp:61
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)
bool routeDistance(double &distance, const marti_nav_msgs::RoutePosition &start, const marti_nav_msgs::RoutePosition &end, const Route &route)
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


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