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 
53 {
54  using TFConnection = boost::signals2::connection;
55 
56 public:
62  CurrentStateMonitor(const moveit::core::RobotModelConstPtr& robot_model,
63  const std::shared_ptr<tf2_ros::Buffer>& tf_buffer);
64 
70  CurrentStateMonitor(const moveit::core::RobotModelConstPtr& robot_model,
71  const std::shared_ptr<tf2_ros::Buffer>& tf_buffer, const ros::NodeHandle& nh);
72 
74 
78  void startStateMonitor(const std::string& joint_states_topic = "joint_states");
79 
82  void stopStateMonitor();
83 
85  bool isActive() const;
86 
88  const moveit::core::RobotModelConstPtr& getRobotModel() const
89  {
90  return robot_model_;
91  }
92 
94  std::string getMonitoredTopic() const;
95 
99  inline bool haveCompleteState() const
100  {
101  return haveCompleteStateHelper(ros::Time(0), nullptr);
102  }
103 
108  inline bool haveCompleteState(const ros::Time& oldest_allowed_update_time) const
109  {
110  return haveCompleteStateHelper(oldest_allowed_update_time, nullptr);
111  }
112 
118  inline bool haveCompleteState(const ros::Duration& age) const
119  {
120  return haveCompleteStateHelper(ros::Time::now() - age, nullptr);
121  }
122 
127  inline bool haveCompleteState(std::vector<std::string>& missing_joints) const
128  {
129  return haveCompleteStateHelper(ros::Time(0), &missing_joints);
130  }
131 
137  inline bool haveCompleteState(const ros::Time& oldest_allowed_update_time,
138  std::vector<std::string>& missing_joints) const
139  {
140  return haveCompleteStateHelper(oldest_allowed_update_time, &missing_joints);
141  }
142 
147  inline bool haveCompleteState(const ros::Duration& age, std::vector<std::string>& missing_joints) const
148  {
149  return haveCompleteStateHelper(ros::Time::now() - age, &missing_joints);
150  }
151 
154  moveit::core::RobotStatePtr getCurrentState() const;
155 
158 
161 
164  std::pair<moveit::core::RobotStatePtr, ros::Time> getCurrentStateAndTime() const;
165 
168  std::map<std::string, double> getCurrentStateValues() const;
169 
173  bool waitForCurrentState(const ros::Time t = ros::Time::now(), double wait_time = 1.0) const;
174 
177  bool waitForCompleteState(double wait_time) const;
178 
181  bool waitForCompleteState(const std::string& group, double wait_time) const;
182 
184  const ros::Time& getMonitorStartTime() const
185  {
186  return monitor_start_time_;
187  }
188 
191 
193  void clearUpdateCallbacks();
194 
199  void setBoundsError(double error)
200  {
201  error_ = (error > 0) ? error : -error;
202  }
203 
208  double getBoundsError() const
209  {
210  return error_;
211  }
212 
216  void enableCopyDynamics(bool enabled)
217  {
218  copy_dynamics_ = enabled;
219  }
220 
221 private:
222  bool haveCompleteStateHelper(const ros::Time& oldest_allowed_update_time,
223  std::vector<std::string>* missing_joints) const;
224 
225  void jointStateCallback(const sensor_msgs::JointStateConstPtr& joint_state);
226  void tfCallback();
227 
229  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
230  moveit::core::RobotModelConstPtr robot_model_;
232  std::map<const moveit::core::JointModel*, ros::Time> joint_time_;
234  bool copy_dynamics_; // Copy velocity and effort from joint_state
236  double error_;
239 
240  mutable boost::mutex state_update_lock_;
241  mutable boost::condition_variable state_update_condition_;
242  std::vector<JointStateUpdateCallback> update_callbacks_;
243 
244  std::shared_ptr<TFConnection> tf_connection_;
245 };
246 
247 MOVEIT_CLASS_FORWARD(CurrentStateMonitor); // Defines CurrentStateMonitorPtr, ConstPtr, WeakPtr... etc
248 } // 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:131
planning_scene_monitor::CurrentStateMonitor::robot_model_
moveit::core::RobotModelConstPtr robot_model_
Definition: current_state_monitor.h:262
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:264
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:120
ros.h
planning_scene_monitor::CurrentStateMonitor::state_update_lock_
boost::mutex state_update_lock_
Definition: current_state_monitor.h:272
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:260
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:231
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
Definition: current_state_monitor.h:84
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:248
planning_scene_monitor::CurrentStateMonitor::current_state_time_
ros::Time current_state_time_
Definition: current_state_monitor.h:270
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:273
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:240
planning_scene_monitor::CurrentStateMonitor::update_callbacks_
std::vector< JointStateUpdateCallback > update_callbacks_
Definition: current_state_monitor.h:274
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:86
planning_scene_monitor::CurrentStateMonitor::error_
double error_
Definition: current_state_monitor.h:268
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:276
planning_scene_monitor::CurrentStateMonitor::tf_buffer_
std::shared_ptr< tf2_ros::Buffer > tf_buffer_
Definition: current_state_monitor.h:261
planning_scene_monitor::CurrentStateMonitor::joint_state_subscriber_
ros::Subscriber joint_state_subscriber_
Definition: current_state_monitor.h:269
ros::Time
planning_scene_monitor::CurrentStateMonitor::tfCallback
void tfCallback()
Definition: current_state_monitor.cpp:370
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:263
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:267
planning_scene_monitor::CurrentStateMonitor::copy_dynamics_
bool copy_dynamics_
Definition: current_state_monitor.h:266
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:216
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:265
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 Sun Mar 3 2024 03:24:15