plan_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/Plan.h>
33 #include <marti_nav_msgs/PlanPosition.h>
35 
36 namespace swri_route_util
37 {
38 // Transform a plan. The path will be transformed in place using
39 // the supplied transform. The
40 // frame_id of the transformed route will be set to the required
41 // target_frame argument, because forgetting to up the frame_id has
42 // caused difficult bugs several times.
43 void transform(marti_nav_msgs::Plan &path,
45  const std::string &target_frame);
46 
47 // Fill in the orientation of the path points using an estimate from
48 // the path geometry. This function
49 // assumes the route is in a cartesian (e.g. not WGS84) frame.
50 void fillOrientations(marti_nav_msgs::Plan &path);
51 
52 // Set z's to zeros
53 void projectToXY(marti_nav_msgs::Plan &route);
54 
55 
56 // Finds the nearest point on a path to a given position starting
57 // from the first point in the path
59  const marti_nav_msgs::Plan& path,
60  const double x, const double y,
61  marti_nav_msgs::PlanPosition& nearest_position,
62  double& nearest_separation);
63 
64 // Normalizes a position along a plan so that it refers to the closest point
65 void normalizePlanPosition(marti_nav_msgs::PlanPosition& position,
66  const marti_nav_msgs::Plan& path);
67 
68 // Gets a transform representing a position along a route
69 // Automatically normalizes the path positions
70 void getPathPose(const marti_nav_msgs::Plan& path,
71  const marti_nav_msgs::PlanPosition position,
73  const bool allow_extrapolation = false);
74 
75 // Gets a vector3 representing a position along a route
76 // Automatically normalizes the path positions
77 void getPlanPosition(const marti_nav_msgs::Plan& path,
78  const marti_nav_msgs::PlanPosition position,
79  tf::Vector3& tf,
80  bool extrapolate = false);
81 
82 // Gets an interpolated planpoint at the given position
83 void interpolatePlanPosition(const marti_nav_msgs::Plan& path,
84  const marti_nav_msgs::PlanPosition position,
85  marti_nav_msgs::PlanPoint& pt,
86  bool extrapolate = false);
87 
88 // Small utility function to make a tf::Vector3 from a PlanPoint
89 inline tf::Vector3 getPointPosition(const marti_nav_msgs::PlanPoint& pt)
90 {
91  return tf::Vector3(pt.x, pt.y, pt.z);
92 }
93 
94 // Gets the nearest PlanPosition for a given point along a plan
95 bool projectOntoPlan(marti_nav_msgs::PlanPosition &position,
96  const marti_nav_msgs::Plan &route,
97  const tf::Vector3 &point,
98  bool extrapolate_before_start,
99  bool extrapolate_past_end);
100 
101 // Gets the nearest PlanPosition for a given point along a subsection of a plan
103  marti_nav_msgs::PlanPosition &position,
104  const marti_nav_msgs::Plan &route,
105  const tf::Vector3 &point,
106  const marti_nav_msgs::PlanPosition &window_start,
107  const marti_nav_msgs::PlanPosition &window_end);
108 
109 // Get signed distance between two positions along a plan
110 // Handles both wgs84 and cartesian frames
111 bool planDistance(
112  double &distance,
113  const marti_nav_msgs::PlanPosition &start,
114  const marti_nav_msgs::PlanPosition &end,
115  const marti_nav_msgs::Plan &route);
116 }
117 #endif // SWRI_ROUTE_UTIL_UTIL_H_
void normalizePlanPosition(marti_nav_msgs::PlanPosition &position, const marti_nav_msgs::Plan &path)
Definition: plan_util.cpp:154
ROSCPP_DECL void start()
tf::Vector3 getPointPosition(const marti_nav_msgs::PlanPoint &pt)
Definition: plan_util.h:89
void projectToXY(marti_nav_msgs::Plan &route)
Definition: plan_util.cpp:59
bool projectOntoPlan(marti_nav_msgs::PlanPosition &position, const marti_nav_msgs::Plan &route, const tf::Vector3 &point, bool extrapolate_before_start, bool extrapolate_past_end)
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 getPlanPosition(const marti_nav_msgs::Plan &path, const marti_nav_msgs::PlanPosition position, tf::Vector3 &tf, bool extrapolate=false)
Definition: plan_util.cpp:568
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 interpolatePlanPosition(const marti_nav_msgs::Plan &path, const marti_nav_msgs::PlanPosition position, marti_nav_msgs::PlanPoint &pt, bool extrapolate=false)
Definition: plan_util.cpp:535
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
bool projectOntoPlanWindow(marti_nav_msgs::PlanPosition &position, const marti_nav_msgs::Plan &route, const tf::Vector3 &point, const marti_nav_msgs::PlanPosition &window_start, const marti_nav_msgs::PlanPosition &window_end)
Definition: plan_util.cpp:366
bool planDistance(double &distance, const marti_nav_msgs::PlanPosition &start, const marti_nav_msgs::PlanPosition &end, const marti_nav_msgs::Plan &route)


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