#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. More...

void	clearUpdateCallbacks ()
	Clear the functions to be called when an update to the joint state is received. More...

	CurrentStateMonitor (const moveit::core::RobotModelConstPtr &robot_model, const std::shared_ptr< tf2_ros::Buffer > &tf_buffer)
	Constructor. More...

	CurrentStateMonitor (const moveit::core::RobotModelConstPtr &robot_model, const std::shared_ptr< tf2_ros::Buffer > &tf_buffer, const ros::NodeHandle &nh)
	Constructor. More...

void	enableCopyDynamics (bool enabled)
	Allow the joint_state arrrays velocity and effort to be copied into the robot state this is useful in some but not all applications. More...

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"). More...

moveit::core::RobotStatePtr	getCurrentState () const
	Get the current state. More...

std::pair< moveit::core::RobotStatePtr, ros::Time >	getCurrentStateAndTime (const std::string &group="") const
	Get the current state and its time stamp. More...

ros::Time	getCurrentStateTime (const std::string &group="") const
	Get the time stamp for the current state. More...

std::map< std::string, double >	getCurrentStateValues () const
	Get the current state values as a map from joint names to joint state values. More...

std::string	getMonitoredTopic () const
	Get the name of the topic being monitored. Returns an empty string if the monitor is inactive. More...

const ros::Time &	getMonitorStartTime () const
	Get the time point when the monitor was started. More...

const moveit::core::RobotModelConstPtr &	getRobotModel () const
	Get the RobotModel for which we are monitoring state. More...

bool	haveCompleteState (const ros::Duration &age, const std::string &group="") const
	Query whether we have joint state information for all DOFs in the kinematic model. More...

bool	haveCompleteState (const ros::Duration &age, std::vector< std::string > &missing_joints, const std::string &group="") const
	Query whether we have joint state information for all DOFs in the kinematic model. More...

bool	haveCompleteState (const ros::Time &oldest_allowed_update_time, const std::string &group="") const
	Query whether we have joint state information for all DOFs in the kinematic model. More...

bool	haveCompleteState (const ros::Time &oldest_allowed_update_time, std::vector< std::string > &missing_joints, const std::string &group="") const
	Query whether we have joint state information for all DOFs in the kinematic model. More...

bool	haveCompleteState (const std::string &group="") const
	Query whether we have joint state information for all DOFs in the kinematic model. More...

bool	haveCompleteState (std::vector< std::string > &missing_joints, const std::string &group="") const
	Query whether we have joint state information for all DOFs in the kinematic model. More...

bool	isActive () const
	Check if the state monitor is started. More...

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. More...

void	setToCurrentState (moveit::core::RobotState &upd) const
	Set the state upd to the current state maintained by this class. More...

void	startStateMonitor (const std::string &joint_states_topic="joint_states")
	Start monitoring joint states on a particular topic. More...

void	stopStateMonitor ()
	Stop monitoring the "joint_states" topic. More...

