Classes | Typedefs | Functions
nav_2d_utils Namespace Reference

Classes

class  OdomSubscriber
 
class  PluginMux
 An organizer for switching between multiple different plugins of the same type. More...
 
class  PolygonParseException
 Exception to throw when Polygon doesn't load correctly. More...
 

Typedefs

using PoseList = std::vector< geometry_msgs::Pose2D >
 

Functions

void addPose (nav_2d_msgs::Path2D &path, double x, double y, double theta=0.0)
 Convenience function to add a pose to a path in one line. More...
 
nav_2d_msgs::Path2D adjustPlanResolution (const nav_2d_msgs::Path2D &global_plan_in, double resolution)
 Increase plan resolution to match that of the costmap by adding points linearly between points. More...
 
void calculateMinAndMaxDistances (const nav_2d_msgs::Polygon2D &polygon, double &min_dist, double &max_dist)
 Calculate the minimum and maximum distance from (0, 0) to any point on the polygon. More...
 
nav_2d_msgs::Path2D compressPlan (const nav_2d_msgs::Path2D &input_path, double epsilon=0.1)
 Decrease the length of the plan by eliminating colinear points. More...
 
PoseList compressPlan (const PoseList &input, unsigned int start_index, unsigned int end_index, double epsilon)
 Helper function for other version of compressPlan. More...
 
double distanceToLine (double pX, double pY, double x0, double y0, double x1, double y1)
 Distance from point (pX, pY) to closest point on line from (x0, y0) to (x1, y1) More...
 
bool equals (const nav_2d_msgs::Polygon2D &polygon0, const nav_2d_msgs::Polygon2D &polygon1)
 check if two polygons are equal. Used in testing More...
 
nav_2d_msgs::Polygon2D footprintFromParams (ros::NodeHandle &nh, bool write=true)
 Load the robot footprint either as a polygon from the footprint parameter or from robot_radius. More...
 
nav_grid::NavGridInfo fromMsg (const nav_2d_msgs::NavGridInfo &msg)
 
nav_core2::UIntBounds fromMsg (const nav_2d_msgs::UIntBounds &msg)
 
double getNumberFromXMLRPC (XmlRpc::XmlRpcValue &value)
 Helper function. Convert value to double. More...
 
std::vector< double > getNumberVectorFromXMLRPC (XmlRpc::XmlRpcValue &value)
 Helper function. Convert value to double array. More...
 
double getPlanLength (const nav_2d_msgs::Path2D &plan, const unsigned int start_index=0)
 Calculate the length of the plan, starting from the given index. More...
 
double getPlanLength (const nav_2d_msgs::Path2D &plan, const geometry_msgs::Pose2D &query_pose)
 Calculate the length of the plan from the pose on the plan closest to the given pose. More...
 
nav_grid::NavGridInfo infoToInfo (const nav_msgs::MapMetaData &metadata)
 
nav_msgs::MapMetaData infoToInfo (const nav_grid::NavGridInfo &info)
 
bool isInside (const nav_2d_msgs::Polygon2D &polygon, const double x, const double y)
 Check if a given point is inside of a polygon. More...
 
template<class param_t >
param_t loadParameterWithDeprecation (const ros::NodeHandle &nh, const std::string current_name, const std::string old_name, const param_t &default_value)
 Load a parameter from one of two namespaces. Complain if it uses the old name. More...
 
template<class param_t >
void moveDeprecatedParameter (const ros::NodeHandle &nh, const std::string current_name, const std::string old_name)
 If a deprecated parameter exists, complain and move to its new location. More...
 
template<class param_t >
void moveParameter (const ros::NodeHandle &nh, std::string old_name, std::string current_name, param_t default_value, bool should_delete=true)
 Move a parameter from one place to another. More...
 
nav_2d_msgs::Polygon2D movePolygonToPose (const nav_2d_msgs::Polygon2D &polygon, const geometry_msgs::Pose2D &pose)
 Translate and rotate a polygon to a new pose. More...
 
std::vector< std::vector< double > > parseVVD (const std::string &input)
 Parse a vector of vectors of doubles from a string. Syntax is [[1.0, 2.0], [3.3, 4.4, 5.5], ...]. More...
 
