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  inline bool haveCompleteState() const
102  {
103  return haveCompleteStateHelper(ros::Time(0), nullptr);
104  }
105 
110  inline bool haveCompleteState(const ros::Time& oldest_allowed_update_time) const
111  {
112  return haveCompleteStateHelper(oldest_allowed_update_time, nullptr);
113  }
114 
120  inline bool haveCompleteState(const ros::Duration& age) const
121  {
122  return haveCompleteStateHelper(ros::Time::now() - age, nullptr);
123  }
124 
129  inline bool haveCompleteState(std::vector<std::string>& missing_joints) const
130  {
131  return haveCompleteStateHelper(ros::Time(0), &missing_joints);
132  }
133 
139  inline bool haveCompleteState(const ros::Time& oldest_allowed_update_time,
140  std::vector<std::string>& missing_joints) const
141  {
142  return haveCompleteStateHelper(oldest_allowed_update_time, &missing_joints);
143  }
144 
149  inline bool haveCompleteState(const ros::Duration& age, std::vector<std::string>& missing_joints) const
150  {
151  return haveCompleteStateHelper(ros::Time::now() - age, &missing_joints);
152  }
153 
156  robot_state::RobotStatePtr getCurrentState() const;
157 
159  void setToCurrentState(robot_state::RobotState& upd) const;
160 
163 
166  std::pair<robot_state::RobotStatePtr, ros::Time> getCurrentStateAndTime() const;
167 
170  std::map<std::string, double> getCurrentStateValues() const;
171 
175  bool waitForCurrentState(const ros::Time t = ros::Time::now(), double wait_time = 1.0) const;
176 
179  bool waitForCompleteState(double wait_time) const;
180 
183  bool waitForCompleteState(const std::string& group, double wait_time) const;
184 
187  {
188  return monitor_start_time_;
189  }
190 
192  void addUpdateCallback(const JointStateUpdateCallback& fn);
193 
195  void clearUpdateCallbacks();
196 
201  void setBoundsError(double error)
202  {
203  error_ = (error > 0) ? error : -error;
204  }
205 
210  double getBoundsError() const
211  {
212  return error_;
213  }
214 
218  void enableCopyDynamics(bool enabled)
219  {
220  copy_dynamics_ = enabled;
221  }
222 
223 private:
224  bool haveCompleteStateHelper(const ros::Time& oldest_allowed_update_time,
225  std::vector<std::string>* missing_joints) const;
226 
227  void jointStateCallback(const sensor_msgs::JointStateConstPtr& joint_state);
228  void tfCallback();
229 
231  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
232  robot_model::RobotModelConstPtr robot_model_;
233  robot_state::RobotState robot_state_;
234  std::map<const moveit::core::JointModel*, ros::Time> joint_time_;
236  bool copy_dynamics_; // Copy velocity and effort from joint_state
238  double error_;
241 
242  mutable boost::mutex state_update_lock_;
243  mutable boost::condition_variable state_update_condition_;
244  std::vector<JointStateUpdateCallback> update_callbacks_;
245 
246  std::shared_ptr<TFConnection> tf_connection_;
247 };
248 
249 MOVEIT_CLASS_FORWARD(CurrentStateMonitor); // Defines CurrentStateMonitorPtr, ConstPtr, WeakPtr... etc
250 } // namespace planning_scene_monitor
251 
252 #endif
void setToCurrentState(robot_state::RobotState &upd) const
Set the state upd to the current state maintained by this class.
bool haveCompleteState(const ros::Time &oldest_allowed_update_time) const
Query whether we have joint state information for all DOFs in the kinematic model.
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
bool haveCompleteState(const ros::Time &oldest_allowed_update_time, std::vector< std::string > &missing_joints) const
Query whether we have joint state information for all DOFs in the kinematic model.
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::pair< robot_state::RobotStatePtr, ros::Time > getCurrentStateAndTime() const
Get the current state and its time stamp.
bool haveCompleteState(std::vector< std::string > &missing_joints) const
Query whether we have joint state information for all DOFs in the kinematic model.
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)
bool haveCompleteState(const ros::Duration &age) const
Query whether we have joint state information for all DOFs in the kinematic model.
bool haveCompleteStateHelper(const ros::Time &oldest_allowed_update_time, std::vector< std::string > *missing_joints) const
bool waitForCompleteState(double wait_time) const
Wait for at most wait_time seconds until the complete robot state is known.
robot_state::RobotStatePtr getCurrentState() const
Get the current state.
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.
bool haveCompleteState(const ros::Duration &age, std::vector< std::string > &missing_joints) const
Query whether we have joint state information for all DOFs in the kinematic model.
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.


planning
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Wed Jun 23 2021 02:52:45