planning_environment: 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 
00036 #include <boost/bind.hpp>
00037 #include <climits>
00038 #include <sstream>
00039 
00040 #include <planning_environment/monitors/collision_space_monitor.h>
00041 #include <planning_environment/monitors/monitor_utils.h>
00042 
00043 namespace planning_environment
00044 {
00045     
00046 static inline double maxCoord(const geometry_msgs::Point32 &point)
00047 {
00048   return std::max(std::max(point.x, point.y), point.z);
00049 }
00050 }
00051 
00052 void planning_environment::CollisionSpaceMonitor::setupCSM(void)
00053 {
00054   envMonitorStarted_ = false;
00055 
00056   collisionMapFilter_ = NULL;
00057   collisionMapUpdateFilter_ = NULL;
00058   collisionObjectFilter_ = NULL;
00059     
00060   collisionMapSubscriber_ = NULL;
00061   collisionMapUpdateSubscriber_ = NULL;
00062   collisionObjectSubscriber_ = NULL;
00063     
00064   have_map_ = false;
00065   use_collision_map_ = false;
00066 
00067   nh_.param<double>("pointcloud_padd", pointcloud_padd_, 0.00);
00068 }
00069 
00070 void planning_environment::CollisionSpaceMonitor::startEnvironmentMonitor(void)
00071 {
00072   if (envMonitorStarted_)
00073     return;
00074 
00075   if(use_collision_map_) {
00076     collisionMapSubscriber_ = new message_filters::Subscriber<arm_navigation_msgs::CollisionMap>(root_handle_, "collision_map_occ", 1);
00077     collisionMapFilter_ = new tf::MessageFilter<arm_navigation_msgs::CollisionMap>(*collisionMapSubscriber_, *tf_, cm_->getWorldFrameId(), 1);
00078     collisionMapFilter_->registerCallback(boost::bind(&CollisionSpaceMonitor::collisionMapCallback, this, _1));
00079     ROS_INFO("Listening to collision_map using message notifier with target frame %s", collisionMapFilter_->getTargetFramesString().c_str());
00080     
00081     collisionMapUpdateSubscriber_ = new message_filters::Subscriber<arm_navigation_msgs::CollisionMap>(root_handle_, "collision_map_update", 1024);
00082     collisionMapUpdateFilter_ = new tf::MessageFilter<arm_navigation_msgs::CollisionMap>(*collisionMapUpdateSubscriber_, *tf_, cm_->getWorldFrameId(), 1);
00083     collisionMapUpdateFilter_->registerCallback(boost::bind(&CollisionSpaceMonitor::collisionMapUpdateCallback, this, _1));
00084     ROS_DEBUG("Listening to collision_map_update using message notifier with target frame %s", collisionMapUpdateFilter_->getTargetFramesString().c_str());
00085   }
00086 
00087   collisionObjectSubscriber_ = new message_filters::Subscriber<arm_navigation_msgs::CollisionObject>(root_handle_, "collision_object", 1024);
00088   collisionObjectFilter_ = new tf::MessageFilter<arm_navigation_msgs::CollisionObject>(*collisionObjectSubscriber_, *tf_, cm_->getWorldFrameId(), 1024);
00089   collisionObjectFilter_->registerCallback(boost::bind(&CollisionSpaceMonitor::collisionObjectCallback, this, _1));
00090   ROS_DEBUG("Listening to object_in_map using message notifier with target frame %s", collisionObjectFilter_->getTargetFramesString().c_str());
00091   
00092   //using regular message filter as there's no header
00093   attachedCollisionObjectSubscriber_ = new message_filters::Subscriber<arm_navigation_msgs::AttachedCollisionObject>(root_handle_, "attached_collision_object", 1024);  
00094   attachedCollisionObjectSubscriber_->registerCallback(boost::bind(&CollisionSpaceMonitor::attachObjectCallback, this, _1));    
00095 
00096   envMonitorStarted_ = true;
00097 }
00098 
00099 void planning_environment::CollisionSpaceMonitor::setUseCollisionMap(bool use_collision_map) {
00100   if(use_collision_map_ == use_collision_map) return;
00101   
00102   use_collision_map_ = use_collision_map;
00103 
00104   if(!envMonitorStarted_) return;
00105 
00106   if(use_collision_map_) {
00107     collisionMapSubscriber_ = new message_filters::Subscriber<arm_navigation_msgs::CollisionMap>(root_handle_, "collision_map", 1);
00108     collisionMapFilter_ = new tf::MessageFilter<arm_navigation_msgs::CollisionMap>(*collisionMapSubscriber_, *tf_, cm_->getWorldFrameId(), 1);
00109     collisionMapFilter_->registerCallback(boost::bind(&CollisionSpaceMonitor::collisionMapCallback, this, _1));
00110     ROS_DEBUG("Listening to collision_map using message notifier with target frame %s", collisionMapFilter_->getTargetFramesString().c_str());
00111     
00112     collisionMapUpdateSubscriber_ = new message_filters::Subscriber<arm_navigation_msgs::CollisionMap>(root_handle_, "collision_map_update", 1);
00113     collisionMapUpdateFilter_ = new tf::MessageFilter<arm_navigation_msgs::CollisionMap>(*collisionMapUpdateSubscriber_, *tf_, cm_->getWorldFrameId(), 1);
00114     collisionMapUpdateFilter_->registerCallback(boost::bind(&CollisionSpaceMonitor::collisionMapUpdateCallback, this, _1));
00115     ROS_DEBUG("Listening to collision_map_update using message notifier with target frame %s", collisionMapUpdateFilter_->getTargetFramesString().c_str());
00116   } else {
00117     if(collisionMapUpdateFilter_) {
00118       delete collisionMapUpdateFilter_;
00119       collisionMapUpdateFilter_ = NULL;
00120     }
00121     if(collisionMapUpdateSubscriber_) {
00122       delete collisionMapUpdateSubscriber_;
00123       collisionMapUpdateSubscriber_ = NULL;
00124     }
00125     if(collisionMapFilter_) {
00126       delete collisionMapFilter_;
00127       collisionMapFilter_ = NULL;
00128     }
00129     
00130     if(collisionMapSubscriber_) {
00131       delete collisionMapSubscriber_;
00132       collisionMapSubscriber_ = NULL;
00133     }
00134   }
00135 }
00136   
00137 void planning_environment::CollisionSpaceMonitor::stopEnvironmentMonitor(void)
00138 {
00139   if (!envMonitorStarted_)
00140     return;
00141     
00142   if(collisionMapUpdateFilter_) {
00143     delete collisionMapUpdateFilter_;
00144     collisionMapUpdateFilter_ = NULL;
00145   }
00146   if(collisionMapUpdateSubscriber_) {
00147     delete collisionMapUpdateSubscriber_;
00148     collisionMapUpdateSubscriber_ = NULL;
00149   }
00150   if(collisionMapFilter_) {
00151     delete collisionMapFilter_;
00152     collisionMapFilter_ = NULL;
00153   }
00154   
00155   if(collisionMapSubscriber_) {
00156     delete collisionMapSubscriber_;
00157     collisionMapSubscriber_ = NULL;
00158   }
00159    
00160   if(collisionObjectFilter_) {
00161     delete collisionObjectFilter_;
00162     collisionObjectFilter_ = NULL;
00163   }
00164 
00165   if(collisionObjectSubscriber_) {
00166     delete collisionObjectSubscriber_;
00167     collisionObjectSubscriber_ = NULL;
00168   }
00169   
00170   if(attachedCollisionObjectSubscriber_) {
00171     delete attachedCollisionObjectSubscriber_;
00172     attachedCollisionObjectSubscriber_ = NULL;
00173   }
00174 
00175   ROS_DEBUG("Environment state is no longer being monitored");
00176 
00177   envMonitorStarted_ = false;
00178 }
00179 
00180 bool planning_environment::CollisionSpaceMonitor::isMapUpdated(double sec) const
00181 {
00182   if(sec < 1e-5) 
00183   {
00184     return false;
00185   }
00186 
00187   //2. it hasn't yet been a full second interval but we've updated
00188   if(last_map_update_ > ros::TIME_MIN && ros::Time::now() < ros::Time(sec)) 
00189   {
00190     return true;
00191   }
00192 
00193   //3. Been longer than sec interval, so we check that the update has happened in the indicated interval
00194   if (last_map_update_ < ros::Time::now()-ros::Duration(sec)) 
00195   {
00196     return false;
00197   }
00198 
00199   return true;
00200 }
00201 
00202 void planning_environment::CollisionSpaceMonitor::waitForMap(void) const
00203 {
00204   if(!use_collision_map_) {
00205     ROS_INFO("Not subscribing to map so not waiting");
00206     return;
00207   }
00208   int s = 0;
00209   while (nh_.ok() && !haveMap())
00210   {
00211     if (s == 0)
00212       ROS_INFO("Waiting for map ...");
00213     s = (s + 1) % 40;
00214     ros::spinOnce();
00215     ros::Duration().fromSec(0.05).sleep();
00216   }
00217   if (haveMap())
00218     ROS_INFO("Map received!");
00219 }
00220 
00221 void planning_environment::CollisionSpaceMonitor::collisionMapUpdateCallback(const arm_navigation_msgs::CollisionMapConstPtr &collisionMap)
00222 {
00223   if (collisionMap->boxes.size() > 0)
00224     updateCollisionSpace(collisionMap, false);
00225 }
00226 
00227 void planning_environment::CollisionSpaceMonitor::collisionMapCallback(const arm_navigation_msgs::CollisionMapConstPtr &collisionMap)
00228 {
00229   updateCollisionSpace(collisionMap, true);
00230 }
00231 
00232 void planning_environment::CollisionSpaceMonitor::collisionMapAsSpheres(const arm_navigation_msgs::CollisionMapConstPtr &collisionMap,
00233                                                                         std::vector<shapes::Shape*> &spheres, std::vector<btTransform> &poses)
00234 {
00235   // we want to make sure the frame the robot model is kept in is the same as the frame of the collisionMap
00236   bool transform = collisionMap->header.frame_id != cm_->getWorldFrameId();
00237   const int n = collisionMap->boxes.size();
00238     
00239   spheres.resize(n);
00240   poses.resize(n);
00241     
00242   if (transform)
00243   {
00244     std::string target = cm_->getWorldFrameId();
00245     bool err = false;
00246         
00247     //#pragma omp parallel for
00248     for (int i = 0 ; i < n ; ++i)
00249     {
00250       geometry_msgs::PointStamped psi;
00251       psi.header  = collisionMap->header;
00252       psi.point.x = collisionMap->boxes[i].center.x;
00253       psi.point.y = collisionMap->boxes[i].center.y;
00254       psi.point.z = collisionMap->boxes[i].center.z;
00255             
00256       geometry_msgs::PointStamped pso;
00257       try
00258       {
00259         tf_->transformPoint(target, psi, pso);
00260       }
00261       catch(...)
00262       {
00263         err = true;
00264         pso = psi;
00265       }
00266             
00267       poses[i].setIdentity();
00268       poses[i].setOrigin(btVector3(pso.point.x, pso.point.y, pso.point.z));
00269       spheres[i] = new shapes::Sphere(maxCoord(collisionMap->boxes[i].extents) * 0.867 + pointcloud_padd_);
00270     }
00271         
00272     if (err)
00273       ROS_ERROR("Some errors encountered in transforming the collision map to frame '%s' from frame '%s'", target.c_str(), collisionMap->header.frame_id.c_str());
00274   }
00275   else
00276   {
00277 
00278     //#pragma omp parallel for
00279     for (int i = 0 ; i < n ; ++i)
00280     {
00281       poses[i].setIdentity();
00282       poses[i].setOrigin(btVector3(collisionMap->boxes[i].center.x, collisionMap->boxes[i].center.y, collisionMap->boxes[i].center.z));
00283       spheres[i] = new shapes::Sphere(maxCoord(collisionMap->boxes[i].extents) * 0.867 + pointcloud_padd_);
00284     }
00285   }
00286 }
00287 
00288 void planning_environment::CollisionSpaceMonitor::collisionMapAsBoxes(const arm_navigation_msgs::CollisionMapConstPtr &collision_map,
00289                                                                       std::vector<shapes::Shape*> &boxes, std::vector<btTransform> &poses)
00290 {
00291   collisionMapAsBoxes(*collision_map, boxes, poses);
00292 }
00293 
00294 void planning_environment::CollisionSpaceMonitor::collisionMapAsBoxes(const arm_navigation_msgs::CollisionMap& collision_map,
00295                                                                       std::vector<shapes::Shape*> &boxes, std::vector<btTransform> &poses)
00296 {
00297 
00298   // we want to make sure the frame the robot model is kept in is the same as the frame of the collision_map
00299   bool transform = collision_map.header.frame_id != cm_->getWorldFrameId();
00300   const int n = collision_map.boxes.size();
00301     
00302   double pd = 2.0 * pointcloud_padd_;
00303     
00304   boxes.resize(n);
00305   poses.resize(n);
00306     
00307   if (transform)
00308   {
00309     std::string target = cm_->getWorldFrameId();
00310     bool err = false;
00311         
00312     //#pragma omp parallel for
00313     for (int i = 0 ; i < n ; ++i)
00314     {
00315       geometry_msgs::PointStamped psi;
00316       psi.header  = collision_map.header;
00317       psi.point.x = collision_map.boxes[i].center.x;
00318       psi.point.y = collision_map.boxes[i].center.y;
00319       psi.point.z = collision_map.boxes[i].center.z;
00320             
00321       geometry_msgs::PointStamped pso;
00322       try
00323       {
00324         tf_->transformPoint(target, psi, pso);
00325       }
00326       catch(...)
00327       {
00328         err = true;
00329         pso = psi;
00330       }
00331             
00332       poses[i].setRotation(btQuaternion(btVector3(collision_map.boxes[i].axis.x, collision_map.boxes[i].axis.y, collision_map.boxes[i].axis.z), collision_map.boxes[i].angle));
00333       poses[i].setOrigin(btVector3(pso.point.x, pso.point.y, pso.point.z));
00334       boxes[i] = new shapes::Box(collision_map.boxes[i].extents.x + pd, collision_map.boxes[i].extents.y + pd, collision_map.boxes[i].extents.z + pd);
00335     }
00336         
00337     if (err)
00338       ROS_ERROR("Some errors encountered in transforming the collision map to frame '%s' from frame '%s'", target.c_str(), collision_map.header.frame_id.c_str());
00339   }
00340   else
00341   {
00342 
00343     //#pragma omp parallel for
00344     for (int i = 0 ; i < n ; ++i)
00345     {
00346       poses[i].setRotation(btQuaternion(btVector3(collision_map.boxes[i].axis.x, collision_map.boxes[i].axis.y, collision_map.boxes[i].axis.z), collision_map.boxes[i].angle));
00347       poses[i].setOrigin(btVector3(collision_map.boxes[i].center.x, collision_map.boxes[i].center.y, collision_map.boxes[i].center.z));
00348       boxes[i] = new shapes::Box(collision_map.boxes[i].extents.x + pd, collision_map.boxes[i].extents.y + pd, collision_map.boxes[i].extents.z + pd);
00349     }
00350   }
00351 }
00352 
00353 void planning_environment::CollisionSpaceMonitor::updateCollisionSpace(const arm_navigation_msgs::CollisionMapConstPtr &collision_map, bool clear)
00354 { 
00355   std::vector<shapes::Shape*> shapes;
00356   std::vector<btTransform> poses;
00357   
00358   collisionMapAsBoxes(*collision_map, shapes, poses);
00359   //not masking here
00360   cm_->setCollisionMap(shapes, poses, false);
00361   last_map_update_ = collision_map->header.stamp;
00362   have_map_ = true;
00363 }
00364 
00365 
00366 void planning_environment::CollisionSpaceMonitor::collisionObjectCallback(const arm_navigation_msgs::CollisionObjectConstPtr &collision_object)
00367 {
00368   processCollisionObjectMsg(collision_object, *tf_, cm_);
00369 }
00370 
00371 bool planning_environment::CollisionSpaceMonitor::attachObjectCallback(const arm_navigation_msgs::AttachedCollisionObjectConstPtr &attached_object)
00372 {
00373   return processAttachedCollisionObjectMsg(attached_object, *tf_, cm_);
00374 }
00375 
00376
collision_space_monitor.cpp
