Monitors the joint_states topic and tf to maintain the current state of the robot. More...

#include <current_state_monitor.h>

Public Member Functions
void	addUpdateCallback (const JointStateUpdateCallback &fn)
	Add a function that will be called whenever the joint state is updated.
void	clearUpdateCallbacks ()
	Clear the functions to be called when an update to the joint state is received.
	CurrentStateMonitor (const robot_model::RobotModelConstPtr &robot_model, const boost::shared_ptr< tf::Transformer > &tf)
	Constructor.
	CurrentStateMonitor (const robot_model::RobotModelConstPtr &robot_model, const boost::shared_ptr< tf::Transformer > &tf, ros::NodeHandle nh)
	Constructor.
double	getBoundsError () const
	When a joint value is received to be out of bounds, it is changed slightly to fit within bounds, if the difference is less than a specified value (labeled the "allowed bounds error").
robot_state::RobotStatePtr	getCurrentState () const
	Get the current state.
std::pair < robot_state::RobotStatePtr, ros::Time >	getCurrentStateAndTime () const
	Get the current state and its time stamp.
ros::Time	getCurrentStateTime () const
	Get the time stamp for the current state.
std::map< std::string, double >	getCurrentStateValues () const
	Get the current state values as a map from joint names to joint state values.
std::string	getMonitoredTopic () const
	Get the name of the topic being monitored. Returns an empty string if the monitor is inactive.
const ros::Time &	getMonitorStartTime () const
	Get the time point when the monitor was started.
const robot_model::RobotModelConstPtr &	getRobotModel () const
	Get the RobotModel for which we are monitoring state.
bool	haveCompleteState () const
	Query whether we have joint state information for all DOFs in the kinematic model.
bool	haveCompleteState (const ros::Duration &age) const
	Query whether we have joint state information for all DOFs in the kinematic model.
bool	haveCompleteState (std::vector< std::string > &missing_joints) const
	Query whether we have joint state information for all DOFs in the kinematic model.
bool	haveCompleteState (const ros::Duration &age, std::vector< std::string > &missing_states) const
	Query whether we have joint state information for all DOFs in the kinematic model.
bool	isActive () const
	Check if the state monitor is started.
void	setBoundsError (double error)
	When a joint value is received to be out of bounds, it is changed slightly to fit within bounds, if the difference is less than a specified value (labeled the "allowed bounds error"). This value can be set using this function.
void	setToCurrentState (robot_state::RobotState &upd) const
	Set the state upd to the current state maintained by this class.
void	startStateMonitor (const std::string &joint_states_topic="joint_states")
	Start monitoring joint states on a particular topic.
void	stopStateMonitor ()
	Stop monitoring the "joint_states" topic.
bool	waitForCurrentState (const ros::Time t=ros::Time::now(), double wait_time=1.0) const
	Wait for at most wait_time seconds (default 1s) for a robot state more recent than t.
bool	waitForCurrentState (double wait_time) const
	Wait for at most wait_time seconds until the complete current state is known. Return true if the full state is known.
bool	waitForCurrentState (const std::string &group, double wait_time) const
	Wait for at most wait_time seconds until the joint values from the group group are known. Return true if values for all joints in group are known.
	~CurrentStateMonitor ()
Private Member Functions
bool	isPassiveOrMimicDOF (const std::string &dof) const
void	jointStateCallback (const sensor_msgs::JointStateConstPtr &joint_state)
Private Attributes
ros::Time	current_state_time_
double	error_
ros::Subscriber	joint_state_subscriber_
std::map< std::string, ros::Time >	joint_time_
ros::Time	last_tf_update_
ros::Time	monitor_start_time_
ros::NodeHandle	nh_
robot_model::RobotModelConstPtr	robot_model_
robot_state::RobotState	robot_state_
bool	state_monitor_started_
boost::mutex	state_update_lock_
boost::shared_ptr < tf::Transformer >	tf_
std::vector < JointStateUpdateCallback >	update_callbacks_