bool	waitForCompleteState (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. More...

bool	waitForCompleteState (double wait_time) const
	Wait for at most wait_time seconds until the complete robot state is known. More...

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. More...

	~CurrentStateMonitor ()

Private Types
using	TFConnection = boost::signals2::connection

Private Member Functions
ros::Time	getCurrentStateTimeHelper (const std::string &group="") const

bool	haveCompleteStateHelper (const ros::Time &oldest_allowed_update_time, std::vector< std::string > *missing_joints, const std::string &group) const

void	jointStateCallback (const sensor_msgs::JointStateConstPtr &joint_state)

void	tfCallback ()

Private Attributes
bool	copy_dynamics_

double	error_

ros::Subscriber	joint_state_subscriber_

std::map< const moveit::core::JointModel *, ros::Time >	joint_time_

ros::Time	monitor_start_time_

ros::NodeHandle	nh_

moveit::core::RobotModelConstPtr	robot_model_

moveit::core::RobotState	robot_state_

bool	state_monitor_started_

boost::condition_variable	state_update_condition_

boost::mutex	state_update_lock_

std::shared_ptr< tf2_ros::Buffer >	tf_buffer_

std::shared_ptr< TFConnection >	tf_connection_

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 84 of file current_state_monitor.h.

Member Typedef Documentation

◆ TFConnection

using planning_scene_monitor::CurrentStateMonitor::TFConnection = boost::signals2::connection

private

Definition at line 86 of file current_state_monitor.h.

Constructor & Destructor Documentation

◆ CurrentStateMonitor() [1/2]

planning_scene_monitor::CurrentStateMonitor::CurrentStateMonitor	(	const moveit::core::RobotModelConstPtr &	robot_model,
		const std::shared_ptr< tf2_ros::Buffer > &	tf_buffer
	)

Constructor.

Parameters

robot_model	The current kinematic model to build on
tf_buffer	A pointer to the tf2_ros Buffer to use

Definition at line 48 of file current_state_monitor.cpp.

◆ CurrentStateMonitor() [2/2]

planning_scene_monitor::CurrentStateMonitor::CurrentStateMonitor	(	const moveit::core::RobotModelConstPtr &	robot_model,
		const std::shared_ptr< tf2_ros::Buffer > &	tf_buffer,
		const ros::NodeHandle &	nh
	)

Constructor.

Parameters

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

Definition at line 54 of file current_state_monitor.cpp.

◆ ~CurrentStateMonitor()

planning_scene_monitor::CurrentStateMonitor::~CurrentStateMonitor ( )

Definition at line 67 of file current_state_monitor.cpp.

Member Function Documentation

◆ addUpdateCallback()

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 168 of file current_state_monitor.cpp.

◆ clearUpdateCallbacks()

void planning_scene_monitor::CurrentStateMonitor::clearUpdateCallbacks ( )

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

Definition at line 174 of file current_state_monitor.cpp.

◆ enableCopyDynamics()

void planning_scene_monitor::CurrentStateMonitor::enableCopyDynamics ( bool enabled )

inline

Allow the joint_state arrrays velocity and effort to be copied into the robot state this is useful in some but not all applications.

Definition at line 258 of file current_state_monitor.h.

◆ getBoundsError()

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 250 of file current_state_monitor.h.

◆ getCurrentState()

moveit::core::RobotStatePtr planning_scene_monitor::CurrentStateMonitor::getCurrentState ( ) const

Get the current state.

Returns: Returns the current state

Definition at line 72 of file current_state_monitor.cpp.

◆ getCurrentStateAndTime()

std::pair< moveit::core::RobotStatePtr, ros::Time > planning_scene_monitor::CurrentStateMonitor::getCurrentStateAndTime ( const std::string & group = "" ) const

Get the current state and its time stamp.

Parameters

group The name of the joint group whose current state is asked, defaults to empty string, which means all the active joints.

Note: If there are multiple sources of the constituent joints, then the oldest joint's timestamp is retrieved for the particular group

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

Definition at line 125 of file current_state_monitor.cpp.

◆ getCurrentStateTime()

ros::Time planning_scene_monitor::CurrentStateMonitor::getCurrentStateTime ( const std::string & group = "" ) const

Get the time stamp for the current state.

Parameters

group The name of the joint group whose current state is asked, defaults to empty string, which means all the active joints.

Note: If there are multiple joint sources, then the oldest joint's timestamp is retrieved for the particular group

Returns: The current state timestamp related with group

Definition at line 118 of file current_state_monitor.cpp.

◆ getCurrentStateTimeHelper()

ros::Time planning_scene_monitor::CurrentStateMonitor::getCurrentStateTimeHelper ( const std::string & group = "" ) const

private

Lock-free method that is used by getCurrentStateTime and getCurrentStateAndTime methods.

Definition at line 79 of file current_state_monitor.cpp.

◆ getCurrentStateValues()

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 132 of file current_state_monitor.cpp.

◆ getMonitoredTopic()

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 219 of file current_state_monitor.cpp.

◆ getMonitorStartTime()

const ros::Time& planning_scene_monitor::CurrentStateMonitor::getMonitorStartTime ( ) const

inline

Get the time point when the monitor was started.

Definition at line 226 of file current_state_monitor.h.

◆ getRobotModel()

const moveit::core::RobotModelConstPtr& planning_scene_monitor::CurrentStateMonitor::getRobotModel ( ) const

inline

Get the RobotModel for which we are monitoring state.

Definition at line 120 of file current_state_monitor.h.

◆ haveCompleteState() [1/6]

bool planning_scene_monitor::CurrentStateMonitor::haveCompleteState	(	const ros::Duration &	age,
		const std::string &	group = `""`
	)		const

inline

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

Parameters

age	Joint information must be at most this old

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 150 of file current_state_monitor.h.

◆ haveCompleteState() [2/6]

bool planning_scene_monitor::CurrentStateMonitor::haveCompleteState	(	const ros::Duration &	age,
		std::vector< std::string > &	missing_joints,
		const std::string &	group = `""`
	)		const

inline

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 179 of file current_state_monitor.h.

◆ haveCompleteState() [3/6]

bool planning_scene_monitor::CurrentStateMonitor::haveCompleteState	(	const ros::Time &	oldest_allowed_update_time,
		const std::string &	group = `""`
	)		const

inline

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

Parameters

oldest_allowed_update_time All joint information must be from this time or more current

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 140 of file current_state_monitor.h.

◆ haveCompleteState() [4/6]

bool planning_scene_monitor::CurrentStateMonitor::haveCompleteState	(	const ros::Time &	oldest_allowed_update_time,
		std::vector< std::string > &	missing_joints,
		const std::string &	group = `""`
	)		const

inline

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

Parameters

oldest_allowed_update_time	All joint information must be from this time or more current
missing_joints	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 169 of file current_state_monitor.h.

◆ haveCompleteState() [5/6]

bool planning_scene_monitor::CurrentStateMonitor::haveCompleteState ( const std::string & group = "" ) const

inline

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 131 of file current_state_monitor.h.

◆ haveCompleteState() [6/6]

bool planning_scene_monitor::CurrentStateMonitor::haveCompleteState	(	std::vector< std::string > &	missing_joints,
		const std::string &	group = `""`
	)		const

inline

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 159 of file current_state_monitor.h.

◆ haveCompleteStateHelper()

bool planning_scene_monitor::CurrentStateMonitor::haveCompleteStateHelper	(	const ros::Time &	oldest_allowed_update_time,
		std::vector< std::string > *	missing_joints,
		const std::string &	group
	)		const

private

Definition at line 227 of file current_state_monitor.cpp.

◆ isActive()

bool planning_scene_monitor::CurrentStateMonitor::isActive ( ) const

Check if the state monitor is started.

Definition at line 199 of file current_state_monitor.cpp.

◆ jointStateCallback()

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

private

Definition at line 326 of file current_state_monitor.cpp.

◆ setBoundsError()

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 241 of file current_state_monitor.h.

◆ setToCurrentState()

void planning_scene_monitor::CurrentStateMonitor::setToCurrentState ( moveit::core::RobotState & upd ) const

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

Definition at line 143 of file current_state_monitor.cpp.

◆ startStateMonitor()

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 179 of file current_state_monitor.cpp.

◆ stopStateMonitor()

void planning_scene_monitor::CurrentStateMonitor::stopStateMonitor ( )

Stop monitoring the "joint_states" topic.

Definition at line 204 of file current_state_monitor.cpp.

◆ tfCallback()

void planning_scene_monitor::CurrentStateMonitor::tfCallback ( )

private

Definition at line 416 of file current_state_monitor.cpp.

◆ waitForCompleteState() [1/2]

bool planning_scene_monitor::CurrentStateMonitor::waitForCompleteState	(	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 307 of file current_state_monitor.cpp.

◆ waitForCompleteState() [2/2]

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

Wait for at most wait_time seconds until the complete robot state is known.

Returns: true if the full state is known

Definition at line 294 of file current_state_monitor.cpp.

◆ waitForCurrentState()

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 272 of file current_state_monitor.cpp.

Member Data Documentation

◆ copy_dynamics_

bool planning_scene_monitor::CurrentStateMonitor::copy_dynamics_

private

Definition at line 280 of file current_state_monitor.h.

◆ error_

double planning_scene_monitor::CurrentStateMonitor::error_

private

Definition at line 282 of file current_state_monitor.h.

◆ joint_state_subscriber_

ros::Subscriber planning_scene_monitor::CurrentStateMonitor::joint_state_subscriber_

private

Definition at line 283 of file current_state_monitor.h.

◆ joint_time_

std::map<const moveit::core::JointModel*, ros::Time> planning_scene_monitor::CurrentStateMonitor::joint_time_

private

Definition at line 278 of file current_state_monitor.h.

◆ monitor_start_time_

ros::Time planning_scene_monitor::CurrentStateMonitor::monitor_start_time_

private

Definition at line 281 of file current_state_monitor.h.

◆ nh_

ros::NodeHandle planning_scene_monitor::CurrentStateMonitor::nh_

private

Definition at line 274 of file current_state_monitor.h.

◆ robot_model_

moveit::core::RobotModelConstPtr planning_scene_monitor::CurrentStateMonitor::robot_model_

private

Definition at line 276 of file current_state_monitor.h.

◆ robot_state_

moveit::core::RobotState planning_scene_monitor::CurrentStateMonitor::robot_state_

private

Definition at line 277 of file current_state_monitor.h.

◆ state_monitor_started_

bool planning_scene_monitor::CurrentStateMonitor::state_monitor_started_

private

Definition at line 279 of file current_state_monitor.h.

◆ state_update_condition_

boost::condition_variable planning_scene_monitor::CurrentStateMonitor::state_update_condition_

mutableprivate

Definition at line 286 of file current_state_monitor.h.

◆ state_update_lock_

boost::mutex planning_scene_monitor::CurrentStateMonitor::state_update_lock_

mutableprivate

Definition at line 285 of file current_state_monitor.h.

◆ tf_buffer_

std::shared_ptr<tf2_ros::Buffer> planning_scene_monitor::CurrentStateMonitor::tf_buffer_

private

Definition at line 275 of file current_state_monitor.h.

◆ tf_connection_

std::shared_ptr<TFConnection> planning_scene_monitor::CurrentStateMonitor::tf_connection_

private

Definition at line 289 of file current_state_monitor.h.

◆ update_callbacks_

std::vector<JointStateUpdateCallback> planning_scene_monitor::CurrentStateMonitor::update_callbacks_

private

Definition at line 287 of file current_state_monitor.h.

The documentation for this class was generated from the following files:

Public Member Functions

Private Types

Private Member Functions

Private Attributes

Detailed Description

Member Typedef Documentation

◆ TFConnection

Constructor & Destructor Documentation

◆ CurrentStateMonitor() [1/2]

◆ CurrentStateMonitor() [2/2]

◆ ~CurrentStateMonitor()

Member Function Documentation

◆ addUpdateCallback()

◆ clearUpdateCallbacks()

◆ enableCopyDynamics()

◆ getBoundsError()

◆ getCurrentState()

◆ getCurrentStateAndTime()

◆ getCurrentStateTime()

◆ getCurrentStateTimeHelper()

◆ getCurrentStateValues()

◆ getMonitoredTopic()

◆ getMonitorStartTime()

◆ getRobotModel()

◆ haveCompleteState() [1/6]

◆ haveCompleteState() [2/6]

◆ haveCompleteState() [3/6]

◆ haveCompleteState() [4/6]

◆ haveCompleteState() [5/6]

◆ haveCompleteState() [6/6]

◆ haveCompleteStateHelper()

◆ isActive()

◆ jointStateCallback()

◆ setBoundsError()

◆ setToCurrentState()

◆ startStateMonitor()

◆ stopStateMonitor()

◆ tfCallback()

◆ waitForCompleteState() [1/2]

◆ waitForCompleteState() [2/2]

◆ waitForCurrentState()

Member Data Documentation

◆ copy_dynamics_

◆ error_

◆ joint_state_subscriber_

◆ joint_time_

◆ monitor_start_time_

◆ nh_

◆ robot_model_

◆ robot_state_

◆ state_monitor_started_

◆ state_update_condition_

◆ state_update_lock_

◆ tf_buffer_

◆ tf_connection_

◆ update_callbacks_