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. | |
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 (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 | isPassiveDOF (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_ |
Monitors the joint_states topic and tf to maintain the current state of the robot.
Definition at line 55 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 41 of file current_state_monitor.cpp.
Definition at line 51 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 94 of file current_state_monitor.cpp.
Clear the functions to be called when an update to the joint state is received.
Definition at line 100 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").
Definition at line 161 of file current_state_monitor.h.
robot_state::RobotStatePtr planning_scene_monitor::CurrentStateMonitor::getCurrentState | ( | ) | const |
Get the current state.
Definition at line 56 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 69 of file current_state_monitor.cpp.
Get the time stamp for the current state.
Definition at line 63 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 76 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 135 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 137 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 80 of file current_state_monitor.h.
Query whether we have joint state information for all DOFs in the kinematic model.
Definition at line 165 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 197 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 182 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 225 of file current_state_monitor.cpp.
bool planning_scene_monitor::CurrentStateMonitor::isActive | ( | ) | const |
Check if the state monitor is started.
Definition at line 120 of file current_state_monitor.cpp.
bool planning_scene_monitor::CurrentStateMonitor::isPassiveDOF | ( | const std::string & | dof | ) | const [private] |
Definition at line 143 of file current_state_monitor.cpp.
void planning_scene_monitor::CurrentStateMonitor::jointStateCallback | ( | const sensor_msgs::JointStateConstPtr & | joint_state | ) | [private] |
Definition at line 296 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.
error | The specified value for the "allowed bounds error". The default is machine precision. |
Definition at line 152 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 87 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 105 of file current_state_monitor.cpp.
Stop monitoring the "joint_states" topic.
Definition at line 125 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 256 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 269 of file current_state_monitor.cpp.
Definition at line 180 of file current_state_monitor.h.
double planning_scene_monitor::CurrentStateMonitor::error_ [private] |
Definition at line 178 of file current_state_monitor.h.
Definition at line 179 of file current_state_monitor.h.
std::map<std::string, ros::Time> planning_scene_monitor::CurrentStateMonitor::joint_time_ [private] |
Definition at line 175 of file current_state_monitor.h.
Definition at line 181 of file current_state_monitor.h.
Definition at line 177 of file current_state_monitor.h.
Definition at line 171 of file current_state_monitor.h.
robot_model::RobotModelConstPtr planning_scene_monitor::CurrentStateMonitor::robot_model_ [private] |
Definition at line 173 of file current_state_monitor.h.
robot_state::RobotState planning_scene_monitor::CurrentStateMonitor::robot_state_ [private] |
Definition at line 174 of file current_state_monitor.h.
Definition at line 176 of file current_state_monitor.h.
boost::mutex planning_scene_monitor::CurrentStateMonitor::state_update_lock_ [mutable, private] |
Definition at line 183 of file current_state_monitor.h.
boost::shared_ptr<tf::Transformer> planning_scene_monitor::CurrentStateMonitor::tf_ [private] |
Definition at line 172 of file current_state_monitor.h.
std::vector< JointStateUpdateCallback > planning_scene_monitor::CurrentStateMonitor::update_callbacks_ [private] |
Definition at line 184 of file current_state_monitor.h.