nav_2d_msgs::Path2D pathToPath (const nav_msgs::Path &path)
 
nav_msgs::Path pathToPath (const nav_2d_msgs::Path2D &path2d)
 
nav_2d_msgs::Polygon2D polygonFromParallelArrays (const std::vector< double > &xs, const std::vector< double > &ys)
 Create a polygon from two parallel arrays. More...
 
nav_2d_msgs::Polygon2D polygonFromParams (const ros::NodeHandle &nh, const std::string parameter_name, bool search=true)
 Load a polygon from a parameter, whether it is a string or array, or two arrays. More...
 
nav_2d_msgs::Polygon2D polygonFromRadius (const double radius, const unsigned int num_points=16)
 Create a "circular" polygon from a given radius. More...
 
nav_2d_msgs::Polygon2D polygonFromString (const std::string &polygon_string)
 Make a polygon from the given string. Format should be bracketed array of arrays of doubles, like so: [[1.0, 2.2], [3.3, 4.2], ...]. More...
 
nav_2d_msgs::Polygon2D polygonFromXMLRPC (XmlRpc::XmlRpcValue &polygon_xmlrpc)
 Create a polygon from the given XmlRpcValue. More...
 
void polygonToParallelArrays (const nav_2d_msgs::Polygon2D polygon, std::vector< double > &xs, std::vector< double > &ys)
 Create two parallel arrays from a polygon. More...
 
void polygonToParams (const nav_2d_msgs::Polygon2D &polygon, const ros::NodeHandle &nh, const std::string parameter_name, bool array_of_arrays=true)
 Save a polygon to a parameter. More...
 
XmlRpc::XmlRpcValue polygonToXMLRPC (const nav_2d_msgs::Polygon2D &polygon, bool array_of_arrays=true)
 Create XMLRPC Value for writing the polygon to the parameter server. More...
 
geometry_msgs::Pose pose2DToPose (const geometry_msgs::Pose2D &pose2d)
 
geometry_msgs::PoseStamped pose2DToPoseStamped (const nav_2d_msgs::Pose2DStamped &pose2d)
 
geometry_msgs::PoseStamped pose2DToPoseStamped (const geometry_msgs::Pose2D &pose2d, const std::string &frame, const ros::Time &stamp)
 
double poseDistance (const geometry_msgs::Pose2D &pose0, const geometry_msgs::Pose2D &pose1)
 Calculate the linear distance between two poses. More...
 
nav_msgs::Path poses2DToPath (const std::vector< geometry_msgs::Pose2D > &poses, const std::string &frame, const ros::Time &stamp)
 
nav_2d_msgs::Pose2DStamped poseStampedToPose2D (const geometry_msgs::PoseStamped &pose)
 
nav_msgs::Path posesToPath (const std::vector< geometry_msgs::PoseStamped > &poses)
 
nav_2d_msgs::Path2D posesToPath2D (const std::vector< geometry_msgs::PoseStamped > &poses)
 
template<class param_t >
param_t searchAndGetParam (const ros::NodeHandle &nh, const std::string &param_name, const param_t &default_value)
 Search for a parameter and load it, or use the default value. More...
 
nav_2d_msgs::Pose2DStamped stampedPoseToPose2D (const tf::Stamped< tf::Pose > &pose)
 
nav_2d_msgs::NavGridInfo toMsg (const nav_grid::NavGridInfo &info)
 
nav_2d_msgs::UIntBounds toMsg (const nav_core2::UIntBounds &bounds)
 
bool transformPose (const TFListenerPtr tf, const std::string frame, const geometry_msgs::PoseStamped &in_pose, geometry_msgs::PoseStamped &out_pose, const bool extrapolation_fallback=true)
 Transform a PoseStamped from one frame to another while catching exceptions. More...
 
bool transformPose (const TFListenerPtr tf, const std::string frame, const nav_2d_msgs::Pose2DStamped &in_pose, nav_2d_msgs::Pose2DStamped &out_pose, const bool extrapolation_fallback=true)
 Transform a Pose2DStamped from one frame to another while catching exceptions. More...
 
geometry_msgs::Pose2D transformStampedPose (const TFListenerPtr tf, const nav_2d_msgs::Pose2DStamped &pose, const std::string &frame_id)
 
