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 <tf2_ros/buffer.h>
43 #include <sensor_msgs/JointState.h>
44 #include <boost/function.hpp>
45 #include <boost/thread/mutex.hpp>
46 #include <boost/thread/condition_variable.hpp>
47 
49 {
50 typedef boost::function<void(const sensor_msgs::JointStateConstPtr& joint_state)> JointStateUpdateCallback;
51 
55 {
56  typedef boost::signals2::connection TFConnection;
57 
58 public:
64  CurrentStateMonitor(const robot_model::RobotModelConstPtr& robot_model,
65  const std::shared_ptr<tf2_ros::Buffer>& tf_buffer);
66 
72  CurrentStateMonitor(const robot_model::RobotModelConstPtr& robot_model,
73  const std::shared_ptr<tf2_ros::Buffer>& tf_buffer, const ros::NodeHandle& nh);
74 
76 
80  void startStateMonitor(const std::string& joint_states_topic = "joint_states");
81 
84  void stopStateMonitor();
85 
87  bool isActive() const;
88 
90  const robot_model::RobotModelConstPtr& getRobotModel() const
91  {
92  return robot_model_;
93  }
94 
96  std::string getMonitoredTopic() const;
97 
101  bool haveCompleteState() const;
102 
107  bool haveCompleteState(const ros::Duration& age) const;
108 
113  bool haveCompleteState(std::vector<std::string>& missing_joints) const;
114 
120  bool haveCompleteState(const ros::Duration& age, std::vector<std::string>& missing_states) const;
121 
124  robot_state::RobotStatePtr getCurrentState() const;
125 
127  void setToCurrentState(robot_state::RobotState& upd) const;
128 
131 
134  std::pair<robot_state::RobotStatePtr, ros::Time> getCurrentStateAndTime() const;
135 
138  std::map<std::string, double> getCurrentStateValues() const;
139 
143  bool waitForCurrentState(const ros::Time t = ros::Time::now(), double wait_time = 1.0) const;
144 
147  bool waitForCompleteState(double wait_time) const;
148 
151  bool waitForCompleteState(const std::string& group, double wait_time) const;
152 
155  {
156  return monitor_start_time_;
157  }
158 
160  void addUpdateCallback(const JointStateUpdateCallback& fn);
161 
163  void clearUpdateCallbacks();
164 
169  void setBoundsError(double error)
170  {
171  error_ = (error > 0) ? error : -error;
172  }
173 
178  double getBoundsError() const
179  {
180  return error_;
181  }
182 
186  void enableCopyDynamics(bool enabled)
187  {
188  copy_dynamics_ = enabled;
189  }
190 
191 private:
192  void jointStateCallback(const sensor_msgs::JointStateConstPtr& joint_state);
193  void tfCallback();
194 
196  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
197  robot_model::RobotModelConstPtr robot_model_;
198  robot_state::RobotState robot_state_;
199  std::map<const moveit::core::JointModel*, ros::Time> joint_time_;
201  bool copy_dynamics_; // Copy velocity and effort from joint_state
203  double error_;
206 
207  mutable boost::mutex state_update_lock_;
209  std::vector<JointStateUpdateCallback> update_callbacks_;
210 
211  std::shared_ptr<TFConnection> tf_connection_;
212 };
213 
215 }
216 
217 #endif
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 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.
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...
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_
void jointStateCallback(const sensor_msgs::JointStateConstPtr &joint_state)
std::shared_ptr< TFConnection > tf_connection_
std::shared_ptr< tf2_ros::Buffer > tf_buffer_
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.
static Time now()
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.
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.


planning
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Fri Oct 11 2019 03:56:35