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 boost::shared_ptr< tf::Transformer > &tf) | |
Constructor. More... | |
CurrentStateMonitor (const robot_model::RobotModelConstPtr &robot_model, const boost::shared_ptr< tf::Transformer > &tf, 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::Time > | getCurrentStateAndTime () 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::Time & | getMonitorStartTime () 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... | |
MOVEIT_DEPRECATED bool | waitForCurrentState (double wait_time) const |
MOVEIT_DEPRECATED bool | waitForCurrentState (const std::string &group, double wait_time) const |
~CurrentStateMonitor () | |
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::Time > | joint_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_ |
boost::shared_ptr< tf::Transformer > | tf_ |
std::shared_ptr< TFConnection > | tf_connection_ |
decltype(tf::Transformer().addTransformsChangedListener(boost::function< void(void)>())) typedef | TFConnection |
std::vector< JointStateUpdateCallback > | update_callbacks_ |
Monitors the joint_states topic and tf to maintain the current state of the robot.
Definition at line 56 of file current_state_monitor.h.
planning_scene_monitor::CurrentStateMonitor::CurrentStateMonitor | ( | const robot_model::RobotModelConstPtr & | robot_model, |
const boost::shared_ptr< tf::Transformer > & | tf | ||
) |
Constructor.
robot_model | The current kinematic model to build on |
tf | A pointer to the tf transformer to use |
Definition at line 43 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.
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 49 of file current_state_monitor.cpp.
planning_scene_monitor::CurrentStateMonitor::~CurrentStateMonitor | ( | ) |
Definition at line 63 of file current_state_monitor.cpp.
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 125 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 131 of file current_state_monitor.cpp.
|
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 193 of file current_state_monitor.h.
|
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").
Definition at line 185 of file current_state_monitor.h.
robot_state::RobotStatePtr planning_scene_monitor::CurrentStateMonitor::getCurrentState | ( | ) | const |
Get the current state.
Definition at line 68 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.
Definition at line 82 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 75 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.
Definition at line 89 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 176 of file current_state_monitor.cpp.
|
inline |
Get the time point when the monitor was started.
Definition at line 161 of file current_state_monitor.h.
|
inline |
Get the RobotModel for which we are monitoring state.
Definition at line 93 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.
Definition at line 184 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.
Definition at line 216 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.
missing_joints | Returns the list of joints that are missing |
Definition at line 201 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.
age | The max allowed age of the joint state information |
missing_states | Returns the list of joints that are missing |
Definition at line 243 of file current_state_monitor.cpp.
bool planning_scene_monitor::CurrentStateMonitor::isActive | ( | ) | const |
Check if the state monitor is started.
Definition at line 156 of file current_state_monitor.cpp.
|
private |
Definition at line 344 of file current_state_monitor.cpp.
|
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.
error | The specified value for the "allowed bounds error". The default is machine precision. |
Definition at line 176 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 100 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.
joint_states_topic | The topic name for joint states (defaults to "joint_states") |
Definition at line 136 of file current_state_monitor.cpp.
void planning_scene_monitor::CurrentStateMonitor::stopStateMonitor | ( | ) |
Stop monitoring the "joint_states" topic.
Definition at line 161 of file current_state_monitor.cpp.
|
private |
Definition at line 418 of file current_state_monitor.cpp.
bool planning_scene_monitor::CurrentStateMonitor::waitForCompleteState | ( | double | wait_time | ) | const |
Wait for at most wait_time seconds until the complete robot state is known.
Definition at line 295 of file current_state_monitor.cpp.
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 312 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.
Definition at line 273 of file current_state_monitor.cpp.
bool planning_scene_monitor::CurrentStateMonitor::waitForCurrentState | ( | double | wait_time | ) | const |
replaced by waitForCompleteState, will be removed in L-turtle: function waits for complete robot state
Definition at line 307 of file current_state_monitor.cpp.
bool planning_scene_monitor::CurrentStateMonitor::waitForCurrentState | ( | const std::string & | group, |
double | wait_time | ||
) | const |
replaced by waitForCompleteState, will be removed in L-turtle: function waits for complete robot state
Definition at line 339 of file current_state_monitor.cpp.
|
private |
Definition at line 208 of file current_state_monitor.h.
|
private |
Definition at line 212 of file current_state_monitor.h.
|
private |
Definition at line 210 of file current_state_monitor.h.
|
private |
Definition at line 211 of file current_state_monitor.h.
|
private |
Definition at line 206 of file current_state_monitor.h.
|
private |
Definition at line 209 of file current_state_monitor.h.
|
private |
Definition at line 202 of file current_state_monitor.h.
|
private |
Definition at line 204 of file current_state_monitor.h.
|
private |
Definition at line 205 of file current_state_monitor.h.
|
private |
Definition at line 207 of file current_state_monitor.h.
|
mutableprivate |
Definition at line 215 of file current_state_monitor.h.
|
mutableprivate |
Definition at line 214 of file current_state_monitor.h.
|
private |
Definition at line 203 of file current_state_monitor.h.
|
private |
Definition at line 218 of file current_state_monitor.h.
|
private |
Definition at line 60 of file current_state_monitor.h.
|
private |
Definition at line 216 of file current_state_monitor.h.