Detailed Description

Monitors the joint_states topic and tf to maintain the current state of the robot.

Definition at line 54 of file current_state_monitor.h.

Constructor & Destructor Documentation

planning_scene_monitor::CurrentStateMonitor::CurrentStateMonitor	(	const robot_model::RobotModelConstPtr &	robot_model,
		const boost::shared_ptr< tf::Transformer > &	tf
	)

Constructor.

Parameters:

robot_model	The current kinematic model to build on
tf	A pointer to the tf transformer to use

Definition at line 41 of file current_state_monitor.cpp.

planning_scene_monitor::CurrentStateMonitor::CurrentStateMonitor	(	const robot_model::RobotModelConstPtr &	robot_model,
		const boost::shared_ptr< tf::Transformer > &	tf,
		ros::NodeHandle	nh
	)

Constructor.

Parameters:

robot_model	The current kinematic model to build on
tf	A pointer to the tf transformer to use
nh	A ros::NodeHandle to pass node specific options

Definition at line 47 of file current_state_monitor.cpp.

planning_scene_monitor::CurrentStateMonitor::~CurrentStateMonitor ( )

Definition at line 60 of file current_state_monitor.cpp.

Member Function Documentation

void planning_scene_monitor::CurrentStateMonitor::addUpdateCallback ( const JointStateUpdateCallback & fn )

Add a function that will be called whenever the joint state is updated.

Definition at line 104 of file current_state_monitor.cpp.

void planning_scene_monitor::CurrentStateMonitor::clearUpdateCallbacks ( )

Clear the functions to be called when an update to the joint state is received.

Definition at line 110 of file current_state_monitor.cpp.

double planning_scene_monitor::CurrentStateMonitor::getBoundsError ( ) const [inline]

When a joint value is received to be out of bounds, it is changed slightly to fit within bounds, if the difference is less than a specified value (labeled the "allowed bounds error").

Returns:: The stored value for the "allowed bounds error"

Definition at line 175 of file current_state_monitor.h.

robot_state::RobotStatePtr planning_scene_monitor::CurrentStateMonitor::getCurrentState ( ) const

Get the current state.

Returns:: Returns the current state

Definition at line 65 of file current_state_monitor.cpp.

std::pair< robot_state::RobotStatePtr, ros::Time > planning_scene_monitor::CurrentStateMonitor::getCurrentStateAndTime ( ) const

Get the current state and its time stamp.

Returns:: Returns a pair of the current state and its time stamp

Definition at line 79 of file current_state_monitor.cpp.

ros::Time planning_scene_monitor::CurrentStateMonitor::getCurrentStateTime ( ) const

Get the time stamp for the current state.

Definition at line 72 of file current_state_monitor.cpp.

std::map< std::string, double > planning_scene_monitor::CurrentStateMonitor::getCurrentStateValues ( ) const

Get the current state values as a map from joint names to joint state values.

Returns:: Returns the map from joint names to joint state values

Definition at line 86 of file current_state_monitor.cpp.

std::string planning_scene_monitor::CurrentStateMonitor::getMonitoredTopic ( ) const

Get the name of the topic being monitored. Returns an empty string if the monitor is inactive.

Definition at line 145 of file current_state_monitor.cpp.

const ros::Time& planning_scene_monitor::CurrentStateMonitor::getMonitorStartTime ( ) const [inline]

Get the time point when the monitor was started.

Definition at line 151 of file current_state_monitor.h.

const robot_model::RobotModelConstPtr& planning_scene_monitor::CurrentStateMonitor::getRobotModel ( ) const [inline]

Get the RobotModel for which we are monitoring state.

Definition at line 87 of file current_state_monitor.h.

bool planning_scene_monitor::CurrentStateMonitor::haveCompleteState ( ) const

Query whether we have joint state information for all DOFs in the kinematic model.