geometry_msgs::Twist twist2Dto3D (const nav_2d_msgs::Twist2D &cmd_vel_2d)
 
nav_2d_msgs::Twist2D twist3Dto2D (const geometry_msgs::Twist &cmd_vel)
 
XmlRpc::XmlRpcValue vectorToXMLRPC (const std::vector< double > &array)
 Helper method to convert a vector of doubles. More...
 

Typedef Documentation

using nav_2d_utils::PoseList = typedef std::vector<geometry_msgs::Pose2D>

Definition at line 121 of file path_ops.cpp.

Function Documentation

void nav_2d_utils::addPose ( nav_2d_msgs::Path2D &  path,
double  x,
double  y,
double  theta = 0.0 
)

Convenience function to add a pose to a path in one line.

Parameters
pathPath to add to
xx-coordinate
yy-coordinate
thetatheta (if needed)

Definition at line 184 of file path_ops.cpp.

nav_2d_msgs::Path2D nav_2d_utils::adjustPlanResolution ( const nav_2d_msgs::Path2D &  global_plan_in,
double  resolution 
)

Increase plan resolution to match that of the costmap by adding points linearly between points.

Parameters
global_plan_ininput plan
resolutiondesired distance between waypoints
Returns
Higher resolution plan

Definition at line 76 of file path_ops.cpp.

void nav_2d_utils::calculateMinAndMaxDistances ( const nav_2d_msgs::Polygon2D &  polygon,
double &  min_dist,
double &  max_dist 
)

Calculate the minimum and maximum distance from (0, 0) to any point on the polygon.

Parameters
[in]polygonpolygon to analyze
[out]min_dist
[out]max_dist

Definition at line 403 of file polygons.cpp.

nav_2d_msgs::Path2D nav_2d_utils::compressPlan ( const nav_2d_msgs::Path2D &  input_path,
double  epsilon = 0.1 
)

Decrease the length of the plan by eliminating colinear points.

Uses the Ramer Douglas Peucker algorithm. Ignores theta values

Parameters
input_pathPath to compress
epsilonmaximum allowable error. Increase for greater compression.
Returns
Path2D with possibly fewer poses

Definition at line 176 of file path_ops.cpp.

PoseList nav_2d_utils::compressPlan ( const PoseList input,
unsigned int  start_index,
unsigned int  end_index,
double  epsilon 
)

Helper function for other version of compressPlan.

Uses the Ramer Douglas Peucker algorithm. Ignores theta values

Parameters
inputFull list of poses
start_indexIndex of first pose (inclusive)
end_indexIndex of last pose (inclusive)
epsilonmaximum allowable error. Increase for greater compression.
listof poses possibly compressed for the poses[start_index, end_index]

Definition at line 134 of file path_ops.cpp.

double nav_2d_utils::distanceToLine ( double  pX,
double  pY,
double  x0,
double  y0,
double  x1,
double  y1 
)
inline

Distance from point (pX, pY) to closest point on line from (x0, y0) to (x1, y1)

Parameters
pX
pY
x0
y0
x1
y1
Returns
shortest distance from point to line

Definition at line 52 of file geometry_help.h.

bool nav_2d_utils::equals ( const nav_2d_msgs::Polygon2D &  polygon0,
const nav_2d_msgs::Polygon2D &  polygon1 
)

check if two polygons are equal. Used in testing

Definition at line 349 of file polygons.cpp.

nav_2d_msgs::Polygon2D nav_2d_utils::footprintFromParams ( ros::NodeHandle nh,
bool  write = true 
)

Load the robot footprint either as a polygon from the footprint parameter or from robot_radius.

Analagous to costmap_2d::makeFootprintFromParams in that it will return an empty polygon if neither parameter is present.

Definition at line 41 of file footprint.cpp.

nav_grid::NavGridInfo nav_2d_utils::fromMsg ( const nav_2d_msgs::NavGridInfo &  msg)

Definition at line 198 of file conversions.cpp.

nav_core2::UIntBounds nav_2d_utils::fromMsg ( const nav_2d_msgs::UIntBounds &  msg)

Definition at line 248 of file conversions.cpp.

double nav_2d_utils::getNumberFromXMLRPC ( XmlRpc::XmlRpcValue value)

