current_state_monitor.h
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Ioan Sucan */
36 
37 #ifndef MOVEIT_PLANNING_SCENE_MONITOR_CURRENT_STATE_MONITOR_
38 #define MOVEIT_PLANNING_SCENE_MONITOR_CURRENT_STATE_MONITOR_
39 
40 #include <ros/ros.h>
41 #include <tf/tf.h>
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>
49 
51 {
52 typedef boost::function<void(const sensor_msgs::JointStateConstPtr& joint_state)> JointStateUpdateCallback;
53 
57 {
58  /* tf changed their interface between indigo and kinetic
59  from boost::signals::connection to boost::signals2::connection */
60  typedef decltype(tf::Transformer().addTransformsChangedListener(boost::function<void(void)>())) TFConnection;
61 
62 public:
68  CurrentStateMonitor(const robot_model::RobotModelConstPtr& robot_model, const boost::shared_ptr<tf::Transformer>& tf);
69 
75  CurrentStateMonitor(const robot_model::RobotModelConstPtr& robot_model, const boost::shared_ptr<tf::Transformer>& tf,
76  ros::NodeHandle nh);
77 
79 
83  void startStateMonitor(const std::string& joint_states_topic = "joint_states");
84 
87  void stopStateMonitor();
88 
90  bool isActive() const;
91 
93  const robot_model::RobotModelConstPtr& getRobotModel() const
94  {
95  return robot_model_;
96  }
97 
99  std::string getMonitoredTopic() const;
100 
104  bool haveCompleteState() const;
105 
110  bool haveCompleteState(const ros::Duration& age) const;
111 
116  bool haveCompleteState(std::vector<std::string>& missing_joints) const;
117 
123  bool haveCompleteState(const ros::Duration& age, std::vector<std::string>& missing_states) const;
124 
127  robot_state::RobotStatePtr getCurrentState() const;
128 
130  void setToCurrentState(robot_state::RobotState& upd) const;
131 
134 
137  std::pair<robot_state::RobotStatePtr, ros::Time> getCurrentStateAndTime() const;
138 
141  std::map<std::string, double> getCurrentStateValues() const;
142 
146  bool waitForCurrentState(const ros::Time t = ros::Time::now(), double wait_time = 1.0) const;
147 
150  bool waitForCompleteState(double wait_time) const;
152  MOVEIT_DEPRECATED bool waitForCurrentState(double wait_time) const;
153 
156  bool waitForCompleteState(const std::string& group, double wait_time) const;
158  MOVEIT_DEPRECATED bool waitForCurrentState(const std::string& group, double wait_time) const;
159 
162  {
163  return monitor_start_time_;
164  }
165 
167  void addUpdateCallback(const JointStateUpdateCallback& fn);
168 
170  void clearUpdateCallbacks();
171 
176  void setBoundsError(double error)
177  {
178  error_ = (error > 0) ? error : -error;
179  }
180 
185  double getBoundsError() const
186  {
187  return error_;
188  }
189 
193  void enableCopyDynamics(bool enabled)
194  {
195  copy_dynamics_ = enabled;
196  }
197 
198 private:
199  void jointStateCallback(const sensor_msgs::JointStateConstPtr& joint_state);
200  void tfCallback();
201 
204  robot_model::RobotModelConstPtr robot_model_;
205  robot_state::RobotState robot_state_;
206  std::map<const moveit::core::JointModel*, ros::Time> joint_time_;
208  bool copy_dynamics_; // Copy velocity and effort from joint_state
210  double error_;
213 
214  mutable boost::mutex state_update_lock_;
216  std::vector<JointStateUpdateCallback> update_callbacks_;
217 
218  std::shared_ptr<TFConnection> tf_connection_;
219 };
220 
222 }
223 
224 #endif
void stopStateMonitor()
Stop monitoring the "joint_states" topic.
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
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...
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_
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_
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.
static Time now()
boost::function< void(const sensor_msgs::JointStateConstPtr &joint_state)> JointStateUpdateCallback
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.


planning
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Wed Jul 10 2019 04:03:32