collision_space_monitor.h
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  * 
00004  *  Copyright (c) 2008, 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 the 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 
00037 #ifndef PLANNING_ENVIRONMENT_MONITORS_COLLISION_SPACE_MONITOR_
00038 #define PLANNING_ENVIRONMENT_MONITORS_COLLISION_SPACE_MONITOR_
00039 
00040 #include <planning_environment/models/collision_models.h>
00041 #include <planning_environment/monitors/kinematic_model_state_monitor.h>
00042 #include <arm_navigation_msgs/CollisionMap.h>
00043 #include <arm_navigation_msgs/CollisionObject.h>
00044 #include <arm_navigation_msgs/AttachedCollisionObject.h>
00045 #include <boost/thread/mutex.hpp>
00046 #include <std_srvs/Empty.h>
00047 
00048 namespace planning_environment
00049 {
00050 
00054 class CollisionSpaceMonitor : public KinematicModelStateMonitor
00055 {
00056 public: 
00057 
00058   CollisionSpaceMonitor(CollisionModels *cm, tf::TransformListener *tf) : KinematicModelStateMonitor(static_cast<RobotModels*>(cm), tf)
00059   {
00060     cm_ = cm;
00061     setupCSM();
00062   }
00063         
00064   virtual ~CollisionSpaceMonitor(void)
00065   {
00066     if (collisionObjectFilter_)
00067       delete collisionObjectFilter_;
00068     if (collisionObjectSubscriber_)
00069       delete collisionObjectSubscriber_;
00070     if (collisionMapFilter_)
00071       delete collisionMapFilter_;
00072     if (collisionMapSubscriber_)
00073       delete collisionMapSubscriber_;
00074     if (collisionMapUpdateFilter_)
00075       delete collisionMapUpdateFilter_;
00076     if (collisionMapUpdateSubscriber_)
00077       delete collisionMapUpdateSubscriber_;
00078     if (attachedCollisionObjectSubscriber_)
00079       delete attachedCollisionObjectSubscriber_;
00080 
00081   }
00082 
00084   void startEnvironmentMonitor(void);
00085         
00087   void stopEnvironmentMonitor(void);
00088         
00090   bool isEnvironmentMonitorStarted(void) const
00091   {
00092     return envMonitorStarted_;
00093   }     
00094 
00096   const collision_space::EnvironmentModel* getEnvironmentModel(void) const
00097   {
00098     return cm_->getCollisionSpace();
00099   }
00100         
00102   CollisionModels* getCollisionModels(void) const
00103   {
00104     return cm_;
00105   }
00106         
00108   bool haveMap(void) const
00109   {
00110     return have_map_;
00111   }
00112         
00114   bool isMapUpdated(double sec) const;
00115         
00117   void waitForMap(void) const;  
00118 
00120   const ros::Time& lastMapUpdate(void) const
00121   {
00122     return last_map_update_;
00123   }
00124         
00126   double getPointCloudPadd(void) const
00127   {
00128     return pointcloud_padd_;
00129   }
00130         
00131   void setUseCollisionMap(bool use);
00132 
00133 protected:
00134   
00135   void setupCSM(void);
00136   void updateCollisionSpace(const arm_navigation_msgs::CollisionMapConstPtr &collisionMap, bool clear);
00137   void collisionMapAsSpheres(const arm_navigation_msgs::CollisionMapConstPtr &collisionMap,
00138                              std::vector<shapes::Shape*> &spheres, std::vector<tf::Transform> &poses);
00139   void collisionMapAsBoxes(const arm_navigation_msgs::CollisionMap &collisionMap,
00140                            std::vector<shapes::Shape*> &boxes, std::vector<tf::Transform> &poses);
00141   void collisionMapAsBoxes(const arm_navigation_msgs::CollisionMapConstPtr &collisionMap,
00142                            std::vector<shapes::Shape*> &boxes, std::vector<tf::Transform> &poses);
00143   void collisionMapCallback(const arm_navigation_msgs::CollisionMapConstPtr &collisionMap);
00144   void collisionMapUpdateCallback(const arm_navigation_msgs::CollisionMapConstPtr &collisionMap);
00145   void collisionObjectCallback(const arm_navigation_msgs::CollisionObjectConstPtr &collisionObject);
00146   virtual bool attachObjectCallback(const arm_navigation_msgs::AttachedCollisionObjectConstPtr &attachedObject);
00147 
00148   CollisionModels *cm_;
00149   double pointcloud_padd_;
00150         
00151   bool envMonitorStarted_;
00152         
00153   bool have_map_;
00154   ros::Time last_map_update_;   
00155         
00156   message_filters::Subscriber<arm_navigation_msgs::CollisionMap> *collisionMapSubscriber_;
00157   tf::MessageFilter<arm_navigation_msgs::CollisionMap> *collisionMapFilter_;
00158   message_filters::Subscriber<arm_navigation_msgs::CollisionMap> *collisionMapUpdateSubscriber_;
00159   tf::MessageFilter<arm_navigation_msgs::CollisionMap> *collisionMapUpdateFilter_;
00160   message_filters::Subscriber<arm_navigation_msgs::CollisionObject> *collisionObjectSubscriber_;
00161   tf::MessageFilter<arm_navigation_msgs::CollisionObject> *collisionObjectFilter_;
00162 
00163   message_filters::Subscriber<arm_navigation_msgs::AttachedCollisionObject> *attachedCollisionObjectSubscriber_;
00164 
00165   bool use_collision_map_;
00166 
00167   boost::recursive_mutex collision_map_lock_;
00168 };
00169     
00170         
00171 }
00172 
00173 #endif
00174 


planning_environment
Author(s): Ioan Sucan
autogenerated on Mon Dec 2 2013 12:34:43