Helper function. Convert value to double.

Definition at line 160 of file polygons.cpp.

std::vector<double> nav_2d_utils::getNumberVectorFromXMLRPC ( XmlRpc::XmlRpcValue value)

Helper function. Convert value to double array.

Definition at line 179 of file polygons.cpp.

double nav_2d_utils::getPlanLength ( const nav_2d_msgs::Path2D &  plan,
const unsigned int  start_index = 0 
)

Calculate the length of the plan, starting from the given index.

Definition at line 48 of file path_ops.cpp.

double nav_2d_utils::getPlanLength ( const nav_2d_msgs::Path2D &  plan,
const geometry_msgs::Pose2D &  query_pose 
)

Calculate the length of the plan from the pose on the plan closest to the given pose.

Definition at line 58 of file path_ops.cpp.

nav_grid::NavGridInfo nav_2d_utils::infoToInfo ( const nav_msgs::MapMetaData &  metadata)

Definition at line 210 of file conversions.cpp.

nav_msgs::MapMetaData nav_2d_utils::infoToInfo ( const nav_grid::NavGridInfo info)

Definition at line 226 of file conversions.cpp.

bool nav_2d_utils::isInside ( const nav_2d_msgs::Polygon2D &  polygon,
const double  x,
const double  y 
)

Check if a given point is inside of a polygon.

Borders are considered outside.

Parameters
polygonPolygon to check
xx coordinate
yy coordinate
Returns
true if point is inside polygon

Definition at line 381 of file polygons.cpp.

template<class param_t >
param_t nav_2d_utils::loadParameterWithDeprecation ( const ros::NodeHandle nh,
const std::string  current_name,
const std::string  old_name,
const param_t &  default_value 
)

Load a parameter from one of two namespaces. Complain if it uses the old name.

Parameters
nhNodeHandle to look for the parameter in
current_nameParameter name that is current, i.e. not deprecated
old_nameDeprecated parameter name
default_valueIf neither parameter is present, return this value
Returns
The value of the parameter or the default value

Definition at line 77 of file parameters.h.

template<class param_t >
void nav_2d_utils::moveDeprecatedParameter ( const ros::NodeHandle nh,
const std::string  current_name,
const std::string  old_name 
)

If a deprecated parameter exists, complain and move to its new location.

Parameters
nhNodeHandle to look for the parameter in
current_nameParameter name that is current, i.e. not deprecated
old_nameDeprecated parameter name

Definition at line 102 of file parameters.h.

template<class param_t >
void nav_2d_utils::moveParameter ( const ros::NodeHandle nh,
std::string  old_name,
std::string  current_name,
param_t  default_value,
bool  should_delete = true 
)

Move a parameter from one place to another.

For use loading old parameter structures into new places.

If the new name already has a value, don't move the old value there.

Parameters
nhNodeHandle for loading/saving params
old_nameParameter name to move/remove
current_nameDestination parameter name
default_valueValue to save in the new name if old parameter is not found.
should_deleteIf true, whether to delete the parameter from the old name

Definition at line 126 of file parameters.h.

nav_2d_msgs::Polygon2D nav_2d_utils::movePolygonToPose ( const nav_2d_msgs::Polygon2D &  polygon,
const geometry_msgs::Pose2D &  pose 
)

Translate and rotate a polygon to a new pose.

Parameters
polygonThe polygon
poseThe x, y and theta to use when moving the polygon
Returns
A new moved polygon

Definition at line 365 of file polygons.cpp.

std::vector< std::vector< double > > nav_2d_utils::parseVVD ( const std::string &  input)

Parse a vector of vectors of doubles from a string. Syntax is [[1.0, 2.0], [3.3, 4.4, 5.5], ...].

Parameters
inputThe string to parse
Returns
a vector of vectors of doubles

Definition at line 48 of file polygons.cpp.

nav_2d_msgs::Path2D nav_2d_utils::pathToPath ( const nav_msgs::Path &  path)

Definition at line 110 of file conversions.cpp.

nav_msgs::Path nav_2d_utils::pathToPath ( const nav_2d_msgs::Path2D &  path2d)

Definition at line 173 of file conversions.cpp.

