Public Member Functions | Private Types | Private Member Functions | Private Attributes | List of all members
planning_scene_monitor::CurrentStateMonitor Class Reference

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. More...
 
void clearUpdateCallbacks ()
 Clear the functions to be called when an update to the joint state is received. More...
 
 CurrentStateMonitor (const robot_model::RobotModelConstPtr &robot_model, const std::shared_ptr< tf2_ros::Buffer > &tf_buffer)
 Constructor. More...
 
 CurrentStateMonitor (const robot_model::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...
 
robot_state::RobotStatePtr getCurrentState () const
 Get the current state. More...
 
std::pair< robot_state::RobotStatePtr, ros::TimegetCurrentStateAndTime () const
 Get the current state and its time stamp. More...
 
ros::Time getCurrentStateTime () 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::TimegetMonitorStartTime () const
 Get the time point when the monitor was started. More...
 
const robot_model::RobotModelConstPtr & getRobotModel () const
 Get the RobotModel for which we are monitoring state. More...
 
bool haveCompleteState () const
 Query whether we have joint state information for all DOFs in the kinematic model. More...
 
bool haveCompleteState (const ros::Duration &age) 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
 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_states) 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 (robot_state::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 (double wait_time) const
 Wait for at most wait_time seconds until the complete robot state is known. 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 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

typedef boost::signals2::connection TFConnection
 

Private Member Functions

void jointStateCallback (const sensor_msgs::JointStateConstPtr &joint_state)
 
void tfCallback ()
 

Private Attributes

bool copy_dynamics_
 
ros::Time current_state_time_
 
double error_
 
ros::Subscriber joint_state_subscriber_
 
std::map< const moveit::core::JointModel *, ros::Timejoint_time_
 
ros::Time monitor_start_time_
 
ros::NodeHandle nh_
 
robot_model::RobotModelConstPtr robot_model_
 
robot_state::RobotState robot_state_
 
bool state_monitor_started_
 
boost::condition_variable state_update_condition_
 
boost::mutex state_update_lock_
 
std::shared_ptr< tf2_ros::Buffertf_buffer_
 
std::shared_ptr< TFConnectiontf_connection_
 
std::vector< JointStateUpdateCallbackupdate_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.

Member Typedef Documentation

◆ TFConnection

typedef boost::signals2::connection planning_scene_monitor::CurrentStateMonitor::TFConnection
private

Definition at line 56 of file current_state_monitor.h.

Constructor & Destructor Documentation

◆ CurrentStateMonitor() [1/2]

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

Constructor.

Parameters
robot_modelThe current kinematic model to build on
tf_bufferA pointer to the tf2_ros Buffer to use

Definition at line 44 of file current_state_monitor.cpp.

◆ CurrentStateMonitor() [2/2]

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

Constructor.

Parameters
robot_modelThe current kinematic model to build on
tf_bufferA pointer to the tf2_ros Buffer to use
nhA ros::NodeHandle to pass node specific options

Definition at line 50 of file current_state_monitor.cpp.

◆ ~CurrentStateMonitor()

planning_scene_monitor::CurrentStateMonitor::~CurrentStateMonitor ( )

Definition at line 64 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 126 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 132 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 186 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 178 of file current_state_monitor.h.

◆ getCurrentState()

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

Get the current state.

Returns
Returns the current state

Definition at line 69 of file current_state_monitor.cpp.

◆ getCurrentStateAndTime()

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

◆ getCurrentStateTime()

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

Get the time stamp for the current state.

Definition at line 76 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 90 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 177 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 154 of file current_state_monitor.h.

◆ getRobotModel()

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

Get the RobotModel for which we are monitoring state.

Definition at line 90 of file current_state_monitor.h.

◆ haveCompleteState() [1/4]

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

◆ haveCompleteState() [2/4]

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

◆ haveCompleteState() [3/4]

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

◆ haveCompleteState() [4/4]

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
ageThe max allowed age of the joint state information
missing_statesReturns 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 244 of file current_state_monitor.cpp.

◆ isActive()

bool planning_scene_monitor::CurrentStateMonitor::isActive ( ) const

Check if the state monitor is started.

Definition at line 157 of file current_state_monitor.cpp.

◆ jointStateCallback()

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

Definition at line 336 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
errorThe specified value for the "allowed bounds error". The default is machine precision.

Definition at line 169 of file current_state_monitor.h.

◆ setToCurrentState()

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 101 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_topicThe topic name for joint states (defaults to "joint_states")

Definition at line 137 of file current_state_monitor.cpp.

◆ stopStateMonitor()

void planning_scene_monitor::CurrentStateMonitor::stopStateMonitor ( )

Stop monitoring the "joint_states" topic.

Definition at line 162 of file current_state_monitor.cpp.

◆ tfCallback()

void planning_scene_monitor::CurrentStateMonitor::tfCallback ( )
private

Definition at line 414 of file current_state_monitor.cpp.

◆ waitForCompleteState() [1/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 296 of file current_state_monitor.cpp.

◆ waitForCompleteState() [2/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 309 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 274 of file current_state_monitor.cpp.

Member Data Documentation

◆ copy_dynamics_

bool planning_scene_monitor::CurrentStateMonitor::copy_dynamics_
private

Definition at line 201 of file current_state_monitor.h.

◆ current_state_time_

ros::Time planning_scene_monitor::CurrentStateMonitor::current_state_time_
private

Definition at line 205 of file current_state_monitor.h.

◆ error_

double planning_scene_monitor::CurrentStateMonitor::error_
private

Definition at line 203 of file current_state_monitor.h.

◆ joint_state_subscriber_

ros::Subscriber planning_scene_monitor::CurrentStateMonitor::joint_state_subscriber_
private

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

◆ monitor_start_time_

ros::Time planning_scene_monitor::CurrentStateMonitor::monitor_start_time_
private

Definition at line 202 of file current_state_monitor.h.

◆ nh_

ros::NodeHandle planning_scene_monitor::CurrentStateMonitor::nh_
private

Definition at line 195 of file current_state_monitor.h.

◆ robot_model_

robot_model::RobotModelConstPtr planning_scene_monitor::CurrentStateMonitor::robot_model_
private

Definition at line 197 of file current_state_monitor.h.

◆ robot_state_

robot_state::RobotState planning_scene_monitor::CurrentStateMonitor::robot_state_
private

Definition at line 198 of file current_state_monitor.h.

◆ state_monitor_started_

bool planning_scene_monitor::CurrentStateMonitor::state_monitor_started_
private

Definition at line 200 of file current_state_monitor.h.

◆ state_update_condition_

boost::condition_variable planning_scene_monitor::CurrentStateMonitor::state_update_condition_
mutableprivate

Definition at line 208 of file current_state_monitor.h.

◆ state_update_lock_

boost::mutex planning_scene_monitor::CurrentStateMonitor::state_update_lock_
mutableprivate

Definition at line 207 of file current_state_monitor.h.

◆ tf_buffer_

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

Definition at line 196 of file current_state_monitor.h.

◆ tf_connection_

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

Definition at line 211 of file current_state_monitor.h.

◆ update_callbacks_

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

Definition at line 209 of file current_state_monitor.h.


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


planning
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Mon Nov 23 2020 03:53:16