37 #ifndef MOVEIT_PLANNING_SCENE_MONITOR_CURRENT_STATE_MONITOR_ 38 #define MOVEIT_PLANNING_SCENE_MONITOR_CURRENT_STATE_MONITOR_ 43 #include <sensor_msgs/JointState.h> 44 #include <boost/function.hpp> 45 #include <boost/shared_ptr.hpp> 46 #include <boost/thread/mutex.hpp> 48 #include <boost/thread/condition_variable.hpp> 75 CurrentStateMonitor(const robot_model::RobotModelConstPtr& robot_model, const
boost::shared_ptr<tf::Transformer>& tf,
178 error_ = (error > 0) ? error : -error;
void stopStateMonitor()
Stop monitoring the "joint_states" topic.
bool state_monitor_started_
boost::shared_ptr< tf::Transformer > tf_
void setToCurrentState(robot_state::RobotState &upd) const
Set the state upd to the current state maintained by this class.
decltype(tf::Transformer().addTransformsChangedListener(boost::function< void(void)>())) typedef TFConnection
boost::mutex state_update_lock_
std::map< const moveit::core::JointModel *, ros::Time > joint_time_
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...
Monitors the joint_states topic and tf to maintain the current state of the robot.
#define MOVEIT_DEPRECATED
robot_model::RobotModelConstPtr robot_model_
std::map< std::string, double > getCurrentStateValues() const
Get the current state values as a map from joint names to joint state values.
void startStateMonitor(const std::string &joint_states_topic="joint_states")
Start monitoring joint states on a particular topic.
void setBoundsError(double error)
When a joint value is received to be out of bounds, it is changed slightly to fit within bounds...
std::pair< robot_state::RobotStatePtr, ros::Time > getCurrentStateAndTime() const
Get the current state and its time stamp.
void enableCopyDynamics(bool enabled)
Allow the joint_state arrrays velocity and effort to be copied into the robot state this is useful in...
boost::condition_variable state_update_condition_
double getBoundsError() const
When a joint value is received to be out of bounds, it is changed slightly to fit within bounds...
std::string getMonitoredTopic() const
Get the name of the topic being monitored. Returns an empty string if the monitor is inactive...
std::vector< JointStateUpdateCallback > update_callbacks_
ros::Time current_state_time_
robot_state::RobotState robot_state_
void jointStateCallback(const sensor_msgs::JointStateConstPtr &joint_state)
const ros::Time & getMonitorStartTime() const
Get the time point when the monitor was started.
std::shared_ptr< TFConnection > tf_connection_
ros::Subscriber joint_state_subscriber_
bool waitForCompleteState(double wait_time) const
Wait for at most wait_time seconds until the complete robot state is known.
MOVEIT_CLASS_FORWARD(CurrentStateMonitor)
bool haveCompleteState() const
Query whether we have joint state information for all DOFs in the kinematic model.
void clearUpdateCallbacks()
Clear the functions to be called when an update to the joint state is received.
boost::function< void(const sensor_msgs::JointStateConstPtr &joint_state)> JointStateUpdateCallback
ros::Time monitor_start_time_
ros::Time getCurrentStateTime() const
Get the time stamp for the current state.
void addUpdateCallback(const JointStateUpdateCallback &fn)
Add a function that will be called whenever the joint state is updated.
bool isActive() const
Check if the state monitor is started.
const robot_model::RobotModelConstPtr & getRobotModel() const
Get the RobotModel for which we are monitoring state.
robot_state::RobotStatePtr getCurrentState() const
Get the current state.