#include <ROSFunctions.h>

Public Types
typedef baselib_binding::shared_ptr < ROSFunctions >::type	ROSFunctionsPtr
Public Member Functions
bool	canGetTransform (const std_msgs::Header &p1, const std_msgs::Header &p2, bool latest, bool printError) const
bool	canGetTransform (const std::string &f1, const std::string &f2, const ros::Time &useTime, bool printError) const
template<typename GH , typename Res >
void	effectOnGoalHandle (const actionlib::SimpleClientGoalState &state, GH &goalHandle, const Res &res, const std::string &s)
template<typename GH >
void	effectOnGoalHandle (const actionlib::SimpleClientGoalState &state, GH &goalHandle)
int	equalPoses (const geometry_msgs::PoseStamped &p1, const geometry_msgs::PoseStamped &p2, float accuracy_pos, float accuracy_ori, bool useLatestTime, float maxWaitTransform, bool printErrors=true)
template<typename MJointState >
bool	getPoseFromVirtualJointState (const std::vector< std::string > &joint_names, const MJointState &virtualJointState, const std::string &virtualJointName, geometry_msgs::PoseStamped &robotPose)
int	getTransform (const std::string &f1, const std::string &f2, geometry_msgs::Pose &result, const ros::Time &useTime, float maxWaitTransform, bool printErrors=true)
template<typename GH , typename Res >
void	mapToGoalHandle (const actionlib::SimpleClientGoalState &state, GH &goalHandle, const Res &res, const std::string &s)
int	poseDistance (const geometry_msgs::PoseStamped &p1, const geometry_msgs::PoseStamped &p2, float &posDist, float &angleDist, bool useLatestTime, float maxWaitTransform, bool printErrors)
template<typename ROSMessage >
bool	readFromBagFile (const std::string &filename, std::vector< ROSMessage > &msgs)
template<typename ROSMessage >
bool	readFromFile (const std::string &filename, ROSMessage &msg, bool isBinary)
int	relativePose (const geometry_msgs::PoseStamped &origin, const geometry_msgs::PoseStamped &other, geometry_msgs::Pose &result, bool useLatestTime, float maxWaitTransform, bool printErrors=true)
	ROSFunctions (float tf_max_cache_time=DEFAULT_TF_CACHE_TIME)
template<typename ROSMessage >
bool	saveToBagFile (const std::vector< ROSMessage > &msgs, const std::string &filename)
template<typename ROSMessage >
bool	saveToFile (const ROSMessage &msg, const std::string &filename, bool asBinary)
int	transformPose (const geometry_msgs::PoseStamped &p, const std::string &frame_id, geometry_msgs::PoseStamped &result, float maxWaitTransform, bool printErrors=true)
bool	waitForTransform (const std_msgs::Header &p1, const std_msgs::Header &p2, const float &timeout, bool latest, bool printError)
bool	waitForTransform (const std::string &f1, const std::string &f2, const ros::Time &useTime, const float &timeout, bool printError)
	~ROSFunctions ()
