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
00080
00086 PlanningSceneMonitor(const std::string &robot_description,
00087 const boost::shared_ptr<tf::Transformer> &tf = boost::shared_ptr<tf::Transformer>(),
00088 const std::string &name = "");
00089
00095 PlanningSceneMonitor(const robot_model_loader::RobotModelLoaderPtr &rml,
00096 const boost::shared_ptr<tf::Transformer> &tf = boost::shared_ptr<tf::Transformer>(),
00097 const std::string &name = "");
00098
00105 PlanningSceneMonitor(const planning_scene::PlanningScenePtr &scene, const std::string &robot_description,
00106 const boost::shared_ptr<tf::Transformer> &tf = boost::shared_ptr<tf::Transformer>(),
00107 const std::string &name = "");
00108
00115 PlanningSceneMonitor(const planning_scene::PlanningScenePtr &scene,
00116 const robot_model_loader::RobotModelLoaderPtr &rml,
00117 const boost::shared_ptr<tf::Transformer> &tf = boost::shared_ptr<tf::Transformer>(),
00118 const std::string &name = "");
00119
00120 ~PlanningSceneMonitor();
00121
00123 const std::string& getName() const
00124 {
00125 return monitor_name_;
00126 }
00127
00129 const robot_model_loader::RobotModelLoaderPtr& getRobotModelLoader() const
00130 {
00131 return rm_loader_;
00132 }
00133
00134 const robot_model::RobotModelConstPtr& getRobotModel() const
00135 {
00136 return robot_model_;
00137 }
00138
00141 const planning_scene::PlanningScenePtr& getPlanningScene()
00142 {
00143 return scene_;
00144 }
00145
00148 const planning_scene::PlanningSceneConstPtr& getPlanningScene() const
00149 {
00150 return scene_const_;
00151 }
00152
00157 bool updatesScene(const planning_scene::PlanningSceneConstPtr &scene) const;
00158
00163 bool updatesScene(const planning_scene::PlanningScenePtr &scene) const;
00164
00167 const std::string& getRobotDescription() const
00168 {
00169 return robot_description_;
00170 }
00171
00173 double getDefaultRobotPadding() const
00174 {
00175 return default_robot_padd_;
00176 }
00177
00179 double getDefaultRobotScale() const
00180 {
00181 return default_robot_scale_;
00182 }
00183
00185 double getDefaultObjectPadding() const
00186 {
00187 return default_object_padd_;
00188 }
00189
00191 double getDefaultAttachedObjectPadding() const
00192 {
00193 return default_attached_padd_;
00194 }
00195
00197 const boost::shared_ptr<tf::Transformer>& getTFClient() const
00198 {
00199 return tf_;
00200 }
00201
00206 void monitorDiffs(bool flag);
00207
00210 void startPublishingPlanningScene(SceneUpdateType event, const std::string &planning_scene_topic = "monitored_planning_scene");
00211
00213 void stopPublishingPlanningScene();
00214
00216 void setPlanningScenePublishingFrequency(double hz);
00217
00219 double getPlanningScenePublishingFrequency() const
00220 {
00221 return publish_planning_scene_frequency_;
00222 }
00223
00226 const CurrentStateMonitorPtr& getStateMonitor() const
00227 {
00228 return current_state_monitor_;
00229 }
00230
00235 void updateFrameTransforms();
00236
00240 void startStateMonitor(const std::string &joint_states_topic = "joint_states", const std::string &attached_objects_topic = "attached_collision_object");
00241
00243 void stopStateMonitor();
00244
00247 void updateSceneWithCurrentState();
00248
00252 void setStateUpdateFrequency(double hz);
00253
00255 double getStateUpdateFrequency();
00256
00260 void startSceneMonitor(const std::string &scene_topic = "planning_scene");
00261
00263 void stopSceneMonitor();
00264
00269 void startWorldGeometryMonitor(const std::string &collision_objects_topic = "collision_object",
00270 const std::string &collision_map_topic = "collision_map",
00271 const std::string &planning_scene_world_topic = "planning_scene_world");
00272
00274 void stopWorldGeometryMonitor();
00275
00277 void addUpdateCallback(const boost::function<void(SceneUpdateType)> &fn);
00278
00280 void clearUpdateCallbacks();
00281
00283 void getMonitoredTopics(std::vector<std::string> &topics) const;
00284
00286 const ros::Time& getLastUpdateTime() const
00287 {
00288 return last_update_time_;
00289 }
00290
00291 void publishDebugInformation(bool flag);
00292
00294 void triggerSceneUpdateEvent(SceneUpdateType update_type);
00295
00297 void lockSceneRead();
00298
00300 void unlockSceneRead();
00301
00303 void lockSceneWrite();
00304
00306 void unlockSceneWrite();
00307
00308 protected:
00309
00312 void initialize(const planning_scene::PlanningScenePtr &scene);
00313
00315 void configureCollisionMatrix(const planning_scene::PlanningScenePtr &scene);
00316
00318 void configureDefaultPadding();
00319
00321 void newPlanningSceneCallback(const moveit_msgs::PlanningSceneConstPtr &scene);
00322
00324 void collisionObjectCallback(const moveit_msgs::CollisionObjectConstPtr &obj);
00325
00327 void collisionObjectFailTFCallback(const moveit_msgs::CollisionObjectConstPtr &obj, tf::filter_failure_reasons::FilterFailureReason reason);
00328
00330 void newPlanningSceneWorldCallback(const moveit_msgs::PlanningSceneWorldConstPtr &world);
00331
00333 void collisionMapCallback(const moveit_msgs::CollisionMapConstPtr &map);
00334
00336 void octomapUpdateCallback();
00337
00339 void attachObjectCallback(const moveit_msgs::AttachedCollisionObjectConstPtr &obj);
00340
00342 void currentStateAttachedBodyUpdateCallback(robot_state::AttachedBody *attached_body, bool just_attached);
00343
00345 void currentWorldObjectUpdateCallback(const collision_detection::World::ObjectConstPtr &object, collision_detection::World::Action action);
00346
00347 void includeRobotLinksInOctree();
00348 void excludeRobotLinksFromOctree();
00349
00350 void excludeWorldObjectsFromOctree();
00351 void includeWorldObjectsInOctree();
00352 void excludeWorldObjectFromOctree(const collision_detection::World::ObjectConstPtr &obj);
00353 void includeWorldObjectInOctree(const collision_detection::World::ObjectConstPtr &obj);
00354
00355 void excludeAttachedBodiesFromOctree();
00356 void includeAttachedBodiesInOctree();
00357 void excludeAttachedBodyFromOctree(const robot_state::AttachedBody *attached_body);
00358 void includeAttachedBodyInOctree(const robot_state::AttachedBody *attached_body);
00359
00360 bool getShapeTransformCache(const std::string &target_frame, const ros::Time &target_time, occupancy_map_monitor::ShapeTransformCache &cache) const;
00361
00363 std::string monitor_name_;
00364
00365 planning_scene::PlanningScenePtr scene_;
00366 planning_scene::PlanningSceneConstPtr scene_const_;
00367 planning_scene::PlanningScenePtr parent_scene_;
00368 boost::shared_mutex scene_update_mutex_;
00369
00370 ros::NodeHandle nh_;
00371 ros::NodeHandle root_nh_;
00372 boost::shared_ptr<tf::Transformer> tf_;
00373 std::string robot_description_;
00374
00376 double default_robot_padd_;
00378 double default_robot_scale_;
00380 double default_object_padd_;
00382 double default_attached_padd_;
00383
00384
00385 ros::Publisher planning_scene_publisher_;
00386 boost::scoped_ptr<boost::thread> publish_planning_scene_;
00387 double publish_planning_scene_frequency_;
00388 SceneUpdateType publish_update_types_;
00389 SceneUpdateType new_scene_update_;
00390 boost::condition_variable_any new_scene_update_condition_;
00391
00392
00393 ros::Subscriber planning_scene_subscriber_;
00394 ros::Subscriber planning_scene_world_subscriber_;
00395
00396 ros::Subscriber attached_collision_object_subscriber_;
00397
00398 boost::scoped_ptr<message_filters::Subscriber<moveit_msgs::CollisionObject> > collision_object_subscriber_;
00399 boost::scoped_ptr<tf::MessageFilter<moveit_msgs::CollisionObject> > collision_object_filter_;
00400
00401 boost::scoped_ptr<message_filters::Subscriber<moveit_msgs::CollisionMap> > collision_map_subscriber_;
00402 boost::scoped_ptr<tf::MessageFilter<moveit_msgs::CollisionMap> > collision_map_filter_;
00403
00404
00405 boost::scoped_ptr<occupancy_map_monitor::OccupancyMapMonitor> octomap_monitor_;
00406
00407
00408 CurrentStateMonitorPtr current_state_monitor_;
00409
00410
00411 typedef std::map<std::string, occupancy_map_monitor::ShapeHandle> LinkShapeHandles;
00412 typedef std::map<const robot_state::AttachedBody*, std::vector<std::pair<occupancy_map_monitor::ShapeHandle, std::size_t> > > AttachedBodyShapeHandles;
00413 typedef std::map<std::string, std::vector<std::pair<occupancy_map_monitor::ShapeHandle, Eigen::Affine3d*> > > CollisionBodyShapeHandles;
00414
00415 LinkShapeHandles link_shape_handles_;
00416 AttachedBodyShapeHandles attached_body_shape_handles_;
00417 CollisionBodyShapeHandles collision_body_shape_handles_;
00418 mutable boost::recursive_mutex shape_handles_lock_;
00419
00420 std::vector<boost::function<void(SceneUpdateType)> > update_callbacks_;
00421 ros::Time last_update_time_;
00422
00423 private:
00424
00425 void getUpdatedFrameTransforms(std::vector<geometry_msgs::TransformStamped> &transforms);
00426
00427 void scenePublishingThread();
00428
00429 void onStateUpdate(const sensor_msgs::JointStateConstPtr &joint_state);
00430
00432 double dt_state_update_;
00433
00436 ros::WallTime last_state_update_;
00437
00438 robot_model_loader::RobotModelLoaderPtr rm_loader_;
00439 robot_model::RobotModelConstPtr robot_model_;
00440
00441 class DynamicReconfigureImpl;
00442 DynamicReconfigureImpl *reconfigure_impl_;
00443 };
00444
00445 typedef boost::shared_ptr<PlanningSceneMonitor> PlanningSceneMonitorPtr;
00446 typedef boost::shared_ptr<const PlanningSceneMonitor> PlanningSceneMonitorConstPtr;
00447
00449 class LockedPlanningSceneRO
00450 {
00451 public:
00452
00453 LockedPlanningSceneRO(const PlanningSceneMonitorPtr &planning_scene_monitor) :
00454 planning_scene_monitor_(planning_scene_monitor)
00455 {
00456 initialize(true);
00457 }
00458
00459 const PlanningSceneMonitorPtr& getPlanningSceneMonitor()
00460 {
00461 return planning_scene_monitor_;
00462 }
00463
00464 operator bool() const
00465 {
00466 return planning_scene_monitor_ && planning_scene_monitor_->getPlanningScene();
00467 }
00468
00469 operator const planning_scene::PlanningSceneConstPtr&() const
00470 {
00471 return const_cast<const PlanningSceneMonitor*>(planning_scene_monitor_.get())->getPlanningScene();
00472 }
00473
00474 const planning_scene::PlanningSceneConstPtr& operator->() const
00475 {
00476 return const_cast<const PlanningSceneMonitor*>(planning_scene_monitor_.get())->getPlanningScene();
00477 }
00478
00479 protected:
00480
00481 LockedPlanningSceneRO(const PlanningSceneMonitorPtr &planning_scene_monitor, bool read_only) :
00482 planning_scene_monitor_(planning_scene_monitor)
00483 {
00484 initialize(read_only);
00485 }
00486
00487 void initialize(bool read_only)
00488 {
00489 if (planning_scene_monitor_)
00490 lock_.reset(new SingleUnlock(planning_scene_monitor_.get(), read_only));
00491 }
00492
00493
00494
00495 struct SingleUnlock
00496 {
00497 SingleUnlock(PlanningSceneMonitor *planning_scene_monitor, bool read_only) :
00498 planning_scene_monitor_(planning_scene_monitor), read_only_(read_only)
00499 {
00500 if (read_only)
00501 planning_scene_monitor_->lockSceneRead();
00502 else
00503 planning_scene_monitor_->lockSceneWrite();
00504 }
00505 ~SingleUnlock()
00506 {
00507 if (read_only_)
00508 planning_scene_monitor_->unlockSceneRead();
00509 else
00510 planning_scene_monitor_->unlockSceneWrite();
00511 }
00512 PlanningSceneMonitor *planning_scene_monitor_;
00513 bool read_only_;
00514 };
00515
00516 PlanningSceneMonitorPtr planning_scene_monitor_;
00517 boost::shared_ptr<SingleUnlock> lock_;
00518 };
00519
00520 class LockedPlanningSceneRW : public LockedPlanningSceneRO
00521 {
00522 public:
00523
00524 LockedPlanningSceneRW(const PlanningSceneMonitorPtr &planning_scene_monitor) :
00525 LockedPlanningSceneRO(planning_scene_monitor, false)
00526 {
00527 }
00528
00529 operator const planning_scene::PlanningScenePtr&()
00530 {
00531 return planning_scene_monitor_->getPlanningScene();
00532 }
00533
00534 const planning_scene::PlanningScenePtr& operator->()
00535 {
00536 return planning_scene_monitor_->getPlanningScene();
00537 }
00538 };
00539
00540 }
00541
00542 #endif