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 #pragma once
38 
39 #include <ros/ros.h>
40 #include <tf2_ros/buffer.h>
42 #include <sensor_msgs/JointState.h>
43 #include <boost/function.hpp>
44 #include <boost/thread/mutex.hpp>
45 #include <boost/thread/condition_variable.hpp>
46 
48 {
49 using JointStateUpdateCallback = boost::function<void(const sensor_msgs::JointStateConstPtr&)>;
50 
54 {
55  using TFConnection = boost::signals2::connection;
56 
57 public:
63  CurrentStateMonitor(const moveit::core::RobotModelConstPtr& robot_model,
64  const std::shared_ptr<tf2_ros::Buffer>& tf_buffer);
65 
71  CurrentStateMonitor(const moveit::core::RobotModelConstPtr& robot_model,
72  const std::shared_ptr<tf2_ros::Buffer>& tf_buffer, const 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 moveit::core::RobotModelConstPtr& getRobotModel() const
90  {
91  return robot_model_;
92  }
93 
95  std::string getMonitoredTopic() const;
96 
100  inline bool haveCompleteState() const
101  {
102  return haveCompleteStateHelper(ros::Time(0), nullptr);
103  }
104 
109  inline bool haveCompleteState(const ros::Time& oldest_allowed_update_time) const
110  {
111  return haveCompleteStateHelper(oldest_allowed_update_time, nullptr);
112  }
113 
119  inline bool haveCompleteState(const ros::Duration& age) const
120  {
121  return haveCompleteStateHelper(ros::Time::now() - age, nullptr);
122  }
123 
128  inline bool haveCompleteState(std::vector<std::string>& missing_joints) const
129  {
130  return haveCompleteStateHelper(ros::Time(0), &missing_joints);
131  }
132 
138  inline bool haveCompleteState(const ros::Time& oldest_allowed_update_time,
139  std::vector<std::string>& missing_joints) const
140  {
141  return haveCompleteStateHelper(oldest_allowed_update_time, &missing_joints);
142  }
143 
148  inline bool haveCompleteState(const ros::Duration& age, std::vector<std::string>& missing_joints) const
149  {
150  return haveCompleteStateHelper(ros::Time::now() - age, &missing_joints);
151  }
152 
155  moveit::core::RobotStatePtr getCurrentState() const;
156 
159 
162 
165  std::pair<moveit::core::RobotStatePtr, ros::Time> getCurrentStateAndTime() const;
166 
169  std::map<std::string, double> getCurrentStateValues() const;
170 
174  bool waitForCurrentState(const ros::Time t = ros::Time::now(), double wait_time = 1.0) const;
175 
178  bool waitForCompleteState(double wait_time) const;
179 
182  bool waitForCompleteState(const std::string& group, double wait_time) const;
183 
185  const ros::Time& getMonitorStartTime() const
186  {
187  return monitor_start_time_;
188  }
189 
192 
194  void clearUpdateCallbacks();
195 
200  void setBoundsError(double error)
201  {
202  error_ = (error > 0) ? error : -error;
203  }
204 
209  double getBoundsError() const
210  {
211  return error_;
212  }
213 
217  void enableCopyDynamics(bool enabled)
218  {
219  copy_dynamics_ = enabled;
220  }
221 
222 private:
223  bool haveCompleteStateHelper(const ros::Time& oldest_allowed_update_time,
224  std::vector<std::string>* missing_joints) const;
225 
226  void jointStateCallback(const sensor_msgs::JointStateConstPtr& joint_state);
227  void tfCallback();
228 
230  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
231  moveit::core::RobotModelConstPtr robot_model_;
233  std::map<const moveit::core::JointModel*, ros::Time> joint_time_;
235  bool copy_dynamics_; // Copy velocity and effort from joint_state
237  double error_;
240 
241  mutable boost::mutex state_update_lock_;
242  mutable boost::condition_variable state_update_condition_;
243  std::vector<JointStateUpdateCallback> update_callbacks_;
244 
245  std::shared_ptr<TFConnection> tf_connection_;
246 };
247 
248 MOVEIT_CLASS_FORWARD(CurrentStateMonitor); // Defines CurrentStateMonitorPtr, ConstPtr, WeakPtr... etc
249 } // namespace planning_scene_monitor
planning_scene_monitor::CurrentStateMonitor::haveCompleteState
bool haveCompleteState() const
Query whether we have joint state information for all DOFs in the kinematic model.
Definition: current_state_monitor.h:132
planning_scene_monitor::CurrentStateMonitor::robot_model_
moveit::core::RobotModelConstPtr robot_model_
Definition: current_state_monitor.h:263
planning_scene_monitor
Definition: current_state_monitor.h:47
planning_scene_monitor::CurrentStateMonitor::getCurrentStateTime
ros::Time getCurrentStateTime() const
Get the time stamp for the current state.
Definition: current_state_monitor.cpp:79
planning_scene_monitor::CurrentStateMonitor::stopStateMonitor
void stopStateMonitor()
Stop monitoring the "joint_states" topic.
Definition: current_state_monitor.cpp:164
planning_scene_monitor::CurrentStateMonitor::joint_time_
std::map< const moveit::core::JointModel *, ros::Time > joint_time_
Definition: current_state_monitor.h:265
planning_scene_monitor::CurrentStateMonitor::getRobotModel
const moveit::core::RobotModelConstPtr & getRobotModel() const
Get the RobotModel for which we are monitoring state.
Definition: current_state_monitor.h:121
ros.h
planning_scene_monitor::CurrentStateMonitor::state_update_lock_
boost::mutex state_update_lock_
Definition: current_state_monitor.h:273
planning_scene_monitor::CurrentStateMonitor::isActive
bool isActive() const
Check if the state monitor is started.
Definition: current_state_monitor.cpp:159
buffer.h
moveit::core::RobotState
planning_scene_monitor::CurrentStateMonitor::CurrentStateMonitor
CurrentStateMonitor(const moveit::core::RobotModelConstPtr &robot_model, const std::shared_ptr< tf2_ros::Buffer > &tf_buffer)
Constructor.
Definition: current_state_monitor.cpp:48
robot_state.h
planning_scene_monitor::CurrentStateMonitor::~CurrentStateMonitor
~CurrentStateMonitor()
Definition: current_state_monitor.cpp:67
planning_scene_monitor::CurrentStateMonitor::nh_
ros::NodeHandle nh_
Definition: current_state_monitor.h:261
planning_scene_monitor::CurrentStateMonitor::setBoundsError
void setBoundsError(double error)
When a joint value is received to be out of bounds, it is changed slightly to fit within bounds,...
Definition: current_state_monitor.h:232
planning_scene_monitor::JointStateUpdateCallback
boost::function< void(const sensor_msgs::JointStateConstPtr &)> JointStateUpdateCallback
Definition: current_state_monitor.h:81
planning_scene_monitor::CurrentStateMonitor::startStateMonitor
void startStateMonitor(const std::string &joint_states_topic="joint_states")
Start monitoring joint states on a particular topic.
Definition: current_state_monitor.cpp:139
planning_scene_monitor::CurrentStateMonitor
Monitors the joint_states topic and tf to maintain the current state of the robot.
Definition: current_state_monitor.h:85
planning_scene_monitor::CurrentStateMonitor::enableCopyDynamics
void enableCopyDynamics(bool enabled)
Allow the joint_state arrrays velocity and effort to be copied into the robot state this is useful in...
Definition: current_state_monitor.h:249
planning_scene_monitor::CurrentStateMonitor::current_state_time_
ros::Time current_state_time_
Definition: current_state_monitor.h:271
planning_scene_monitor::CurrentStateMonitor::getCurrentStateAndTime
std::pair< moveit::core::RobotStatePtr, ros::Time > getCurrentStateAndTime() const
Get the current state and its time stamp.
Definition: current_state_monitor.cpp:85
planning_scene_monitor::CurrentStateMonitor::state_update_condition_
boost::condition_variable state_update_condition_
Definition: current_state_monitor.h:274
planning_scene_monitor::CurrentStateMonitor::getBoundsError
double getBoundsError() const
When a joint value is received to be out of bounds, it is changed slightly to fit within bounds,...
Definition: current_state_monitor.h:241
planning_scene_monitor::CurrentStateMonitor::update_callbacks_
std::vector< JointStateUpdateCallback > update_callbacks_
Definition: current_state_monitor.h:275
planning_scene_monitor::CurrentStateMonitor::jointStateCallback
void jointStateCallback(const sensor_msgs::JointStateConstPtr &joint_state)
Definition: current_state_monitor.cpp:278
planning_scene_monitor::CurrentStateMonitor::setToCurrentState
void setToCurrentState(moveit::core::RobotState &upd) const
Set the state upd to the current state maintained by this class.
Definition: current_state_monitor.cpp:103
planning_scene_monitor::CurrentStateMonitor::waitForCurrentState
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.
Definition: current_state_monitor.cpp:215
planning_scene_monitor::CurrentStateMonitor::TFConnection
boost::signals2::connection TFConnection
Definition: current_state_monitor.h:87
planning_scene_monitor::CurrentStateMonitor::error_
double error_
Definition: current_state_monitor.h:269
planning_scene_monitor::MOVEIT_CLASS_FORWARD
MOVEIT_CLASS_FORWARD(CurrentStateMonitor)
planning_scene_monitor::CurrentStateMonitor::haveCompleteStateHelper
bool haveCompleteStateHelper(const ros::Time &oldest_allowed_update_time, std::vector< std::string > *missing_joints) const
Definition: current_state_monitor.cpp:187
planning_scene_monitor::CurrentStateMonitor::tf_connection_
std::shared_ptr< TFConnection > tf_connection_
Definition: current_state_monitor.h:277
planning_scene_monitor::CurrentStateMonitor::tf_buffer_
std::shared_ptr< tf2_ros::Buffer > tf_buffer_
Definition: current_state_monitor.h:262
planning_scene_monitor::CurrentStateMonitor::joint_state_subscriber_
ros::Subscriber joint_state_subscriber_
Definition: current_state_monitor.h:270
ros::Time
planning_scene_monitor::CurrentStateMonitor::tfCallback
void tfCallback()
Definition: current_state_monitor.cpp:358
tf_buffer
tf2_ros::Buffer * tf_buffer
planning_scene_monitor::CurrentStateMonitor::waitForCompleteState
bool waitForCompleteState(double wait_time) const
Wait for at most wait_time seconds until the complete robot state is known.
Definition: current_state_monitor.cpp:238
planning_scene_monitor::CurrentStateMonitor::getCurrentState
moveit::core::RobotStatePtr getCurrentState() const
Get the current state.
Definition: current_state_monitor.cpp:72
planning_scene_monitor::CurrentStateMonitor::getCurrentStateValues
std::map< std::string, double > getCurrentStateValues() const
Get the current state values as a map from joint names to joint state values.
Definition: current_state_monitor.cpp:92
planning_scene_monitor::CurrentStateMonitor::robot_state_
moveit::core::RobotState robot_state_
Definition: current_state_monitor.h:264
planning_scene_monitor::CurrentStateMonitor::clearUpdateCallbacks
void clearUpdateCallbacks()
Clear the functions to be called when an update to the joint state is received.
Definition: current_state_monitor.cpp:134
ros::Duration
planning_scene_monitor::CurrentStateMonitor::monitor_start_time_
ros::Time monitor_start_time_
Definition: current_state_monitor.h:268
planning_scene_monitor::CurrentStateMonitor::copy_dynamics_
bool copy_dynamics_
Definition: current_state_monitor.h:267
ros::NodeHandle
ros::Subscriber
planning_scene_monitor::CurrentStateMonitor::getMonitorStartTime
const ros::Time & getMonitorStartTime() const
Get the time point when the monitor was started.
Definition: current_state_monitor.h:217
planning_scene_monitor::CurrentStateMonitor::getMonitoredTopic
std::string getMonitoredTopic() const
Get the name of the topic being monitored. Returns an empty string if the monitor is inactive.
Definition: current_state_monitor.cpp:179
ros::Time::now
static Time now()
planning_scene_monitor::CurrentStateMonitor::state_monitor_started_
bool state_monitor_started_
Definition: current_state_monitor.h:266
planning_scene_monitor::CurrentStateMonitor::addUpdateCallback
void addUpdateCallback(const JointStateUpdateCallback &fn)
Add a function that will be called whenever the joint state is updated.
Definition: current_state_monitor.cpp:128


planning
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Tue Sep 21 2021 02:23:52