Returns:: False if we have no joint state information for one or more of the joints

Definition at line 175 of file current_state_monitor.cpp.

bool planning_scene_monitor::CurrentStateMonitor::haveCompleteState ( const ros::Duration & age ) const

Query whether we have joint state information for all DOFs in the kinematic model.

Returns:: False if we have no joint state information for one of the joints or if our state information is more than age old

Definition at line 207 of file current_state_monitor.cpp.

bool planning_scene_monitor::CurrentStateMonitor::haveCompleteState ( std::vector< std::string > & missing_joints ) const

Query whether we have joint state information for all DOFs in the kinematic model.

Parameters:

missing_joints Returns the list of joints that are missing

Returns:: False if we have no joint state information for one or more of the joints

Definition at line 192 of file current_state_monitor.cpp.

bool planning_scene_monitor::CurrentStateMonitor::haveCompleteState	(	const ros::Duration &	age,
		std::vector< std::string > &	missing_states
	)		const

Query whether we have joint state information for all DOFs in the kinematic model.

Parameters:

age	The max allowed age of the joint state information
missing_states	Returns the list of joints that are missing

Returns:: False if we have no joint state information for one of the joints or if our state information is more than age old

Definition at line 234 of file current_state_monitor.cpp.

bool planning_scene_monitor::CurrentStateMonitor::isActive ( ) const

Check if the state monitor is started.

Definition at line 130 of file current_state_monitor.cpp.

bool planning_scene_monitor::CurrentStateMonitor::isPassiveOrMimicDOF ( const std::string & dof ) const [private]

Definition at line 153 of file current_state_monitor.cpp.

void planning_scene_monitor::CurrentStateMonitor::jointStateCallback ( const sensor_msgs::JointStateConstPtr & joint_state ) [private]

Definition at line 321 of file current_state_monitor.cpp.

void planning_scene_monitor::CurrentStateMonitor::setBoundsError ( double error ) [inline]

When a joint value is received to be out of bounds, it is changed slightly to fit within bounds, if the difference is less than a specified value (labeled the "allowed bounds error"). This value can be set using this function.

Parameters:

error The specified value for the "allowed bounds error". The default is machine precision.

Definition at line 166 of file current_state_monitor.h.

void planning_scene_monitor::CurrentStateMonitor::setToCurrentState ( robot_state::RobotState & upd ) const

Set the state upd to the current state maintained by this class.

Definition at line 97 of file current_state_monitor.cpp.

void planning_scene_monitor::CurrentStateMonitor::startStateMonitor ( const std::string & joint_states_topic = "joint_states" )

Start monitoring joint states on a particular topic.

Parameters:

joint_states_topic The topic name for joint states (defaults to "joint_states")

Definition at line 115 of file current_state_monitor.cpp.

void planning_scene_monitor::CurrentStateMonitor::stopStateMonitor ( )

Stop monitoring the "joint_states" topic.

Definition at line 135 of file current_state_monitor.cpp.

bool planning_scene_monitor::CurrentStateMonitor::waitForCurrentState	(	const ros::Time	t = `ros::Time::now()`,
		double	wait_time = `1.0`
	)		const

Wait for at most wait_time seconds (default 1s) for a robot state more recent than t.

Returns:: true on success, false if up-to-date robot state wasn't received within wait_time

Definition at line 264 of file current_state_monitor.cpp.

bool planning_scene_monitor::CurrentStateMonitor::waitForCurrentState ( double wait_time ) const

Wait for at most wait_time seconds until the complete current state is known. Return true if the full state is known.

Definition at line 281 of file current_state_monitor.cpp.

bool planning_scene_monitor::CurrentStateMonitor::waitForCurrentState	(	const std::string &	group,
		double	wait_time
	)		const

Wait for at most wait_time seconds until the joint values from the group group are known. Return true if values for all joints in group are known.

Definition at line 294 of file current_state_monitor.cpp.