nav_2d_msgs::Polygon2D nav_2d_utils::polygonFromParallelArrays ( const std::vector< double > &  xs,
const std::vector< double > &  ys 
)

Create a polygon from two parallel arrays.

Parameters
xsArray of doubles representing x coordinates, at least three elements long
ysArray of doubles representing y coordinates, matching length of xs

Definition at line 316 of file polygons.cpp.

Polygon2D nav_2d_utils::polygonFromParams ( const ros::NodeHandle nh,
const std::string  parameter_name,
bool  search = true 
)

Load a polygon from a parameter, whether it is a string or array, or two arrays.

Parameters
nhNode handle to load parameter from
parameter_nameName of the parameter
searchWhether to search up the namespace for the parameter name
Returns
Loaded polygon

Definition at line 249 of file polygons.cpp.

Polygon2D nav_2d_utils::polygonFromRadius ( const double  radius,
const unsigned int  num_points = 16 
)

Create a "circular" polygon from a given radius.

Parameters
radiusRadius of the polygon
num_pointsNumber of points in the resulting polygon
Returns
A rotationally symmetric polygon with the specified number of vertices

Definition at line 111 of file polygons.cpp.

Polygon2D nav_2d_utils::polygonFromString ( const std::string &  polygon_string)

Make a polygon from the given string. Format should be bracketed array of arrays of doubles, like so: [[1.0, 2.2], [3.3, 4.2], ...].

Parameters
polygon_stringThe string to parse
Returns
Polygon2D

Definition at line 126 of file polygons.cpp.

Polygon2D nav_2d_utils::polygonFromXMLRPC ( XmlRpc::XmlRpcValue polygon_xmlrpc)

Create a polygon from the given XmlRpcValue.

Parameters
polygon_xmlrpcshould be an array of arrays, where the top-level array should have 3 or more elements, and the sub-arrays should all have exactly 2 elements (x and y coordinates).

Definition at line 193 of file polygons.cpp.

void nav_2d_utils::polygonToParallelArrays ( const nav_2d_msgs::Polygon2D  polygon,
std::vector< double > &  xs,
std::vector< double > &  ys 
)

Create two parallel arrays from a polygon.

Parameters
[in]polygon
[out]xsArray of doubles representing x coordinates, to be populated
[out]ysArray of doubles representing y coordinates, to be populated

Definition at line 338 of file polygons.cpp.

void nav_2d_utils::polygonToParams ( const nav_2d_msgs::Polygon2D &  polygon,
const ros::NodeHandle nh,
const std::string  parameter_name,
bool  array_of_arrays = true 
)

Save a polygon to a parameter.

Parameters
polygonThe Polygon
nhNode handle to save the parameter to
parameter_nameName of the parameter
array_of_arraysIf true, write an array of arrays. Otherwise, write two parallel arrays

Definition at line 310 of file polygons.cpp.

XmlRpc::XmlRpcValue nav_2d_utils::polygonToXMLRPC ( const nav_2d_msgs::Polygon2D &  polygon,
bool  array_of_arrays = true 
)

Create XMLRPC Value for writing the polygon to the parameter server.

Parameters
polygonPolygon to convert
array_of_arraysIf true, write an array of arrays. Otherwise, write two parallel arrays
Returns
XmlRpcValue

Definition at line 286 of file polygons.cpp.

geometry_msgs::Pose nav_2d_utils::pose2DToPose ( const geometry_msgs::Pose2D &  pose2d)

Definition at line 81 of file conversions.cpp.

geometry_msgs::PoseStamped nav_2d_utils::pose2DToPoseStamped ( const nav_2d_msgs::Pose2DStamped &  pose2d)

Definition at line 90 of file conversions.cpp.

geometry_msgs::PoseStamped nav_2d_utils::pose2DToPoseStamped ( const geometry_msgs::Pose2D &  pose2d,
const std::string &  frame,
const ros::Time stamp 
)

Definition at line 98 of file conversions.cpp.

double nav_2d_utils::poseDistance ( const geometry_msgs::Pose2D &  pose0,
const geometry_msgs::Pose2D &  pose1 
)

Calculate the linear distance between two poses.

Definition at line 43 of file path_ops.cpp.

