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

#include <current_state_monitor.h>

Classes
class	MiddlewareHandle
	Dependency injection class for testing the CurrentStateMonitor. More...

Public Member Functions
	CurrentStateMonitor (std::unique_ptr< MiddlewareHandle > middleware_handle, const moveit::core::RobotModelConstPtr &robot_model, const std::shared_ptr< tf2_ros::Buffer > &tf_buffer, bool use_sim_time)
	Constructor.

	CurrentStateMonitor (const rclcpp::Node::SharedPtr &node, const moveit::core::RobotModelConstPtr &robot_model, const std::shared_ptr< tf2_ros::Buffer > &tf_buffer, bool use_sim_time)
	Constructor.

	~CurrentStateMonitor ()

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	isActive () const
	Check if the state monitor is started.

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

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

bool	haveCompleteState () const
	Query whether we have joint state information for all DOFs in the kinematic model.

bool	haveCompleteState (const rclcpp::Time &oldest_allowed_update_time) const
	Query whether we have joint state information for all DOFs in the kinematic model.

bool	haveCompleteState (const rclcpp::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 rclcpp::Time &oldest_allowed_update_time, std::vector< std::string > &missing_joints) const
	Query whether we have joint state information for all DOFs in the kinematic model.

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

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

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

rclcpp::Time	getCurrentStateTime () const
	Get the time stamp for the current state.

std::pair< moveit::core::RobotStatePtr, rclcpp::Time >	getCurrentStateAndTime () const
	Get the current state and its time stamp.

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

bool	waitForCurrentState (const rclcpp::Time &t=rclcpp::Clock(RCL_ROS_TIME).now(), double wait_time_s=1.0) const
	Wait for at most wait_time_s seconds (default 1s) for a robot state more recent than t.

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

bool	waitForCompleteState (const std::string &group, double wait_time_s) const
	Wait for at most wait_time_s seconds until the joint values from the group group are known. Return true if values for all joints in group are known.

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

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.

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.

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

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

Detailed Description

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

Definition at line 61 of file current_state_monitor.h.

Constructor & Destructor Documentation

◆ CurrentStateMonitor() [1/2]

planning_scene_monitor::CurrentStateMonitor::CurrentStateMonitor	(	std::unique_ptr< MiddlewareHandle >	middleware_handle,
		const moveit::core::RobotModelConstPtr &	robot_model,
		const std::shared_ptr< tf2_ros::Buffer > &	tf_buffer,
		bool	use_sim_time
	)

Constructor.

Parameters

middleware_handle	The ros middleware handle
robot_model	The current kinematic model to build on
tf_buffer	A pointer to the tf2_ros Buffer to use
use_sim_time	True when the time is abstracted

◆ CurrentStateMonitor() [2/2]

planning_scene_monitor::CurrentStateMonitor::CurrentStateMonitor	(	const rclcpp::Node::SharedPtr &	node,
		const moveit::core::RobotModelConstPtr &	robot_model,
		const std::shared_ptr< tf2_ros::Buffer > &	tf_buffer,
		bool	use_sim_time
	)

Constructor.

Parameters

node	A shared_ptr to a node used for subscription to joint_states_topic
robot_model	The current kinematic model to build on
tf_buffer	A pointer to the tf2_ros Buffer to use
use_sim_time	True when the time is abstracted

Definition at line 68 of file current_state_monitor.cpp.

◆ ~CurrentStateMonitor()

planning_scene_monitor::CurrentStateMonitor::~CurrentStateMonitor ( )

Definition at line 76 of file current_state_monitor.cpp.

Here is the call graph for this function:

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

◆ enableCopyDynamics()

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

inline

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

Definition at line 316 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 308 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 81 of file current_state_monitor.cpp.

◆ getCurrentStateAndTime()

std::pair< moveit::core::RobotStatePtr, rclcpp::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 94 of file current_state_monitor.cpp.

◆ getCurrentStateTime()

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

Get the time stamp for the current state.

Definition at line 88 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 101 of file current_state_monitor.cpp.

Here is the call graph for this function:

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

