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...
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.
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.
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.
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.
PoseList	compressPlan (const PoseList &input, unsigned int start_index, unsigned int end_index, double epsilon)
	Helper function for other version of compressPlan.
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)
bool	equals (const nav_2d_msgs::Polygon2D &polygon0, const nav_2d_msgs::Polygon2D &polygon1)
	check if two polygons are equal. Used in testing
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.
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.
std::vector< double >	getNumberVectorFromXMLRPC (XmlRpc::XmlRpcValue &value)
	Helper function. Convert value to double array.
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.
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.
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.
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.
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.
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.
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.
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], ...].
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.
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.
nav_2d_msgs::Polygon2D	polygonFromRadius (const double radius, const unsigned int num_points=16)
	Create a "circular" polygon from a given radius.
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], ...].
nav_2d_msgs::Polygon2D	polygonFromXMLRPC (XmlRpc::XmlRpcValue &polygon_xmlrpc)
	Create a polygon from the given XmlRpcValue.
void	polygonToParallelArrays (const nav_2d_msgs::Polygon2D polygon, std::vector< double > &xs, std::vector< double > &ys)
	Create two parallel arrays from a polygon.
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.
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.
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.
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.
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.
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.
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.

---

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:

path	Path to add to
x	x-coordinate
y	y-coordinate
theta	theta (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_in	input plan
resolution	desired 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]	polygon	polygon 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_path	Path to compress
epsilon	maximum 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:

input	Full list of poses
start_index	Index of first pose (inclusive)
end_index	Index of last pose (inclusive)
epsilon	maximum allowable error. Increase for greater compression.
list	of 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:

polygon	Polygon to check
x	x coordinate
y	y 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:

nh	NodeHandle to look for the parameter in
current_name	Parameter name that is current, i.e. not deprecated
old_name	Deprecated parameter name
default_value	If 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:

nh	NodeHandle to look for the parameter in
current_name	Parameter name that is current, i.e. not deprecated
old_name	Deprecated 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:

nh	NodeHandle for loading/saving params
old_name	Parameter name to move/remove
current_name	Destination parameter name
default_value	Value to save in the new name if old parameter is not found.
should_delete	If 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:

polygon	The polygon
pose	The 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:

input

The 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:

xs	Array of doubles representing x coordinates, at least three elements long
ys	Array 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:

nh	Node handle to load parameter from
parameter_name	Name of the parameter
search	Whether 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:

radius	Radius of the polygon
num_points	Number 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_string

The 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_xmlrpc

should 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]	xs	Array of doubles representing x coordinates, to be populated
[out]	ys	Array 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:

polygon	The Polygon
nh	Node handle to save the parameter to
parameter_name	Name of the parameter
array_of_arrays	If 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:

polygon	Polygon to convert
array_of_arrays	If 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:

nh	NodeHandle to start the parameter search from
param_name	Name of the parameter to search for
default_value	Value 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:

tf	Smart pointer to TFListener
frame	Frame to transform the pose into
in_pose	Pose to transform
out_pose	Place to store the resulting transformed pose
extrapolation_fallback	If 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:

tf	Smart pointer to TFListener
frame	Frame to transform the pose into
in_pose	Pose to transform
out_pose	Place to store the resulting transformed pose
extrapolation_fallback	If 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.