Static Public Member Functions
static void	applyTransform (const geometry_msgs::Pose &transform, geometry_msgs::Pose &pose)
static void	assignJointState (const sensor_msgs::JointState &target_joints, sensor_msgs::JointState &joint_state)
static void	destroySingleton ()
template<typename GH , typename Res >
static void	effectOnGoalHandle (const actionlib::SimpleClientGoalState &state, GH &goalHandle, const Res &res, const std::string &s="")
template<typename GH >
static void	effectOnGoalHandle (const actionlib::SimpleClientGoalState &state, GH &goalHandle)
static int	equalJointPositions (const sensor_msgs::JointState &j1, const sensor_msgs::JointState &j2, const float pos_tolerance)
static bool	equalJointPositionsSimple (const sensor_msgs::JointState &j1, const sensor_msgs::JointState &j2, const float pos_tolerance)
static bool	getJointStateAt (int idx, const trajectory_msgs::JointTrajectory &traj, sensor_msgs::JointState &result)
template<typename MJointState >
static bool	getPoseFromVirtualJointState (const std::vector< std::string > &joint_names, const MJointState &virtualJointState, const std::string &virtualJointName, geometry_msgs::PoseStamped &robotPose)
static void	initSingleton ()
static bool	intersectJointState (const sensor_msgs::JointState &s1, const sensor_msgs::JointState &s2, sensor_msgs::JointState &result, bool init_s1, bool s2_is_subset)
static bool	intersectJointStates (const sensor_msgs::JointState &s1, const sensor_msgs::JointState &s2, sensor_msgs::JointState &result, bool s2_is_subset)
template<typename GH , typename Res >
static void	mapToGoalHandle (const actionlib::SimpleClientGoalState &state, GH &goalHandle, const Res &res, const std::string &s)
template<typename ROSMessage >
static bool	readFromBagFile (const std::string &filename, std::vector< ROSMessage > &msgs)
template<typename ROSMessage >
static bool	readFromFile (const std::string &filename, ROSMessage &msg, bool isBinary)
template<typename ROSMessage >
static bool	saveToBagFile (const std::vector< ROSMessage > &msgs, const std::string &filename)
template<typename ROSMessage >
static bool	saveToFile (const ROSMessage &msg, const std::string &filename, bool asBinary)
static ROSFunctionsPtr	Singleton ()
Static Private Member Functions
static int	hasVal (const std::string &val, const std::vector< std::string > &vec)
Private Attributes
tf::TransformListener	tf_listener
Static Private Attributes
static ROSFunctionsPtr	_singleton
static baselib_binding::recursive_mutex	slock

Detailed Description

Keeps a local tf listener which can be shared via a static singleton. It is important, if you use this singleton, to call initSingleton(), or obtain a reference to it via Singleton(), when you want the tf listener to be created and start listening. Once initSingleton()/Singleton() has been called, the tf listener will keep listening, until the program ends of destroySingleton() is called. It can be that the tf listener needs some time to initialize, so it's advisable to call initSingleton() before you need it for the first time in a time-critical context.

Author:: Jennifer Buehler

Definition at line 33 of file ROSFunctions.h.

Member Typedef Documentation

typedef baselib_binding::shared_ptr<ROSFunctions>::type convenience_ros_functions::ROSFunctions::ROSFunctionsPtr

Definition at line 36 of file ROSFunctions.h.

Constructor & Destructor Documentation

ROSFunctions::ROSFunctions ( float tf_max_cache_time = DEFAULT_TF_CACHE_TIME )

Parameters:

tf_max_cache_time maximum cache time for tf listener

Definition at line 16 of file ROSFunctions.cpp.

convenience_ros_functions::ROSFunctions::~ROSFunctions ( ) [inline]

Definition at line 42 of file ROSFunctions.h.

Member Function Documentation

void ROSFunctions::applyTransform	(	const geometry_msgs::Pose &	transform,
		geometry_msgs::Pose &	pose
	)		`[static]`

Apply a transform (given in parameter transform) to the pose. Essentially this does pose=pose*transform.

Definition at line 58 of file ROSFunctions.cpp.

void ROSFunctions::assignJointState	(	const sensor_msgs::JointState &	target_joints,
		sensor_msgs::JointState &	joint_state
	)		`[static]`

Takes joint_state and re-assigns all its fields with the values in target_joints, if the same joint appears in there. All values in target_joints which are not in joint_state, are added to it.

Definition at line 378 of file ROSFunctions.cpp.

bool ROSFunctions::canGetTransform	(	const std_msgs::Header &	p1,
		const std_msgs::Header &	p2,
		bool	latest,
		bool	printError
	)		const

Checks whether based on the current tf transforms a distance between the two poses can be obtained.

Parameters:

latest	The lastest time of both stamps is assumed, or otherwise the most recent transform available for both
p1	target pose
p2	source pose

Definition at line 99 of file ROSFunctions.cpp.

bool ROSFunctions::canGetTransform	(	const std::string &	f1,
		const std::string &	f2,
		const ros::Time &	useTime,
		bool	printError
	)		const

