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/thread/mutex.hpp> 46 #include <boost/thread/condition_variable.hpp> 65 const std::shared_ptr<tf2_ros::Buffer>& tf_buffer);
73 const std::shared_ptr<tf2_ros::Buffer>& tf_buffer,
const ros::NodeHandle& nh);
171 error_ = (error > 0) ? error : -error;
boost::signals2::connection TFConnection
void setToCurrentState(robot_state::RobotState &upd) const
Set the state upd to the current state maintained by this class.
void stopStateMonitor()
Stop monitoring the "joint_states" topic.
bool state_monitor_started_
bool haveCompleteState() const
Query whether we have joint state information for all DOFs in the kinematic model.
ros::Time getCurrentStateTime() const
Get the time stamp for the current state.
boost::mutex state_update_lock_
std::map< const moveit::core::JointModel *, ros::Time > joint_time_
bool isActive() const
Check if the state monitor is started.
Monitors the joint_states topic and tf to maintain the current state of the robot.
robot_model::RobotModelConstPtr robot_model_
geometry_msgs::TransformStamped t
CurrentStateMonitor(const robot_model::RobotModelConstPtr &robot_model, const std::shared_ptr< tf2_ros::Buffer > &tf_buffer)
Constructor.
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...
const robot_model::RobotModelConstPtr & getRobotModel() const
Get the RobotModel for which we are monitoring state.
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::vector< JointStateUpdateCallback > update_callbacks_
ros::Time current_state_time_
robot_state::RobotState robot_state_
void jointStateCallback(const sensor_msgs::JointStateConstPtr &joint_state)
std::shared_ptr< TFConnection > tf_connection_
std::shared_ptr< tf2_ros::Buffer > tf_buffer_
ros::Subscriber joint_state_subscriber_
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...
MOVEIT_CLASS_FORWARD(CurrentStateMonitor)
robot_state::RobotStatePtr getCurrentState() const
Get the current state.
bool waitForCompleteState(double wait_time) const
Wait for at most wait_time seconds until the complete robot state is known.
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
std::map< std::string, double > getCurrentStateValues() const
Get the current state values as a map from joint names to joint state values.
ros::Time monitor_start_time_
const ros::Time & getMonitorStartTime() const
Get the time point when the monitor was started.
std::string getMonitoredTopic() const
Get the name of the topic being monitored. Returns an empty string if the monitor is inactive...
void addUpdateCallback(const JointStateUpdateCallback &fn)
Add a function that will be called whenever the joint state is updated.
std::pair< robot_state::RobotStatePtr, ros::Time > getCurrentStateAndTime() const
Get the current state and its time stamp.