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 public:
64  CurrentStateMonitor(const robot_model::RobotModelConstPtr& robot_model, const boost::shared_ptr<tf::Transformer>& tf);
65 
71  CurrentStateMonitor(const robot_model::RobotModelConstPtr& robot_model, const boost::shared_ptr<tf::Transformer>& tf,
72  ros::NodeHandle nh);
73 
75 
79  void startStateMonitor(const std::string& joint_states_topic = "joint_states");
80 
83  void stopStateMonitor();
84 
86  bool isActive() const;
87 
89  const robot_model::RobotModelConstPtr& getRobotModel() const
90  {
91  return robot_model_;
92  }
93 
95  std::string getMonitoredTopic() const;
96 
100  bool haveCompleteState() const;
101 
106  bool haveCompleteState(const ros::Duration& age) const;
107 
112  bool haveCompleteState(std::vector<std::string>& missing_joints) const;
113 
119  bool haveCompleteState(const ros::Duration& age, std::vector<std::string>& missing_states) const;
120 
123  robot_state::RobotStatePtr getCurrentState() const;
124 
126  void setToCurrentState(robot_state::RobotState& upd) const;
127 
130 
133  std::pair<robot_state::RobotStatePtr, ros::Time> getCurrentStateAndTime() const;
134 
137  std::map<std::string, double> getCurrentStateValues() const;
138 
142  bool waitForCurrentState(const ros::Time t = ros::Time::now(), double wait_time = 1.0) const;
143 
146  bool waitForCompleteState(double wait_time) const;
148  MOVEIT_DEPRECATED bool waitForCurrentState(double wait_time) const;
149 
152  bool waitForCompleteState(const std::string& group, double wait_time) const;
154  MOVEIT_DEPRECATED bool waitForCurrentState(const std::string& group, double wait_time) const;
155 
158  {
159  return monitor_start_time_;
160  }
161 
163  void addUpdateCallback(const JointStateUpdateCallback& fn);
164 
166  void clearUpdateCallbacks();
167 
172  void setBoundsError(double error)
173  {
174  error_ = (error > 0) ? error : -error;
175  }
176 
181  double getBoundsError() const
182  {
183  return error_;
184  }
185 
189  void enableCopyDynamics(bool enabled)
190  {
191  copy_dynamics_ = enabled;
192  }
193 
194 private:
195  void jointStateCallback(const sensor_msgs::JointStateConstPtr& joint_state);
196  bool isPassiveOrMimicDOF(const std::string& dof) const;
197 
200  robot_model::RobotModelConstPtr robot_model_;
201  robot_state::RobotState robot_state_;
202  std::map<std::string, ros::Time> joint_time_;
204  bool copy_dynamics_; // Copy velocity and effort from joint_state
206  double error_;
210 
211  mutable boost::mutex state_update_lock_;
213  std::vector<JointStateUpdateCallback> update_callbacks_;
214 };
215 
217 }
218 
219 #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.
CurrentStateMonitor(const robot_model::RobotModelConstPtr &robot_model, const boost::shared_ptr< tf::Transformer > &tf)
Constructor.
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_
bool isPassiveOrMimicDOF(const std::string &dof) const
void jointStateCallback(const sensor_msgs::JointStateConstPtr &joint_state)
const ros::Time & getMonitorStartTime() const
Get the time point when the monitor was started.
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.
std::map< std::string, ros::Time > joint_time_
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 Sun Jan 21 2018 03:55:10