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 
00363   bool waitForCurrentRobotState(const ros::Time& t, double wait_time = 1.);
00364 
00366   void lockSceneRead();
00367 
00369   void unlockSceneRead();
00370 
00373   void lockSceneWrite();
00374 
00377   void unlockSceneWrite();
00378 
00379   void clearOctomap();
00380 
00381   // Called to update the planning scene with a new message.
00382   bool newPlanningSceneMessage(const moveit_msgs::PlanningScene& scene);
00383 
00384 protected:
00388   void initialize(const planning_scene::PlanningScenePtr& scene);
00389 
00391   void configureCollisionMatrix(const planning_scene::PlanningScenePtr& scene);
00392 
00394   void configureDefaultPadding();
00395 
00397   void collisionObjectCallback(const moveit_msgs::CollisionObjectConstPtr& obj);
00398 
00400   void collisionObjectFailTFCallback(const moveit_msgs::CollisionObjectConstPtr& obj,
00401                                      tf::filter_failure_reasons::FilterFailureReason reason);
00402 
00404   void newPlanningSceneWorldCallback(const moveit_msgs::PlanningSceneWorldConstPtr& world);
00405 
00407   void octomapUpdateCallback();
00408 
00410   void attachObjectCallback(const moveit_msgs::AttachedCollisionObjectConstPtr& obj);
00411 
00413   void currentStateAttachedBodyUpdateCallback(robot_state::AttachedBody* attached_body, bool just_attached);
00414 
00416   void currentWorldObjectUpdateCallback(const collision_detection::World::ObjectConstPtr& object,
00417                                         collision_detection::World::Action action);
00418 
00419   void includeRobotLinksInOctree();
00420   void excludeRobotLinksFromOctree();
00421 
00422   void excludeWorldObjectsFromOctree();
00423   void includeWorldObjectsInOctree();
00424   void excludeWorldObjectFromOctree(const collision_detection::World::ObjectConstPtr& obj);
00425   void includeWorldObjectInOctree(const collision_detection::World::ObjectConstPtr& obj);
00426 
00427   void excludeAttachedBodiesFromOctree();
00428   void includeAttachedBodiesInOctree();
00429   void excludeAttachedBodyFromOctree(const robot_state::AttachedBody* attached_body);
00430   void includeAttachedBodyInOctree(const robot_state::AttachedBody* attached_body);
00431 
00432   bool getShapeTransformCache(const std::string& target_frame, const ros::Time& target_time,
00433                               occupancy_map_monitor::ShapeTransformCache& cache) const;
00434 
00436   std::string monitor_name_;
00437 
00438   planning_scene::PlanningScenePtr scene_;
00439   planning_scene::PlanningSceneConstPtr scene_const_;
00440   planning_scene::PlanningScenePtr parent_scene_;  
00441   boost::shared_mutex scene_update_mutex_;         
00442 
00443   ros::NodeHandle nh_;
00444   ros::NodeHandle root_nh_;
00445   boost::shared_ptr<tf::Transformer> tf_;
00446   std::string robot_description_;
00447 
00449   double default_robot_padd_;
00451   double default_robot_scale_;
00453   double default_object_padd_;
00455   double default_attached_padd_;
00457   std::map<std::string, double> default_robot_link_padd_;
00459   std::map<std::string, double> default_robot_link_scale_;
00460 
00461   // variables for planning scene publishing
00462   ros::Publisher planning_scene_publisher_;
00463   boost::scoped_ptr<boost::thread> publish_planning_scene_;
00464   double publish_planning_scene_frequency_;
00465   SceneUpdateType publish_update_types_;
00466   SceneUpdateType new_scene_update_;
00467   boost::condition_variable_any new_scene_update_condition_;
00468 
00469   // subscribe to various sources of data
00470   ros::Subscriber planning_scene_subscriber_;
00471   ros::Subscriber planning_scene_world_subscriber_;
00472 
00473   ros::Subscriber attached_collision_object_subscriber_;
00474 
00475   boost::scoped_ptr<message_filters::Subscriber<moveit_msgs::CollisionObject> > collision_object_subscriber_;
00476   boost::scoped_ptr<tf::MessageFilter<moveit_msgs::CollisionObject> > collision_object_filter_;
00477 
00478   // include a octomap monitor
00479   boost::scoped_ptr<occupancy_map_monitor::OccupancyMapMonitor> octomap_monitor_;
00480 
00481   // include a current state monitor
00482   CurrentStateMonitorPtr current_state_monitor_;
00483 
00484   typedef std::map<const robot_model::LinkModel*,
00485                    std::vector<std::pair<occupancy_map_monitor::ShapeHandle, std::size_t> > >
00486       LinkShapeHandles;
00487   typedef std::map<const robot_state::AttachedBody*,
00488                    std::vector<std::pair<occupancy_map_monitor::ShapeHandle, std::size_t> > >
00489       AttachedBodyShapeHandles;
00490   typedef std::map<std::string, std::vector<std::pair<occupancy_map_monitor::ShapeHandle, Eigen::Affine3d*> > >
00491       CollisionBodyShapeHandles;
00492 
00493   LinkShapeHandles link_shape_handles_;
00494   AttachedBodyShapeHandles attached_body_shape_handles_;
00495   CollisionBodyShapeHandles collision_body_shape_handles_;
00496   mutable boost::recursive_mutex shape_handles_lock_;
00497 
00499   boost::recursive_mutex update_lock_;
00500   std::vector<boost::function<void(SceneUpdateType)> > update_callbacks_;  
00501 
00502   ros::Time last_update_time_;                                             
00503 
00504 private:
00505   void getUpdatedFrameTransforms(std::vector<geometry_msgs::TransformStamped>& transforms);
00506 
00507   // publish planning scene update diffs (runs in its own thread)
00508   void scenePublishingThread();
00509 
00510   // called by current_state_monitor_ when robot state (as monitored on joint state topic) changes
00511   void onStateUpdate(const sensor_msgs::JointStateConstPtr& joint_state);
00512 
00513   // called by state_update_timer_ when a state update it pending
00514   void stateUpdateTimerCallback(const ros::WallTimerEvent& event);
00515 
00516   // Callback for a new planning scene msg
00517   void newPlanningSceneCallback(const moveit_msgs::PlanningSceneConstPtr& scene);
00518 
00519   // Lock for state_update_pending_ and dt_state_update_
00520   boost::mutex state_pending_mutex_;
00521 
00523   // This field is protected by state_pending_mutex_
00524   volatile bool state_update_pending_;
00525 
00527   // This field is protected by state_pending_mutex_
00528   ros::WallDuration dt_state_update_;
00529 
00531   // Setting this to a non-zero value resolves issues when the sensor data is
00532   // arriving so fast that it is preceding the transform state.
00533   ros::Duration shape_transform_cache_lookup_wait_time_;
00534 
00536   // Check if last_state_update_ is true and if so call updateSceneWithCurrentState()
00537   // Not safe to access from callback functions.
00538   ros::WallTimer state_update_timer_;
00539 
00541   // Only access this from callback functions (and constructor)
00542   ros::WallTime last_state_update_;
00543 
00544   robot_model_loader::RobotModelLoaderPtr rm_loader_;
00545   robot_model::RobotModelConstPtr robot_model_;
00546 
00547   collision_detection::CollisionPluginLoader collision_loader_;
00548 
00549   class DynamicReconfigureImpl;
00550   DynamicReconfigureImpl* reconfigure_impl_;
00551 };
00552 
00574 class LockedPlanningSceneRO
00575 {
00576 public:
00577   LockedPlanningSceneRO(const PlanningSceneMonitorPtr& planning_scene_monitor)
00578     : planning_scene_monitor_(planning_scene_monitor)
00579   {
00580     initialize(true);
00581   }
00582 
00583   const PlanningSceneMonitorPtr& getPlanningSceneMonitor()
00584   {
00585     return planning_scene_monitor_;
00586   }
00587 
00588   operator bool() const
00589   {
00590     return planning_scene_monitor_ && planning_scene_monitor_->getPlanningScene();
00591   }
00592 
00593   operator const planning_scene::PlanningSceneConstPtr&() const
00594   {
00595     return static_cast<const PlanningSceneMonitor*>(planning_scene_monitor_.get())->getPlanningScene();
00596   }
00597 
00598   const planning_scene::PlanningSceneConstPtr& operator->() const
00599   {
00600     return static_cast<const PlanningSceneMonitor*>(planning_scene_monitor_.get())->getPlanningScene();
00601   }
00602 
00603 protected:
00604   LockedPlanningSceneRO(const PlanningSceneMonitorPtr& planning_scene_monitor, bool read_only)
00605     : planning_scene_monitor_(planning_scene_monitor)
00606   {
00607     initialize(read_only);
00608   }
00609 
00610   void initialize(bool read_only)
00611   {
00612     if (planning_scene_monitor_)
00613       lock_.reset(new SingleUnlock(planning_scene_monitor_.get(), read_only));
00614   }
00615 
00616   MOVEIT_CLASS_FORWARD(SingleUnlock);
00617 
00618   // we use this struct so that lock/unlock are called only once
00619   // even if the LockedPlanningScene instance is copied around
00620   struct SingleUnlock
00621   {
00622     SingleUnlock(PlanningSceneMonitor* planning_scene_monitor, bool read_only)
00623       : planning_scene_monitor_(planning_scene_monitor), read_only_(read_only)
00624     {
00625       if (read_only)
00626         planning_scene_monitor_->lockSceneRead();
00627       else
00628         planning_scene_monitor_->lockSceneWrite();
00629     }
00630     ~SingleUnlock()
00631     {
00632       if (read_only_)
00633         planning_scene_monitor_->unlockSceneRead();
00634       else
00635         planning_scene_monitor_->unlockSceneWrite();
00636     }
00637     PlanningSceneMonitor* planning_scene_monitor_;
00638     bool read_only_;
00639   };
00640 
00641   PlanningSceneMonitorPtr planning_scene_monitor_;
00642   SingleUnlockPtr lock_;
00643 };
00644 
00666 class LockedPlanningSceneRW : public LockedPlanningSceneRO
00667 {
00668 public:
00669   LockedPlanningSceneRW(const PlanningSceneMonitorPtr& planning_scene_monitor)
00670     : LockedPlanningSceneRO(planning_scene_monitor, false)
00671   {
00672   }
00673 
00674   operator const planning_scene::PlanningScenePtr&()
00675   {
00676     return planning_scene_monitor_->getPlanningScene();
00677   }
00678 
00679   const planning_scene::PlanningScenePtr& operator->()
00680   {
00681     return planning_scene_monitor_->getPlanningScene();
00682   }
00683 };
00684 }
00685 
00686 #endif


planning
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Fri Apr 20 2018 03:31:39