Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
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/planning_scene/planning_scene.h>
00045 #include <moveit/robot_model_loader/robot_model_loader.h>
00046 #include <moveit/occupancy_map_monitor/occupancy_map_monitor.h>
00047 #include <moveit/planning_scene_monitor/current_state_monitor.h>
00048 #include <boost/noncopyable.hpp>
00049 #include <boost/thread/shared_mutex.hpp>
00050 #include <boost/thread/recursive_mutex.hpp>
00051
00052 namespace planning_scene_monitor
00053 {
00054
00058 class PlanningSceneMonitor : private boost::noncopyable
00059 {
00060 public:
00061
00062 enum SceneUpdateType
00063 {
00065 UPDATE_NONE = 0,
00066
00068 UPDATE_STATE = 1,
00069
00071 UPDATE_TRANSFORMS = 2,
00072
00074 UPDATE_GEOMETRY = 4,
00075
00077 UPDATE_SCENE = 8 + UPDATE_STATE + UPDATE_TRANSFORMS + UPDATE_GEOMETRY
00078 };
00079
00081 static const std::string DEFAULT_JOINT_STATES_TOPIC;
00082
00084 static const std::string DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC;
00085
00087 static const std::string DEFAULT_COLLISION_OBJECT_TOPIC;
00088
00090 static const std::string DEFAULT_PLANNING_SCENE_WORLD_TOPIC;
00091
00093 static const std::string DEFAULT_PLANNING_SCENE_TOPIC;
00094
00096 static const std::string DEFAULT_PLANNING_SCENE_SERVICE;
00097
00099 static const std::string MONITORED_PLANNING_SCENE_TOPIC;
00100
00106 PlanningSceneMonitor(const std::string &robot_description,
00107 const boost::shared_ptr<tf::Transformer> &tf = boost::shared_ptr<tf::Transformer>(),
00108 const std::string &name = "");
00109
00115 PlanningSceneMonitor(const robot_model_loader::RobotModelLoaderPtr &rml,
00116 const boost::shared_ptr<tf::Transformer> &tf = boost::shared_ptr<tf::Transformer>(),
00117 const std::string &name = "");
00118
00125 PlanningSceneMonitor(const planning_scene::PlanningScenePtr &scene, const std::string &robot_description,
00126 const boost::shared_ptr<tf::Transformer> &tf = boost::shared_ptr<tf::Transformer>(),
00127 const std::string &name = "");
00128
00135 PlanningSceneMonitor(const planning_scene::PlanningScenePtr &scene,
00136 const robot_model_loader::RobotModelLoaderPtr &rml,
00137 const boost::shared_ptr<tf::Transformer> &tf = boost::shared_ptr<tf::Transformer>(),
00138 const std::string &name = "");
00139
00140 ~PlanningSceneMonitor();
00141
00143 const std::string& getName() const
00144 {
00145 return monitor_name_;
00146 }
00147
00149 const robot_model_loader::RobotModelLoaderPtr& getRobotModelLoader() const
00150 {
00151 return rm_loader_;
00152 }
00153
00154 const robot_model::RobotModelConstPtr& getRobotModel() const
00155 {
00156 return robot_model_;
00157 }
00158
00173 const planning_scene::PlanningScenePtr& getPlanningScene()
00174 {
00175 return scene_;
00176 }
00177
00181 const planning_scene::PlanningSceneConstPtr& getPlanningScene() const
00182 {
00183 return scene_const_;
00184 }
00185
00190 bool updatesScene(const planning_scene::PlanningSceneConstPtr &scene) const;
00191
00196 bool updatesScene(const planning_scene::PlanningScenePtr &scene) const;
00197
00200 const std::string& getRobotDescription() const
00201 {
00202 return robot_description_;
00203 }
00204
00206 double getDefaultRobotPadding() const
00207 {
00208 return default_robot_padd_;
00209 }
00210
00212 double getDefaultRobotScale() const
00213 {
00214 return default_robot_scale_;
00215 }
00216
00218 double getDefaultObjectPadding() const
00219 {
00220 return default_object_padd_;
00221 }
00222
00224 double getDefaultAttachedObjectPadding() const
00225 {
00226 return default_attached_padd_;
00227 }
00228
00230 const boost::shared_ptr<tf::Transformer>& getTFClient() const
00231 {
00232 return tf_;
00233 }
00234
00239 void monitorDiffs(bool flag);
00240
00243 void startPublishingPlanningScene(SceneUpdateType event, const std::string &planning_scene_topic = MONITORED_PLANNING_SCENE_TOPIC);
00244
00246 void stopPublishingPlanningScene();
00247
00249 void setPlanningScenePublishingFrequency(double hz);
00250
00252 double getPlanningScenePublishingFrequency() const
00253 {
00254 return publish_planning_scene_frequency_;
00255 }
00256
00259 const CurrentStateMonitorPtr& getStateMonitor() const
00260 {
00261 return current_state_monitor_;
00262 }
00263
00268 void updateFrameTransforms();
00269
00273 void startStateMonitor(const std::string &joint_states_topic = DEFAULT_JOINT_STATES_TOPIC, const std::string &attached_objects_topic = DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC);
00274
00276 void stopStateMonitor();
00277
00280 void updateSceneWithCurrentState();
00281
00285 void setStateUpdateFrequency(double hz);
00286
00288 double getStateUpdateFrequency();
00289
00293 void startSceneMonitor(const std::string &scene_topic = DEFAULT_PLANNING_SCENE_TOPIC);
00294
00301 bool requestPlanningSceneState(const std::string &service_name = DEFAULT_PLANNING_SCENE_SERVICE);
00302
00304 void stopSceneMonitor();
00305
00311 void startWorldGeometryMonitor(const std::string &collision_objects_topic = DEFAULT_COLLISION_OBJECT_TOPIC,
00312 const std::string &planning_scene_world_topic = DEFAULT_PLANNING_SCENE_WORLD_TOPIC,
00313 const bool load_octomap_monitor = true);
00314
00316 void stopWorldGeometryMonitor();
00317
00319 void addUpdateCallback(const boost::function<void(SceneUpdateType)> &fn);
00320
00322 void clearUpdateCallbacks();
00323
00325 void getMonitoredTopics(std::vector<std::string> &topics) const;
00326
00328 const ros::Time& getLastUpdateTime() const
00329 {
00330 return last_update_time_;
00331 }
00332
00333 void publishDebugInformation(bool flag);
00334
00336 void triggerSceneUpdateEvent(SceneUpdateType update_type);
00337
00339 void lockSceneRead();
00340
00342 void unlockSceneRead();
00343
00345 void lockSceneWrite();
00346
00348 void unlockSceneWrite();
00349
00350 protected:
00351
00354 void initialize(const planning_scene::PlanningScenePtr &scene);
00355
00357 void configureCollisionMatrix(const planning_scene::PlanningScenePtr &scene);
00358
00360 void configureDefaultPadding();
00361
00363 void collisionObjectCallback(const moveit_msgs::CollisionObjectConstPtr &obj);
00364
00366 void collisionObjectFailTFCallback(const moveit_msgs::CollisionObjectConstPtr &obj, tf::filter_failure_reasons::FilterFailureReason reason);
00367
00369 void newPlanningSceneWorldCallback(const moveit_msgs::PlanningSceneWorldConstPtr &world);
00370
00372 void octomapUpdateCallback();
00373
00375 void attachObjectCallback(const moveit_msgs::AttachedCollisionObjectConstPtr &obj);
00376
00378 void currentStateAttachedBodyUpdateCallback(robot_state::AttachedBody *attached_body, bool just_attached);
00379
00381 void currentWorldObjectUpdateCallback(const collision_detection::World::ObjectConstPtr &object, collision_detection::World::Action action);
00382
00383 void includeRobotLinksInOctree();
00384 void excludeRobotLinksFromOctree();
00385
00386 void excludeWorldObjectsFromOctree();
00387 void includeWorldObjectsInOctree();
00388 void excludeWorldObjectFromOctree(const collision_detection::World::ObjectConstPtr &obj);
00389 void includeWorldObjectInOctree(const collision_detection::World::ObjectConstPtr &obj);
00390
00391 void excludeAttachedBodiesFromOctree();
00392 void includeAttachedBodiesInOctree();
00393 void excludeAttachedBodyFromOctree(const robot_state::AttachedBody *attached_body);
00394 void includeAttachedBodyInOctree(const robot_state::AttachedBody *attached_body);
00395
00396 bool getShapeTransformCache(const std::string &target_frame, const ros::Time &target_time, occupancy_map_monitor::ShapeTransformCache &cache) const;
00397
00399 std::string monitor_name_;
00400
00401 planning_scene::PlanningScenePtr scene_;
00402 planning_scene::PlanningSceneConstPtr scene_const_;
00403 planning_scene::PlanningScenePtr parent_scene_;
00404 boost::shared_mutex scene_update_mutex_;
00405
00406 ros::NodeHandle nh_;
00407 ros::NodeHandle root_nh_;
00408 boost::shared_ptr<tf::Transformer> tf_;
00409 std::string robot_description_;
00410
00412 double default_robot_padd_;
00414 double default_robot_scale_;
00416 double default_object_padd_;
00418 double default_attached_padd_;
00420 std::map<std::string, double> default_robot_link_padd_;
00422 std::map<std::string, double> default_robot_link_scale_;
00423
00424
00425 ros::Publisher planning_scene_publisher_;
00426 boost::scoped_ptr<boost::thread> publish_planning_scene_;
00427 double publish_planning_scene_frequency_;
00428 SceneUpdateType publish_update_types_;
00429 SceneUpdateType new_scene_update_;
00430 boost::condition_variable_any new_scene_update_condition_;
00431
00432
00433 ros::Subscriber planning_scene_subscriber_;
00434 ros::Subscriber planning_scene_world_subscriber_;
00435
00436 ros::Subscriber attached_collision_object_subscriber_;
00437
00438 boost::scoped_ptr<message_filters::Subscriber<moveit_msgs::CollisionObject> > collision_object_subscriber_;
00439 boost::scoped_ptr<tf::MessageFilter<moveit_msgs::CollisionObject> > collision_object_filter_;
00440
00441
00442 boost::scoped_ptr<occupancy_map_monitor::OccupancyMapMonitor> octomap_monitor_;
00443
00444
00445 CurrentStateMonitorPtr current_state_monitor_;
00446
00447
00448 typedef std::map<const robot_model::LinkModel*, std::vector<std::pair<occupancy_map_monitor::ShapeHandle, std::size_t> > > LinkShapeHandles;
00449 typedef std::map<const robot_state::AttachedBody*, std::vector<std::pair<occupancy_map_monitor::ShapeHandle, std::size_t> > > AttachedBodyShapeHandles;
00450 typedef std::map<std::string, std::vector<std::pair<occupancy_map_monitor::ShapeHandle, Eigen::Affine3d*> > > CollisionBodyShapeHandles;
00451
00452 LinkShapeHandles link_shape_handles_;
00453 AttachedBodyShapeHandles attached_body_shape_handles_;
00454 CollisionBodyShapeHandles collision_body_shape_handles_;
00455 mutable boost::recursive_mutex shape_handles_lock_;
00456
00458 boost::recursive_mutex update_lock_;
00459 std::vector<boost::function<void(SceneUpdateType)> > update_callbacks_;
00460 ros::Time last_update_time_;
00461
00462 private:
00463
00464 void getUpdatedFrameTransforms(std::vector<geometry_msgs::TransformStamped> &transforms);
00465
00466
00467 void scenePublishingThread();
00468
00469
00470 void onStateUpdate(const sensor_msgs::JointStateConstPtr &joint_state);
00471
00472
00473 void stateUpdateTimerCallback(const ros::WallTimerEvent& event);
00474
00475
00476 void newPlanningSceneCallback(const moveit_msgs::PlanningSceneConstPtr &scene);
00477
00478
00479 void newPlanningSceneMessage(const moveit_msgs::PlanningScene& scene);
00480
00481
00482
00483 boost::mutex state_pending_mutex_;
00484
00486
00487 volatile bool state_update_pending_;
00488
00490
00491 ros::WallDuration dt_state_update_;
00492
00494
00495
00496 ros::Duration shape_transform_cache_lookup_wait_time_;
00497
00499
00500
00501 ros::WallTimer state_update_timer_;
00502
00504
00505 ros::WallTime last_state_update_;
00506
00507 robot_model_loader::RobotModelLoaderPtr rm_loader_;
00508 robot_model::RobotModelConstPtr robot_model_;
00509
00510 class DynamicReconfigureImpl;
00511 DynamicReconfigureImpl *reconfigure_impl_;
00512 };
00513
00514 typedef boost::shared_ptr<PlanningSceneMonitor> PlanningSceneMonitorPtr;
00515 typedef boost::shared_ptr<const PlanningSceneMonitor> PlanningSceneMonitorConstPtr;
00516
00538 class LockedPlanningSceneRO
00539 {
00540 public:
00541
00542 LockedPlanningSceneRO(const PlanningSceneMonitorPtr &planning_scene_monitor) :
00543 planning_scene_monitor_(planning_scene_monitor)
00544 {
00545 initialize(true);
00546 }
00547
00548 const PlanningSceneMonitorPtr& getPlanningSceneMonitor()
00549 {
00550 return planning_scene_monitor_;
00551 }
00552
00553 operator bool() const
00554 {
00555 return planning_scene_monitor_ && planning_scene_monitor_->getPlanningScene();
00556 }
00557
00558 operator const planning_scene::PlanningSceneConstPtr&() const
00559 {
00560 return const_cast<const PlanningSceneMonitor*>(planning_scene_monitor_.get())->getPlanningScene();
00561 }
00562
00563 const planning_scene::PlanningSceneConstPtr& operator->() const
00564 {
00565 return const_cast<const PlanningSceneMonitor*>(planning_scene_monitor_.get())->getPlanningScene();
00566 }
00567
00568 protected:
00569
00570 LockedPlanningSceneRO(const PlanningSceneMonitorPtr &planning_scene_monitor, bool read_only) :
00571 planning_scene_monitor_(planning_scene_monitor)
00572 {
00573 initialize(read_only);
00574 }
00575
00576 void initialize(bool read_only)
00577 {
00578 if (planning_scene_monitor_)
00579 lock_.reset(new SingleUnlock(planning_scene_monitor_.get(), read_only));
00580 }
00581
00582
00583
00584 struct SingleUnlock
00585 {
00586 SingleUnlock(PlanningSceneMonitor *planning_scene_monitor, bool read_only) :
00587 planning_scene_monitor_(planning_scene_monitor), read_only_(read_only)
00588 {
00589 if (read_only)
00590 planning_scene_monitor_->lockSceneRead();
00591 else
00592 planning_scene_monitor_->lockSceneWrite();
00593 }
00594 ~SingleUnlock()
00595 {
00596 if (read_only_)
00597 planning_scene_monitor_->unlockSceneRead();
00598 else
00599 planning_scene_monitor_->unlockSceneWrite();
00600 }
00601 PlanningSceneMonitor *planning_scene_monitor_;
00602 bool read_only_;
00603 };
00604
00605 PlanningSceneMonitorPtr planning_scene_monitor_;
00606 boost::shared_ptr<SingleUnlock> lock_;
00607 };
00608
00630 class LockedPlanningSceneRW : public LockedPlanningSceneRO
00631 {
00632 public:
00633
00634 LockedPlanningSceneRW(const PlanningSceneMonitorPtr &planning_scene_monitor) :
00635 LockedPlanningSceneRO(planning_scene_monitor, false)
00636 {
00637 }
00638
00639 operator const planning_scene::PlanningScenePtr&()
00640 {
00641 return planning_scene_monitor_->getPlanningScene();
00642 }
00643
00644 const planning_scene::PlanningScenePtr& operator->()
00645 {
00646 return planning_scene_monitor_->getPlanningScene();
00647 }
00648 };
00649
00650 }
00651
00652 #endif