nav_msgs::Path nav_2d_utils::poses2DToPath ( const std::vector< geometry_msgs::Pose2D > &  poses,
const std::string &  frame,
const ros::Time stamp 
)

Definition at line 159 of file conversions.cpp.

nav_2d_msgs::Pose2DStamped nav_2d_utils::poseStampedToPose2D ( const geometry_msgs::PoseStamped &  pose)

Definition at line 71 of file conversions.cpp.

nav_msgs::Path nav_2d_utils::posesToPath ( const std::vector< geometry_msgs::PoseStamped > &  poses)

Definition at line 124 of file conversions.cpp.

nav_2d_msgs::Path2D nav_2d_utils::posesToPath2D ( const std::vector< geometry_msgs::PoseStamped > &  poses)

Definition at line 140 of file conversions.cpp.

template<class param_t >
param_t nav_2d_utils::searchAndGetParam ( const ros::NodeHandle nh,
const std::string &  param_name,
const param_t &  default_value 
)

Search for a parameter and load it, or use the default value.

This templated function shortens a commonly used ROS pattern in which you search for a parameter and get its value if it exists, otherwise returning a default value.

Parameters
nhNodeHandle to start the parameter search from
param_nameName of the parameter to search for
default_valueValue to return if not found
Returns
Value of parameter if found, otherwise the default_value

Definition at line 56 of file parameters.h.

nav_2d_msgs::Pose2DStamped nav_2d_utils::stampedPoseToPose2D ( const tf::Stamped< tf::Pose > &  pose)

Definition at line 60 of file conversions.cpp.

nav_2d_msgs::NavGridInfo nav_2d_utils::toMsg ( const nav_grid::NavGridInfo info)

Definition at line 186 of file conversions.cpp.

nav_2d_msgs::UIntBounds nav_2d_utils::toMsg ( const nav_core2::UIntBounds bounds)

Definition at line 238 of file conversions.cpp.

bool nav_2d_utils::transformPose ( const TFListenerPtr  tf,
const std::string  frame,
const geometry_msgs::PoseStamped &  in_pose,
geometry_msgs::PoseStamped &  out_pose,
const bool  extrapolation_fallback = true 
)

Transform a PoseStamped from one frame to another while catching exceptions.

Also returns immediately if the frames are equal.

Parameters
tfSmart pointer to TFListener
frameFrame to transform the pose into
in_posePose to transform
out_posePlace to store the resulting transformed pose
extrapolation_fallbackIf true, if there is an ExtrapolationException, allow looking up the latest timestamp instead.
Returns
True if successful transform

Definition at line 57 of file tf_help.h.

bool nav_2d_utils::transformPose ( const TFListenerPtr  tf,
const std::string  frame,
const nav_2d_msgs::Pose2DStamped &  in_pose,
nav_2d_msgs::Pose2DStamped &  out_pose,
const bool  extrapolation_fallback = true 
)

Transform a Pose2DStamped from one frame to another while catching exceptions.

Also returns immediately if the frames are equal.

Parameters
tfSmart pointer to TFListener
frameFrame to transform the pose into
in_posePose to transform
out_posePlace to store the resulting transformed pose
extrapolation_fallbackIf true, if there is an ExtrapolationException, allow looking up the latest timestamp instead.
Returns
True if successful transform

Definition at line 101 of file tf_help.h.

geometry_msgs::Pose2D nav_2d_utils::transformStampedPose ( const TFListenerPtr  tf,
const nav_2d_msgs::Pose2DStamped &  pose,
const std::string &  frame_id 
)

Definition at line 116 of file tf_help.h.

geometry_msgs::Twist nav_2d_utils::twist2Dto3D ( const nav_2d_msgs::Twist2D &  cmd_vel_2d)

Definition at line 41 of file conversions.cpp.

nav_2d_msgs::Twist2D nav_2d_utils::twist3Dto2D ( const geometry_msgs::Twist &  cmd_vel)

Definition at line 51 of file conversions.cpp.

XmlRpc::XmlRpcValue nav_2d_utils::vectorToXMLRPC ( const std::vector< double > &  array)

Helper method to convert a vector of doubles.

Definition at line 275 of file polygons.cpp.



nav_2d_utils
Author(s):
autogenerated on Wed Jun 26 2019 20:06:11