planning_environment: collision_space

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 
00043 //#include <motion_planning_msgs/AcceptableContact.h>
00044 
00045 
00046 #include <motion_planning_msgs/AllowedContactSpecification.h>
00047 #include <motion_planning_msgs/OrderedCollisionOperations.h>
00048 #include <motion_planning_msgs/LinkPadding.h>
00049 #include <mapping_msgs/CollisionMap.h>
00050 #include <mapping_msgs/CollisionObject.h>
00051 #include <mapping_msgs/AttachedCollisionObject.h>
00052 #include <boost/thread/mutex.hpp>
00053 #include <planning_environment_msgs/GetAllowedCollisionMatrix.h>
00054 #include <planning_environment_msgs/GetCollisionObjects.h>
00055 #include <planning_environment_msgs/SetAllowedCollisions.h>
00056 #include <std_srvs/Empty.h>
00057 
00058 #include <geometric_shapes_msgs/Shape.h>
00059 
00060 namespace planning_environment
00061 {
00062 
00066 class CollisionSpaceMonitor : public KinematicModelStateMonitor
00067 {
00068 public: 
00069 
00070   CollisionSpaceMonitor(CollisionModels *cm, tf::TransformListener *tf) : KinematicModelStateMonitor(static_cast<RobotModels*>(cm), tf)
00071   {
00072     cm_ = cm;
00073     setupCSM();
00074 
00075   }
00076         
00077   virtual ~CollisionSpaceMonitor(void)
00078   {
00079     if (collisionObjectFilter_)
00080       delete collisionObjectFilter_;
00081     if (collisionObjectSubscriber_)
00082       delete collisionObjectSubscriber_;
00083     if (collisionMapFilter_)
00084       delete collisionMapFilter_;
00085     if (collisionMapSubscriber_)
00086       delete collisionMapSubscriber_;
00087     if (collisionMapUpdateFilter_)
00088       delete collisionMapUpdateFilter_;
00089     if (collisionMapUpdateSubscriber_)
00090       delete collisionMapUpdateSubscriber_;
00091     if (attachedCollisionObjectSubscriber_)
00092       delete attachedCollisionObjectSubscriber_;
00093 
00094   }
00095 
00097   void startEnvironmentMonitor(void);
00098         
00100   void stopEnvironmentMonitor(void);
00101         
00102   virtual void advertiseServices();
00103 
00105   bool isEnvironmentMonitorStarted(void) const
00106   {
00107     return envMonitorStarted_;
00108   }     
00109 
00111   collision_space::EnvironmentModel* getEnvironmentModel(void) const
00112   {
00113     return collisionSpace_;
00114   }
00115         
00117   CollisionModels* getCollisionModels(void) const
00118   {
00119     return cm_;
00120   }
00121         
00123   void setOnBeforeMapUpdateCallback(const boost::function<void(const mapping_msgs::CollisionMapConstPtr, bool)> &callback)
00124   {
00125     onBeforeMapUpdate_ = callback;
00126   }
00127 
00129   void setOnAfterMapUpdateCallback(const boost::function<void(const mapping_msgs::CollisionMapConstPtr, bool)> &callback)
00130   {
00131     onAfterMapUpdate_ = callback;
00132   }
00133 
00135   void setOnAfterAttachCollisionObjectCallback(const boost::function<void(const mapping_msgs::AttachedCollisionObjectConstPtr &attachedObject)> &callback)
00136   {
00137     onAfterAttachCollisionObject_ = callback;
00138   }
00139 
00141   void setOnAfterCollisionObjectCallback(const boost::function<void(const mapping_msgs::CollisionObjectConstPtr &attachedObject)> &callback)
00142   {
00143     onCollisionObjectUpdate_ = callback;
00144   }
00145 
00147   bool haveMap(void) const
00148   {
00149     return haveMap_;
00150   }
00151         
00153   bool isMapUpdated(double sec) const;
00154         
00156   void waitForMap(void) const;  
00157 
00159   const ros::Time& lastMapUpdate(void) const
00160   {
00161     return lastMapUpdate_;
00162   }
00163         
00165   double getPointCloudPadd(void) const
00166   {
00167     return pointcloud_padd_;
00168   }
00169         
00171   //    bool computeAllowedContact(const motion_planning_msgs::AcceptableContact &allowedContactMsg, collision_space::EnvironmentModel::AllowedContact &allowedContact) const;
00172   bool computeAllowedContact(const motion_planning_msgs::AllowedContactSpecification &region,
00173                              collision_space::EnvironmentModel::AllowedContact &allowedContact) const;
00174         
00177   void recoverCollisionMap(mapping_msgs::CollisionMap &cmap);
00178 
00181   void recoverCollisionMap(const collision_space::EnvironmentModel *env, mapping_msgs::CollisionMap &cmap);
00182 
00183         
00187   void recoverCollisionObjects(std::vector<mapping_msgs::CollisionObject> &omap);
00188         
00193   void recoverCollisionObjects(const collision_space::EnvironmentModel *env, std::vector<mapping_msgs::CollisionObject> &omap);
00194 
00197   void recoverAttachedCollisionObjects(std::vector<mapping_msgs::AttachedCollisionObject> &avec);
00198         
00202   void recoverAttachedCollisionObjects(const collision_space::EnvironmentModel *env, std::vector<mapping_msgs::AttachedCollisionObject> &avec);
00203   
00204   bool getObjectsService(planning_environment_msgs::GetCollisionObjects::Request &req,
00205                          planning_environment_msgs::GetCollisionObjects::Response &res);    
00211   bool applyOrderedCollisionOperationsToCollisionSpace(const motion_planning_msgs::OrderedCollisionOperations &ord, bool print = false);
00212 
00215   bool getCurrentAllowedCollisionsService(planning_environment_msgs::GetAllowedCollisionMatrix::Request& req,
00216                                           planning_environment_msgs::GetAllowedCollisionMatrix::Response& res);
00217   
00220   bool setAllowedCollisionsService(planning_environment_msgs::SetAllowedCollisions::Request& req,
00221                                    planning_environment_msgs::SetAllowedCollisions::Response& res);
00222 
00225   bool applyOrderedCollisionOperationsToMatrix(const motion_planning_msgs::OrderedCollisionOperations &ord,
00226                                                std::vector<std::vector<bool> > &curAllowed,
00227                                                std::map<std::string, unsigned int> &vecIndices);
00229   bool expandOrderedCollisionOperations(const motion_planning_msgs::OrderedCollisionOperations &ord,
00230                                         motion_planning_msgs::OrderedCollisionOperations &ex);
00231 
00233   bool revertAllowedCollisionMatrixToDefaultService(std_srvs::Empty::Request& req,
00234                                                     std_srvs::Empty::Response& res);
00235     
00237   void applyLinkPaddingToCollisionSpace(const std::vector<motion_planning_msgs::LinkPadding>& link_padding);
00238 
00240   void revertCollisionSpacePaddingToDefault(); 
00241   
00245   void revertAllowedCollisionToDefault();
00246   
00249   void printAllowedCollisionMatrix(const std::vector<std::vector<bool> > &curAllowed,
00250                                    const std::map<std::string, unsigned int> &vecIndices) const;
00251 
00252   void setUseCollisionMap(bool use);
00253 
00254   void setCollisionSpace();
00255 
00256 protected:
00257 
00258   struct KnownObject
00259   {
00260     KnownObject(void)
00261     {
00262     }
00263     
00264     ~KnownObject(void) {
00265       deleteBodies();
00266     }
00267     
00268     void deleteBodies() {
00269       for(unsigned int i = 0; i < bodies.size(); i++) {
00270         delete bodies[i];
00271       }
00272       bodies.clear();
00273     }
00274     
00275     void addBodyWithPose(bodies::Body* body, const btTransform &pose) {
00276       body->setPose(pose);
00277       bodies::BoundingSphere bsphere;
00278       body->computeBoundingSphere(bsphere);
00279       bodies.push_back(body);
00280       double rsquare = bsphere.radius * bsphere.radius;
00281       bspheres.push_back(bsphere);
00282       rsquares.push_back(rsquare);
00283     }
00284     
00285     void usePose(const unsigned int i, const btTransform &pose) {
00286       if(i >= bodies.size()) {
00287         ROS_WARN("Not enough bodies");
00288         return;
00289       }
00290       bodies[i]->setPose(pose);
00291       bodies[i]->computeBoundingSphere(bspheres[i]);
00292       rsquares[i] = bspheres[i].radius*bspheres[i].radius;
00293     }
00294     
00295     std::vector<bodies::Body*> bodies;
00296     std::vector<bodies::BoundingSphere>  bspheres;
00297     std::vector<double>                  rsquares;
00298   };
00299 
00300   void maskCollisionMapForCollisionObjects(std::vector<shapes::Shape*> &all_shapes,
00301                                            std::vector<btTransform> &all_poses,
00302                                            std::vector<bool> &mask);
00303 
00304   void updateStaticObjectBodies(const mapping_msgs::CollisionObjectConstPtr &collisionObject);
00305   
00306   void setupCSM(void);
00307   void updateCollisionSpace(const mapping_msgs::CollisionMapConstPtr &collisionMap, bool clear);
00308   void collisionMapAsSpheres(const mapping_msgs::CollisionMapConstPtr &collisionMap,
00309                              std::vector<shapes::Shape*> &spheres, std::vector<btTransform> &poses);
00310   void collisionMapAsBoxes(const mapping_msgs::CollisionMap &collisionMap,
00311                            std::vector<shapes::Shape*> &boxes, std::vector<btTransform> &poses);
00312   void collisionMapAsBoxes(const mapping_msgs::CollisionMapConstPtr &collisionMap,
00313                            std::vector<shapes::Shape*> &boxes, std::vector<btTransform> &poses);
00314   void collisionMapCallback(const mapping_msgs::CollisionMapConstPtr &collisionMap);
00315   void collisionMapUpdateCallback(const mapping_msgs::CollisionMapConstPtr &collisionMap);
00316   void collisionObjectCallback(const mapping_msgs::CollisionObjectConstPtr &collisionObject);
00317   virtual bool attachObjectCallback(const mapping_msgs::AttachedCollisionObjectConstPtr &attachedObject);
00318   void addAttachedCollisionObjects(std::vector<std::string>& svec) const;
00319 
00320   CollisionModels                                                *cm_;
00321   collision_space::EnvironmentModel                              *collisionSpace_;
00322   boost::mutex                                                    mapUpdateLock_;
00323   double                                                          pointcloud_padd_;
00324         
00325   bool                                                            envMonitorStarted_;
00326         
00327   bool                                                            haveMap_;
00328   ros::Time                                                       lastMapUpdate_;       
00329         
00330   message_filters::Subscriber<mapping_msgs::CollisionMap>        *collisionMapSubscriber_;
00331   tf::MessageFilter<mapping_msgs::CollisionMap>                  *collisionMapFilter_;
00332   message_filters::Subscriber<mapping_msgs::CollisionMap>        *collisionMapUpdateSubscriber_;
00333   tf::MessageFilter<mapping_msgs::CollisionMap>                  *collisionMapUpdateFilter_;
00334   message_filters::Subscriber<mapping_msgs::CollisionObject>         *collisionObjectSubscriber_;
00335   tf::MessageFilter<mapping_msgs::CollisionObject>                   *collisionObjectFilter_;
00336 
00337   message_filters::Subscriber<mapping_msgs::AttachedCollisionObject> *attachedCollisionObjectSubscriber_;
00338 
00339   ros::ServiceServer get_objects_service_;
00340   ros::ServiceServer get_current_collision_map_service_;
00341   ros::ServiceServer set_allowed_collisions_service_;
00342   ros::ServiceServer revert_allowed_collisions_service_;
00343 
00344   bool use_collision_map_;
00345   std::map<std::string, KnownObject*>                                 collisionObjects_;
00346   boost::recursive_mutex                                              collision_objects_lock_;
00347 
00348   boost::recursive_mutex collision_map_lock_;
00349   std::vector<shapes::Shape*> last_collision_map_shapes_;
00350   std::vector<btTransform>    last_collision_map_poses_;
00351   //true is include, false is currently masked
00352   std::vector<bool> last_collision_map_object_mask_;
00353 
00354   mapping_msgs::CollisionMap last_collision_map_;
00355   double                                                             scale_;
00356   double                                                             padd_;
00357 
00358   boost::function<void(const mapping_msgs::AttachedCollisionObjectConstPtr &attachedObject)> onAfterAttachCollisionObject_;     
00359   boost::function<void(const mapping_msgs::CollisionMapConstPtr, bool)> onBeforeMapUpdate_;
00360   boost::function<void(const mapping_msgs::CollisionMapConstPtr, bool)> onAfterMapUpdate_;
00361   boost::function<void(const mapping_msgs::CollisionObjectConstPtr)>        onCollisionObjectUpdate_;
00362     
00363 };
00364     
00365         
00366 }
00367 
00368 #endif
00369
collision_space_monitor.h