◆ getMonitorStartTime()

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

inline

Get the time point when the monitor was started.

Definition at line 284 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 188 of file current_state_monitor.h.

◆ haveCompleteState() [1/6]

bool planning_scene_monitor::CurrentStateMonitor::haveCompleteState ( ) 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 199 of file current_state_monitor.h.

Here is the caller graph for this function:

◆ haveCompleteState() [2/6]

bool planning_scene_monitor::CurrentStateMonitor::haveCompleteState ( const rclcpp::Duration & age ) 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 218 of file current_state_monitor.h.

◆ haveCompleteState() [3/6]

bool planning_scene_monitor::CurrentStateMonitor::haveCompleteState	(	const rclcpp::Duration &	age,
		std::vector< std::string > &	missing_joints
	)		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 247 of file current_state_monitor.h.

◆ haveCompleteState() [4/6]

bool planning_scene_monitor::CurrentStateMonitor::haveCompleteState ( const rclcpp::Time & oldest_allowed_update_time ) 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 208 of file current_state_monitor.h.

◆ haveCompleteState() [5/6]

bool planning_scene_monitor::CurrentStateMonitor::haveCompleteState	(	const rclcpp::Time &	oldest_allowed_update_time,
		std::vector< std::string > &	missing_joints
	)		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 237 of file current_state_monitor.h.

◆ haveCompleteState() [6/6]

bool planning_scene_monitor::CurrentStateMonitor::haveCompleteState ( std::vector< std::string > & missing_joints ) 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 227 of file current_state_monitor.h.

◆ isActive()

bool planning_scene_monitor::CurrentStateMonitor::isActive ( ) const

Check if the state monitor is started.

Definition at line 176 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 299 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 112 of file current_state_monitor.cpp.

Here is the call graph for this function:

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

Here is the caller graph for this function:

◆ stopStateMonitor()

void planning_scene_monitor::CurrentStateMonitor::stopStateMonitor ( )

Stop monitoring the "joint_states" topic.

Definition at line 181 of file current_state_monitor.cpp.

Here is the caller graph for this function:

◆ waitForCompleteState() [1/2]

bool planning_scene_monitor::CurrentStateMonitor::waitForCompleteState	(	const std::string &	group,
		double	wait_time_s
	)		const

Wait for at most wait_time_s 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 293 of file current_state_monitor.cpp.

Here is the call graph for this function:

◆ waitForCompleteState() [2/2]

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

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

Returns: true if the full state is known

Definition at line 280 of file current_state_monitor.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ waitForCurrentState()

bool planning_scene_monitor::CurrentStateMonitor::waitForCurrentState	(	const rclcpp::Time &	t = `rclcpp::Clock(RCL_ROS_TIME).now()`,
		double	wait_time_s = `1.0`
	)		const

Wait for at most wait_time_s 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_s

Definition at line 232 of file current_state_monitor.cpp.

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

moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/current_state_monitor.h
moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp

Classes

Public Member Functions

Detailed Description

Constructor & Destructor Documentation

◆ CurrentStateMonitor() [1/2]

◆ CurrentStateMonitor() [2/2]

◆ ~CurrentStateMonitor()

Member Function Documentation

◆ addUpdateCallback()

◆ clearUpdateCallbacks()

◆ enableCopyDynamics()

◆ getBoundsError()

◆ getCurrentState()

◆ getCurrentStateAndTime()

◆ getCurrentStateTime()

◆ getCurrentStateValues()

◆ getMonitoredTopic()

◆ getMonitorStartTime()

◆ getRobotModel()

◆ haveCompleteState() [1/6]

◆ haveCompleteState() [2/6]

◆ haveCompleteState() [3/6]

◆ haveCompleteState() [4/6]

◆ haveCompleteState() [5/6]

◆ haveCompleteState() [6/6]

◆ isActive()

◆ setBoundsError()

◆ setToCurrentState()

◆ startStateMonitor()

◆ stopStateMonitor()

◆ waitForCompleteState() [1/2]

◆ waitForCompleteState() [2/2]

◆ waitForCurrentState()