Parameters:

f1	target frame
f2	source frame

Definition at line 73 of file ROSFunctions.cpp.

void ROSFunctions::destroySingleton ( ) [static]

Definition at line 36 of file ROSFunctions.cpp.

template<typename GH , typename Res >

void convenience_ros_functions::ROSFunctions::effectOnGoalHandle	(	const actionlib::SimpleClientGoalState &	state,
		GH &	goalHandle,
		const Res &	res,
		const std::string &	s
	)

Definition at line 153 of file ROSFunctions.hpp.

template<typename GH >

void convenience_ros_functions::ROSFunctions::effectOnGoalHandle	(	const actionlib::SimpleClientGoalState &	state,
		GH &	goalHandle
	)

Definition at line 186 of file ROSFunctions.hpp.

template<typename GH , typename Res >

static void convenience_ros_functions::ROSFunctions::effectOnGoalHandle	(	const actionlib::SimpleClientGoalState &	state,
		GH &	goalHandle,
		const Res &	res,
		const std::string &	s = `""`
	)		`[static]`

Uses the SimpleClientGoalState to set the ServerGoalHandle to the corresponding state. E.g. if SimpleClientGoalState is rejected, the ServerGoalHandle gets aborted (not also rejected).

template<typename GH >

static void convenience_ros_functions::ROSFunctions::effectOnGoalHandle	(	const actionlib::SimpleClientGoalState &	state,
		GH &	goalHandle
	)		`[static]`

Like other effectOnGoalHandle(), but without result type and error string.

int ROSFunctions::equalJointPositions	(	const sensor_msgs::JointState &	j1,
		const sensor_msgs::JointState &	j2,
		const float	pos_tolerance
	)		`[static]`

Compares joint states: All joints appearing in *both* joint states are required to be as similar as pos_tolerance

Return values:

1	requirements satisfied
-1	requirements not satisfied
-2	there is no complete intersection of both joint states
<-2	other consistency error

Probably can be implemented more efficient, but for no this will do

Definition at line 478 of file ROSFunctions.cpp.

bool ROSFunctions::equalJointPositionsSimple	(	const sensor_msgs::JointState &	j1,
		const sensor_msgs::JointState &	j2,
		const float	pos_tolerance
	)		`[static]`

Simple implementation to compare joint states, similar to equalJointPositions(), but more efficient. Instead, some assumptions are made: It assumes a common subset of joints are listed (in same order) at the beginning of both states. One state may be larger than the other.

Definition at line 466 of file ROSFunctions.cpp.

int ROSFunctions::equalPoses	(	const geometry_msgs::PoseStamped &	p1,
		const geometry_msgs::PoseStamped &	p2,
		float	accuracy_pos,
		float	accuracy_ori,
		bool	useLatestTime,
		float	maxWaitTransform,
		bool	printErrors = `true`
	)

Checks whether the two poses are equal (using the accuracy). They can be in different frames as long as the tf transforms are provided. If they are, it is also possible to wait for the transform for a certain maximum time before failing.

Parameters:

maxWaitTransform	if >0, this is the maximum time to wait for the transform between different frames to arrive.
useLatestTime	The lastest time of both stamps is assumed, or otherwise the most recent transform available for both

Returns:: -1 if no transform between the frames are possible, -2 if even the wait for it has failed, -3 if we thought we could get a transform but then the actual transfom failed. 0 if the poses are not equal, and 1 if the poses are equal within accuracy

Definition at line 167 of file ROSFunctions.cpp.

bool ROSFunctions::getJointStateAt	(	int	idx,
		const trajectory_msgs::JointTrajectory &	traj,
		sensor_msgs::JointState &	result
	)		`[static]`

Definition at line 507 of file ROSFunctions.cpp.

template<typename MJointState >

bool convenience_ros_functions::ROSFunctions::getPoseFromVirtualJointState	(	const std::vector< std::string > &	joint_names,
		const MJointState &	virtualJointState,
		const std::string &	virtualJointName,
		geometry_msgs::PoseStamped &	robotPose
	)

