planning_scene_monitor.h
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2011, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /* Author: Ioan Sucan */
00036 
00037 #ifndef MOVEIT_PLANNING_SCENE_MONITOR_PLANNING_SCENE_MONITOR_
00038 #define MOVEIT_PLANNING_SCENE_MONITOR_PLANNING_SCENE_MONITOR_
00039 
00040 #include <ros/ros.h>
00041 #include <tf/tf.h>
00042 #include <tf/message_filter.h>
00043 #include <message_filters/subscriber.h>
00044 #include <moveit/macros/class_forward.h>
00045 #include <moveit/planning_scene/planning_scene.h>
00046 #include <moveit/robot_model_loader/robot_model_loader.h>
00047 #include <moveit/occupancy_map_monitor/occupancy_map_monitor.h>
00048 #include <moveit/planning_scene_monitor/current_state_monitor.h>
00049 #include <moveit/collision_plugin_loader/collision_plugin_loader.h>
00050 #include <boost/noncopyable.hpp>
00051 #include <boost/thread/shared_mutex.hpp>
00052 #include <boost/thread/recursive_mutex.hpp>
00053 
00054 namespace planning_scene_monitor
00055 {
00056 MOVEIT_CLASS_FORWARD(PlanningSceneMonitor);
00057 
00061 class PlanningSceneMonitor : private boost::noncopyable
00062 {
00063 public:
00064   enum SceneUpdateType
00065   {
00067     UPDATE_NONE = 0,
00068 
00070     UPDATE_STATE = 1,
00071 
00073     UPDATE_TRANSFORMS = 2,
00074 
00077     UPDATE_GEOMETRY = 4,
00078 
00080     UPDATE_SCENE = 8 + UPDATE_STATE + UPDATE_TRANSFORMS + UPDATE_GEOMETRY
00081   };
00082 
00084   static const std::string DEFAULT_JOINT_STATES_TOPIC;  // "/joint_states"
00085 
00087   static const std::string DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC;  // "/attached_collision_object"
00088 
00090   static const std::string DEFAULT_COLLISION_OBJECT_TOPIC;  // "/collision_object"
00091 
00094   static const std::string DEFAULT_PLANNING_SCENE_WORLD_TOPIC;  // "/planning_scene_world"
00095 
00097   static const std::string DEFAULT_PLANNING_SCENE_TOPIC;  // "/planning_scene"
00098 
00100   static const std::string DEFAULT_PLANNING_SCENE_SERVICE;  // "/get_planning_scene"
00101 
00104   static const std::string MONITORED_PLANNING_SCENE_TOPIC;  // "monitored_planning_scene"
00105 
00111   PlanningSceneMonitor(const std::string& robot_description,
00112                        const boost::shared_ptr<tf::Transformer>& tf = boost::shared_ptr<tf::Transformer>(),
00113                        const std::string& name = "");
00114 
00120   PlanningSceneMonitor(const robot_model_loader::RobotModelLoaderPtr& rml,
00121                        const boost::shared_ptr<tf::Transformer>& tf = boost::shared_ptr<tf::Transformer>(),
00122                        const std::string& name = "");
00123 
00130   PlanningSceneMonitor(const planning_scene::PlanningScenePtr& scene, const std::string& robot_description,
00131                        const boost::shared_ptr<tf::Transformer>& tf = boost::shared_ptr<tf::Transformer>(),
00132                        const std::string& name = "");
00133 
00140   PlanningSceneMonitor(const planning_scene::PlanningScenePtr& scene,
00141                        const robot_model_loader::RobotModelLoaderPtr& rml,
00142                        const boost::shared_ptr<tf::Transformer>& tf = boost::shared_ptr<tf::Transformer>(),
00143                        const std::string& name = "");
00144 
00145   ~PlanningSceneMonitor();
00146 
00148   const std::string& getName() const
00149   {
00150     return monitor_name_;
00151   }
00152 
00154   const robot_model_loader::RobotModelLoaderPtr& getRobotModelLoader() const
00155   {
00156     return rm_loader_;
00157   }
00158 
00159   const robot_model::RobotModelConstPtr& getRobotModel() const
00160   {
00161     return robot_model_;
00162   }
00163 
00178   const planning_scene::PlanningScenePtr& getPlanningScene()
00179   {
00180     return scene_;
00181   }
00182 
00186   const planning_scene::PlanningSceneConstPtr& getPlanningScene() const
00187   {
00188     return scene_const_;
00189   }
00190 
00195   bool updatesScene(const planning_scene::PlanningSceneConstPtr& scene) const;
00196 
00201   bool updatesScene(const planning_scene::PlanningScenePtr& scene) const;
00202 
00205   const std::string& getRobotDescription() const
00206   {
00207     return robot_description_;
00208   }
00209 
00211   double getDefaultRobotPadding() const
00212   {
00213     return default_robot_padd_;
00214   }
00215 
00217   double getDefaultRobotScale() const
00218   {
00219     return default_robot_scale_;
00220   }
00221 
00223   double getDefaultObjectPadding() const
00224   {
00225     return default_object_padd_;
00226   }
00227 
00229   double getDefaultAttachedObjectPadding() const
00230   {
00231     return default_attached_padd_;
00232   }
00233 
00235   const boost::shared_ptr<tf::Transformer>& getTFClient() const
00236   {
00237     return tf_;
00238   }
00239 
00245   void monitorDiffs(bool flag);
00246 
00250   void startPublishingPlanningScene(SceneUpdateType event,
00251                                     const std::string& planning_scene_topic = MONITORED_PLANNING_SCENE_TOPIC);
00252 
00254   void stopPublishingPlanningScene();
00255 
00257   void setPlanningScenePublishingFrequency(double hz);
00258 
00260   double getPlanningScenePublishingFrequency() const
00261   {
00262     return publish_planning_scene_frequency_;
00263   }
00264 
00267   const CurrentStateMonitorPtr& getStateMonitor() const
00268   {
00269     return current_state_monitor_;
00270   }
00271 
00277   void updateFrameTransforms();
00278 
00282   void startStateMonitor(const std::string& joint_states_topic = DEFAULT_JOINT_STATES_TOPIC,
00283                          const std::string& attached_objects_topic = DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC);
00284 
00286   void stopStateMonitor();
00287 
00291   void updateSceneWithCurrentState();
00292 
00298   void setStateUpdateFrequency(double hz);
00299 
00301   double getStateUpdateFrequency() const
00302   {
00303     if (!dt_state_update_.isZero())
00304       return 1.0 / dt_state_update_.toSec();
00305     else
00306       return 0.0;
00307   }
00308 
00312   void startSceneMonitor(const std::string& scene_topic = DEFAULT_PLANNING_SCENE_TOPIC);
00313 
00320   bool requestPlanningSceneState(const std::string& service_name = DEFAULT_PLANNING_SCENE_SERVICE);
00321 
00323   void stopSceneMonitor();
00324 
00331   void startWorldGeometryMonitor(const std::string& collision_objects_topic = DEFAULT_COLLISION_OBJECT_TOPIC,
00332                                  const std::string& planning_scene_world_topic = DEFAULT_PLANNING_SCENE_WORLD_TOPIC,
00333                                  const bool load_octomap_monitor = true);
00334 
00336   void stopWorldGeometryMonitor();
00337 
00339   void addUpdateCallback(const boost::function<void(SceneUpdateType)>& fn);
00340 
00342   void clearUpdateCallbacks();
00343 
00345   void getMonitoredTopics(std::vector<std::string>& topics) const;
00346 
00348   const ros::Time& getLastUpdateTime() const
00349   {
00350     return last_update_time_;
00351   }
00352 
00353   void publishDebugInformation(bool flag);
00354 
00356   void triggerSceneUpdateEvent(SceneUpdateType update_type);
00357 
00359   void lockSceneRead();
00360 
00362   void unlockSceneRead();
00363 
00366   void lockSceneWrite();
00367 
00370   void unlockSceneWrite();
00371 
00372   void clearOctomap();
00373 
00374   // Called to update the planning scene with a new message.
00375   bool newPlanningSceneMessage(const moveit_msgs::PlanningScene& scene);
00376 
00377 protected:
00381   void initialize(const planning_scene::PlanningScenePtr& scene);
00382 
00384   void configureCollisionMatrix(const planning_scene::PlanningScenePtr& scene);
00385 
00387   void configureDefaultPadding();
00388 
00390   void collisionObjectCallback(const moveit_msgs::CollisionObjectConstPtr& obj);
00391 
00393   void collisionObjectFailTFCallback(const moveit_msgs::CollisionObjectConstPtr& obj,
00394                                      tf::filter_failure_reasons::FilterFailureReason reason);
00395 
00397   void newPlanningSceneWorldCallback(const moveit_msgs::PlanningSceneWorldConstPtr& world);
00398 
00400   void octomapUpdateCallback();
00401 
00403   void attachObjectCallback(const moveit_msgs::AttachedCollisionObjectConstPtr& obj);
00404 
00406   void currentStateAttachedBodyUpdateCallback(robot_state::AttachedBody* attached_body, bool just_attached);
00407 
00409   void currentWorldObjectUpdateCallback(const collision_detection::World::ObjectConstPtr& object,
00410                                         collision_detection::World::Action action);
00411 
00412   void includeRobotLinksInOctree();
00413   void excludeRobotLinksFromOctree();
00414 
00415   void excludeWorldObjectsFromOctree();
00416   void includeWorldObjectsInOctree();
00417   void excludeWorldObjectFromOctree(const collision_detection::World::ObjectConstPtr& obj);
00418   void includeWorldObjectInOctree(const collision_detection::World::ObjectConstPtr& obj);
00419 
00420   void excludeAttachedBodiesFromOctree();
00421   void includeAttachedBodiesInOctree();
00422   void excludeAttachedBodyFromOctree(const robot_state::AttachedBody* attached_body);
00423   void includeAttachedBodyInOctree(const robot_state::AttachedBody* attached_body);
00424 
00425   bool getShapeTransformCache(const std::string& target_frame, const ros::Time& target_time,
00426                               occupancy_map_monitor::ShapeTransformCache& cache) const;
00427 
00429   std::string monitor_name_;
00430 
00431   planning_scene::PlanningScenePtr scene_;
00432   planning_scene::PlanningSceneConstPtr scene_const_;
00433   planning_scene::PlanningScenePtr parent_scene_;  
00434   boost::shared_mutex scene_update_mutex_;         
00435 
00436   ros::NodeHandle nh_;
00437   ros::NodeHandle root_nh_;
00438   boost::shared_ptr<tf::Transformer> tf_;
00439   std::string robot_description_;
00440 
00442   double default_robot_padd_;
00444   double default_robot_scale_;
00446   double default_object_padd_;
00448   double default_attached_padd_;
00450   std::map<std::string, double> default_robot_link_padd_;
00452   std::map<std::string, double> default_robot_link_scale_;
00453 
00454   // variables for planning scene publishing
00455   ros::Publisher planning_scene_publisher_;
00456   boost::scoped_ptr<boost::thread> publish_planning_scene_;
00457   double publish_planning_scene_frequency_;
00458   SceneUpdateType publish_update_types_;
00459   SceneUpdateType new_scene_update_;
00460   boost::condition_variable_any new_scene_update_condition_;
00461 
00462   // subscribe to various sources of data
00463   ros::Subscriber planning_scene_subscriber_;
00464   ros::Subscriber planning_scene_world_subscriber_;
00465 
00466   ros::Subscriber attached_collision_object_subscriber_;
00467 
00468   boost::scoped_ptr<message_filters::Subscriber<moveit_msgs::CollisionObject> > collision_object_subscriber_;
00469   boost::scoped_ptr<tf::MessageFilter<moveit_msgs::CollisionObject> > collision_object_filter_;
00470 
00471   // include a octomap monitor
00472   boost::scoped_ptr<occupancy_map_monitor::OccupancyMapMonitor> octomap_monitor_;
00473 
00474   // include a current state monitor
00475   CurrentStateMonitorPtr current_state_monitor_;
00476 
00477   typedef std::map<const robot_model::LinkModel*,
00478                    std::vector<std::pair<occupancy_map_monitor::ShapeHandle, std::size_t> > >
00479       LinkShapeHandles;
00480   typedef std::map<const robot_state::AttachedBody*,
00481                    std::vector<std::pair<occupancy_map_monitor::ShapeHandle, std::size_t> > >
00482       AttachedBodyShapeHandles;
00483   typedef std::map<std::string, std::vector<std::pair<occupancy_map_monitor::ShapeHandle, Eigen::Affine3d*> > >
00484       CollisionBodyShapeHandles;
00485 
00486   LinkShapeHandles link_shape_handles_;
00487   AttachedBodyShapeHandles attached_body_shape_handles_;
00488   CollisionBodyShapeHandles collision_body_shape_handles_;
00489   mutable boost::recursive_mutex shape_handles_lock_;
00490 
00492   boost::recursive_mutex update_lock_;
00493   std::vector<boost::function<void(SceneUpdateType)> > update_callbacks_;  
00494 
00495   ros::Time last_update_time_;                                             
00496 
00497 private:
00498   void getUpdatedFrameTransforms(std::vector<geometry_msgs::TransformStamped>& transforms);
00499 
00500   // publish planning scene update diffs (runs in its own thread)
00501   void scenePublishingThread();
00502 
00503   // called by current_state_monitor_ when robot state (as monitored on joint state topic) changes
00504   void onStateUpdate(const sensor_msgs::JointStateConstPtr& joint_state);
00505 
00506   // called by state_update_timer_ when a state update it pending
00507   void stateUpdateTimerCallback(const ros::WallTimerEvent& event);
00508 
00509   // Callback for a new planning scene msg
00510   void newPlanningSceneCallback(const moveit_msgs::PlanningSceneConstPtr& scene);
00511 
00512   // Lock for state_update_pending_ and dt_state_update_
00513   boost::mutex state_pending_mutex_;
00514 
00516   // This field is protected by state_pending_mutex_
00517   volatile bool state_update_pending_;
00518 
00520   // This field is protected by state_pending_mutex_
00521   ros::WallDuration dt_state_update_;
00522 
00524   // Setting this to a non-zero value resolves issues when the sensor data is
00525   // arriving so fast that it is preceding the transform state.
00526   ros::Duration shape_transform_cache_lookup_wait_time_;
00527 
00529   // Check if last_state_update_ is true and if so call updateSceneWithCurrentState()
00530   // Not safe to access from callback functions.
00531   ros::WallTimer state_update_timer_;
00532 
00534   // Only access this from callback functions (and constructor)
00535   ros::WallTime last_state_update_;
00536 
00537   robot_model_loader::RobotModelLoaderPtr rm_loader_;
00538   robot_model::RobotModelConstPtr robot_model_;
00539 
00540   collision_detection::CollisionPluginLoader collision_loader_;
00541 
00542   class DynamicReconfigureImpl;
00543   DynamicReconfigureImpl* reconfigure_impl_;
00544 };
00545 
00567 class LockedPlanningSceneRO
00568 {
00569 public:
00570   LockedPlanningSceneRO(const PlanningSceneMonitorPtr& planning_scene_monitor)
00571     : planning_scene_monitor_(planning_scene_monitor)
00572   {
00573     initialize(true);
00574   }
00575 
00576   const PlanningSceneMonitorPtr& getPlanningSceneMonitor()
00577   {
00578     return planning_scene_monitor_;
00579   }
00580 
00581   operator bool() const
00582   {
00583     return planning_scene_monitor_ && planning_scene_monitor_->getPlanningScene();
00584   }
00585 
00586   operator const planning_scene::PlanningSceneConstPtr&() const
00587   {
00588     return static_cast<const PlanningSceneMonitor*>(planning_scene_monitor_.get())->getPlanningScene();
00589   }
00590 
00591   const planning_scene::PlanningSceneConstPtr& operator->() const
00592   {
00593     return static_cast<const PlanningSceneMonitor*>(planning_scene_monitor_.get())->getPlanningScene();
00594   }
00595 
00596 protected:
00597   LockedPlanningSceneRO(const PlanningSceneMonitorPtr& planning_scene_monitor, bool read_only)
00598     : planning_scene_monitor_(planning_scene_monitor)
00599   {
00600     initialize(read_only);
00601   }
00602 
00603   void initialize(bool read_only)
00604   {
00605     if (planning_scene_monitor_)
00606       lock_.reset(new SingleUnlock(planning_scene_monitor_.get(), read_only));
00607   }
00608 
00609   MOVEIT_CLASS_FORWARD(SingleUnlock);
00610 
00611   // we use this struct so that lock/unlock are called only once
00612   // even if the LockedPlanningScene instance is copied around
00613   struct SingleUnlock
00614   {
00615     SingleUnlock(PlanningSceneMonitor* planning_scene_monitor, bool read_only)
00616       : planning_scene_monitor_(planning_scene_monitor), read_only_(read_only)
00617     {
00618       if (read_only)
00619         planning_scene_monitor_->lockSceneRead();
00620       else
00621         planning_scene_monitor_->lockSceneWrite();
00622     }
00623     ~SingleUnlock()
00624     {
00625       if (read_only_)
00626         planning_scene_monitor_->unlockSceneRead();
00627       else
00628         planning_scene_monitor_->unlockSceneWrite();
00629     }
00630     PlanningSceneMonitor* planning_scene_monitor_;
00631     bool read_only_;
00632   };
00633 
00634   PlanningSceneMonitorPtr planning_scene_monitor_;
00635   SingleUnlockPtr lock_;
00636 };
00637 
00659 class LockedPlanningSceneRW : public LockedPlanningSceneRO
00660 {
00661 public:
00662   LockedPlanningSceneRW(const PlanningSceneMonitorPtr& planning_scene_monitor)
00663     : LockedPlanningSceneRO(planning_scene_monitor, false)
00664   {
00665   }
00666 
00667   operator const planning_scene::PlanningScenePtr&()
00668   {
00669     return planning_scene_monitor_->getPlanningScene();
00670   }
00671 
00672   const planning_scene::PlanningScenePtr& operator->()
00673   {
00674     return planning_scene_monitor_->getPlanningScene();
00675   }
00676 };
00677 }
00678 
00679 #endif


planning
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Wed Jan 17 2018 03:32:07