path_util.h
Go to the documentation of this file.
1 // *****************************************************************************
2 //
3 // Copyright (c) 2020, 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_PATH_UTIL_H_
30 #define SWRI_ROUTE_UTIL_PATH_UTIL_H_
31 
32 #include <marti_nav_msgs/Path.h>
34 
35 namespace swri_route_util
36 {
37 // Transform a path. The path will be transformed in place using
38 // the supplied transform. The
39 // frame_id of the transformed route will be set to the required
40 // target_frame argument, because forgetting to up the frame_id has
41 // caused difficult bugs several times.
42 void transform(marti_nav_msgs::Path &path,
44  const std::string &target_frame);
45 
46 // Fill in the orientation of the path points using an estimate from
47 // the path geometry. This function
48 // assumes the route is in a cartesian (e.g. not WGS84) frame.
49 void fillOrientations(marti_nav_msgs::Path &path);
50 
51 
52 // Stores a position along a path
54 {
55  size_t index;
56  double distance;
57 };
58 
59 // Finds the nearest point on a path to a given position starting
60 // from the first point in the path
62  const marti_nav_msgs::Path& path,
63  const double x, const double y,
64  PathPosition& nearest_position,
65  double& nearest_separation);
66 
67 // Gets a transform representing a position along a route
68 // Automatically normalizes the path positions
69 void getPathPose(const marti_nav_msgs::Path& path,
70  const PathPosition position,
72  const bool allow_extrapolation = false);
73 
74 // Gets a vector3 representing a position along a route
75 // Automatically normalizes the path positions
76 void getPathPosition(const marti_nav_msgs::Path& path,
77  const PathPosition position,
78  tf::Vector3& tf);
79 }
80 #endif // SWRI_ROUTE_UTIL_UTIL_H_
bool findLocalNearestDistanceForward(const marti_nav_msgs::Path &path, const double x, const double y, PathPosition &nearest_position, double &nearest_separation)
Definition: path_util.cpp:90
void getPathPose(const marti_nav_msgs::Path &path, const PathPosition position, tf::Transform &tf, const bool allow_extrapolation=false)
Definition: path_util.cpp:253
void getPathPosition(const marti_nav_msgs::Path &path, const PathPosition position, tf::Vector3 &tf)
Definition: path_util.cpp:228
void transform(marti_nav_msgs::Path &path, const swri_transform_util::Transform &transform, const std::string &target_frame)
Definition: path_util.cpp:42
void fillOrientations(marti_nav_msgs::Path &path)
Definition: path_util.cpp:59


swri_route_util
Author(s):
autogenerated on Sat Jan 21 2023 03:13:29