Definition at line 102 of file ROSFunctions.hpp.

template<typename MJointState >

static bool convenience_ros_functions::ROSFunctions::getPoseFromVirtualJointState	(	const std::vector< std::string > &	joint_names,
		const MJointState &	virtualJointState,
		const std::string &	virtualJointName,
		geometry_msgs::PoseStamped &	robotPose
	)		`[static]`

MJointState must be such as trajectory_msgs::MultiDOFJointTrajectoryPoint or sensor_msgs::MultiDOFJointState, such that <type>.transforms is a vector of transforms of same size as joint_names, containing a transform for each joint.

int ROSFunctions::getTransform	(	const std::string &	f1,
		const std::string &	f2,
		geometry_msgs::Pose &	result,
		const ros::Time &	useTime,
		float	maxWaitTransform,
		bool	printErrors = `true`
	)

gets transform between two frames

Parameters:

f1	target frame
f2	source frame

Definition at line 319 of file ROSFunctions.cpp.

int ROSFunctions::hasVal	(	const std::string &	val,
		const std::vector< std::string > &	vec
	)		`[static, private]`

Definition at line 544 of file ROSFunctions.cpp.

void ROSFunctions::initSingleton ( ) [static]

Initializes the singleton. If this function already was called, it does nothing. Otherwise, it initializes the singleton, which will incur a small wait to ensure the tf listener has received its first frame. If a transform function is called immediately, there are sometimes problems with it. This MUST be called from a valid ROS node!

Definition at line 21 of file ROSFunctions.cpp.

bool ROSFunctions::intersectJointState	(	const sensor_msgs::JointState &	s1,
		const sensor_msgs::JointState &	s2,
		sensor_msgs::JointState &	result,
		bool	init_s1,
		bool	s2_is_subset
	)		`[static]`

Creates a new joint state with only the joint names which are both in s1 and s2. The current values of the joints are initialized with the ones set s1 if parameter init_s1 is true, otherwise with values in s2. The order the joint names appear are as in s1.

We may impose the restriction that ALL values in s1 also HAVE TO appear in s2 (s2 is subset of s1), by setting the parameter s2_is_subset to true. In this case, the function will return an error if s2 is not subset of s1. This can be used to check consistency.

Definition at line 403 of file ROSFunctions.cpp.

bool ROSFunctions::intersectJointStates	(	const sensor_msgs::JointState &	s1,
		const sensor_msgs::JointState &	s2,
		sensor_msgs::JointState &	result,
		bool	s2_is_subset
	)		`[static]`

Works like doing s1=s2, but only copying the values in s2 that also appear in s1. We may impose the restriction that ALL values in s1 also HAVE TO appear in s2 (s2 is subset of s1), by setting the parameter s2_is_subset to true. In this case, the function will return an error if s2 is not subset of s1. This can be used to check consistency.

Definition at line 437 of file ROSFunctions.cpp.

template<typename GH , typename Res >

void convenience_ros_functions::ROSFunctions::mapToGoalHandle	(	const actionlib::SimpleClientGoalState &	state,
		GH &	goalHandle,
		const Res &	res,
		const std::string &	s
	)

Definition at line 120 of file ROSFunctions.hpp.

template<typename GH , typename Res >

static void convenience_ros_functions::ROSFunctions::mapToGoalHandle	(	const actionlib::SimpleClientGoalState &	state,
		GH &	goalHandle,
		const Res &	res,
		const std::string &	s
	)		`[static]`

transfers the SimpleClientGoalState to a ServerGoalHandle

int ROSFunctions::poseDistance	(	const geometry_msgs::PoseStamped &	p1,
		const geometry_msgs::PoseStamped &	p2,
		float &	posDist,
		float &	angleDist,
		bool	useLatestTime,
		float	maxWaitTransform,
		bool	printErrors
	)

Returns distance of two poses (possibly after transfomring them first) in parameters posDist and angleDist. Other parameters and return value just as in relativePose().

Definition at line 190 of file ROSFunctions.cpp.