self_mask.h
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #ifndef ROBOT_SELF_FILTER_SELF_MASK_
00031 #define ROBOT_SELF_FILTER_SELF_MASK_
00032 
00033 #include <pcl/point_types.h>
00034 #include <pcl_ros/point_cloud.h>
00035 #include <sensor_msgs/PointCloud2.h>
00036 #include <pr2_navigation_self_filter/bodies.h>
00037 #include <tf/transform_listener.h>
00038 #include <boost/bind.hpp>
00039 #include <boost/filesystem.hpp>
00040 #include <string>
00041 #include <vector>
00042 
00043 #include <urdf/model.h>
00044 #include <resource_retriever/retriever.h>
00045 
00046 namespace robot_self_filter
00047 {
00048 
00050     enum
00051     {
00052         INSIDE = 0,
00053         OUTSIDE = 1,
00054         SHADOW = 2,
00055     };
00056 
00057 struct LinkInfo
00058 {
00059   std::string name;
00060   double padding;
00061   double scale;
00062 };
00063     
00064     static inline tf::Transform urdfPose2TFTransform(const urdf::Pose &pose)
00065     {
00066       return tf::Transform(tf::Quaternion(pose.rotation.x, pose.rotation.y, pose.rotation.z, pose.rotation.w),
00067                            tf::Vector3(pose.position.x, pose.position.y, pose.position.z));
00068     }
00069 
00070     static shapes::Shape* constructShape(const urdf::Geometry *geom)
00071     {
00072         ROS_ASSERT(geom);
00073         
00074         shapes::Shape *result = NULL;
00075         switch (geom->type)
00076         {
00077         case urdf::Geometry::SPHERE:
00078             result = new shapes::Sphere(dynamic_cast<const urdf::Sphere*>(geom)->radius);
00079             break;      
00080         case urdf::Geometry::BOX:
00081             {
00082                 urdf::Vector3 dim = dynamic_cast<const urdf::Box*>(geom)->dim;
00083                 result = new shapes::Box(dim.x, dim.y, dim.z);
00084             }
00085             break;
00086         case urdf::Geometry::CYLINDER:
00087             result = new shapes::Cylinder(dynamic_cast<const urdf::Cylinder*>(geom)->radius,
00088                                           dynamic_cast<const urdf::Cylinder*>(geom)->length);
00089             break;
00090         case urdf::Geometry::MESH:
00091             {
00092                 const urdf::Mesh *mesh = dynamic_cast<const urdf::Mesh*>(geom);
00093                 if (!mesh->filename.empty())
00094                 {
00095                     resource_retriever::Retriever retriever;
00096                     resource_retriever::MemoryResource res;
00097                     bool ok = true;
00098                     
00099                     try
00100                     {
00101                         res = retriever.get(mesh->filename);
00102                     }
00103                     catch (resource_retriever::Exception& e)
00104                     {
00105                         ROS_ERROR("%s", e.what());
00106                         ok = false;
00107                     }
00108                     
00109                     if (ok)
00110                     {
00111                         if (res.size == 0)
00112                             ROS_WARN("Retrieved empty mesh for resource '%s'", mesh->filename.c_str());
00113                         else
00114                         {
00115                             boost::filesystem::path model_path(mesh->filename);
00116                             std::string ext = model_path.extension().string();
00117                             if (ext == ".dae" || ext == ".DAE") {
00118                               result = shapes::createMeshFromBinaryDAE(mesh->filename.c_str());
00119                             }
00120                             else {
00121                               result = shapes::createMeshFromBinaryStlData(reinterpret_cast<char*>(res.data.get()), res.size);
00122                             }
00123                             if (result == NULL)
00124                                 ROS_ERROR("Failed to load mesh '%s'", mesh->filename.c_str());
00125                         }
00126                     }
00127                 }
00128                 else
00129                     ROS_WARN("Empty mesh filename");
00130             }
00131             
00132             break;
00133         default:
00134             ROS_ERROR("Unknown geometry type: %d", (int)geom->type);
00135             break;
00136         }
00137         
00138         return result;
00139     }
00140 
00144     template <typename PointT>
00145     class SelfMask
00146     {   
00147     protected:
00148         
00149         struct SeeLink
00150         {
00151             SeeLink(void)
00152             {
00153                 body = unscaledBody = NULL;
00154             }
00155             
00156             std::string   name;
00157             bodies::Body *body;
00158             bodies::Body *unscaledBody;
00159             tf::Transform   constTransf;
00160             double        volume;
00161         };
00162         
00163         struct SortBodies
00164         {
00165             bool operator()(const SeeLink &b1, const SeeLink &b2)
00166             {
00167                 return b1.volume > b2.volume;
00168             }
00169         };
00170         
00171     public:
00172         typedef pcl::PointCloud<PointT> PointCloud;
00173 
00175         SelfMask(tf::TransformListener &tf, const std::vector<LinkInfo> &links) : tf_(tf)
00176         {
00177             configure(links);
00178         }
00179         
00182         ~SelfMask(void)
00183         {
00184             freeMemory();
00185         }
00186         
00190         void maskContainment(const PointCloud& data_in, std::vector<int> &mask)
00191         {
00192           mask.resize(data_in.points.size());
00193           if (bodies_.empty())
00194             std::fill(mask.begin(), mask.end(), (int)OUTSIDE);
00195           else
00196           {
00197             std_msgs::Header header = pcl_conversions::fromPCL(data_in.header);
00198             assumeFrame(header);
00199             maskAuxContainment(data_in, mask);
00200           }
00201 
00202         }
00203 
00213         void maskIntersection(const PointCloud& data_in, const std::string &sensor_frame, const double min_sensor_dist,
00214                               std::vector<int> &mask, const boost::function<void(const tf::Vector3&)> &intersectionCallback = NULL)
00215         {
00216           mask.resize(data_in.points.size());
00217           if (bodies_.empty()) {
00218             std::fill(mask.begin(), mask.end(), (int)OUTSIDE);
00219           }
00220           else
00221           {
00222             std_msgs::Header header = pcl_conversions::fromPCL(data_in.header);
00223             assumeFrame(header, sensor_frame, min_sensor_dist);
00224             if (sensor_frame.empty())
00225               maskAuxContainment(data_in, mask);
00226             else
00227               maskAuxIntersection(data_in, mask, intersectionCallback);
00228           }
00229           
00230         }
00231         
00232         
00239         void maskIntersection(const PointCloud& data_in, const tf::Vector3 &sensor_pos, const double min_sensor_dist,
00240                               std::vector<int> &mask, const boost::function<void(const tf::Vector3&)> &intersectionCallback = NULL)
00241         {
00242           mask.resize(data_in.points.size());
00243           if (bodies_.empty())
00244             std::fill(mask.begin(), mask.end(), (int)OUTSIDE);
00245           else
00246           {
00247             std_msgs::Header header = pcl_conversions::fromPCL(data_in.header);
00248             assumeFrame(header, sensor_pos, min_sensor_dist);
00249             maskAuxIntersection(data_in, mask, intersectionCallback);
00250           }
00251 
00252         }
00253         
00256         void assumeFrame(const std_msgs::Header& header)
00257         {
00258               const unsigned int bs = bodies_.size();
00259     
00260     // place the links in the assumed frame 
00261     for (unsigned int i = 0 ; i < bs ; ++i)
00262     {
00263       std::string err;
00264       if(!tf_.waitForTransform(header.frame_id, bodies_[i].name, header.stamp, ros::Duration(.1), ros::Duration(.01), &err)) {
00265         ROS_ERROR("WaitForTransform timed out from %s to %s after 100ms.  Error string: %s", bodies_[i].name.c_str(), header.frame_id.c_str(), err.c_str());
00266         
00267       } 
00268       
00269       // find the transform between the link's frame and the pointcloud frame
00270       tf::StampedTransform transf;
00271       try
00272       {
00273         tf_.lookupTransform(header.frame_id, bodies_[i].name, header.stamp, transf);
00274       }
00275       catch(tf::TransformException& ex)
00276       {
00277         transf.setIdentity();
00278         ROS_ERROR("Unable to lookup transform from %s to %s. Exception: %s", bodies_[i].name.c_str(), header.frame_id.c_str(), ex.what());      
00279       }
00280       
00281       // set it for each body; we also include the offset specified in URDF
00282       bodies_[i].body->setPose(transf * bodies_[i].constTransf);
00283       bodies_[i].unscaledBody->setPose(transf * bodies_[i].constTransf);
00284     }
00285     
00286     computeBoundingSpheres();
00287 
00288         }
00289         
00290         
00293         void assumeFrame(const std_msgs::Header& header, const tf::Vector3 &sensor_pos, const double min_sensor_dist)
00294         {
00295           assumeFrame(header);
00296           sensor_pos_ = sensor_pos;
00297           min_sensor_dist_ = min_sensor_dist;
00298         }
00299 
00302         void assumeFrame(const std_msgs::Header& header, const std::string &sensor_frame, const double min_sensor_dist)
00303         {
00304           assumeFrame(header);
00305 
00306           std::string err;
00307           if(!tf_.waitForTransform(header.frame_id, sensor_frame, header.stamp, ros::Duration(.1), ros::Duration(.01), &err)) {
00308             ROS_ERROR("WaitForTransform timed out from %s to %s after 100ms.  Error string: %s", sensor_frame.c_str(), header.frame_id.c_str(), err.c_str());
00309             sensor_pos_.setValue(0, 0, 0);
00310           } 
00311 
00312           //transform should be there
00313           // compute the origin of the sensor in the frame of the cloud
00314           try
00315           {
00316             tf::StampedTransform transf;
00317             tf_.lookupTransform(header.frame_id, sensor_frame, header.stamp, transf);
00318             sensor_pos_ = transf.getOrigin();
00319           }
00320           catch(tf::TransformException& ex)
00321           {
00322             sensor_pos_.setValue(0, 0, 0);
00323             ROS_ERROR("Unable to lookup transform from %s to %s.  Exception: %s", sensor_frame.c_str(), header.frame_id.c_str(), ex.what());
00324           }
00325   
00326           min_sensor_dist_ = min_sensor_dist;
00327 
00328         }
00329         
00332         int  getMaskContainment(const tf::Vector3 &pt) const
00333         {
00334           const unsigned int bs = bodies_.size();
00335           int out = OUTSIDE;
00336           for (unsigned int j = 0 ; out == OUTSIDE && j < bs ; ++j)
00337             if (bodies_[j].body->containsPoint(pt))
00338               out = INSIDE;
00339           return out;
00340         }
00341         
00344         int  getMaskContainment(double x, double y, double z) const
00345         {
00346           return getMaskContainment(tf::Vector3(x, y, z));
00347         }
00348         
00352         int  getMaskIntersection(double x, double y, double z, const boost::function<void(const tf::Vector3&)> &intersectionCallback = NULL) const
00353         {
00354           return getMaskIntersection(tf::Vector3(x, y, z), intersectionCallback);
00355         }
00356         
00360         int  getMaskIntersection(const tf::Vector3 &pt, const boost::function<void(const tf::Vector3&)> &intersectionCallback = NULL) const
00361         {
00362           const unsigned int bs = bodies_.size();
00363 
00364           // we first check is the point is in the unscaled body. 
00365           // if it is, the point is definitely inside
00366           int out = OUTSIDE;
00367           for (unsigned int j = 0 ; out == OUTSIDE && j < bs ; ++j)
00368             if (bodies_[j].unscaledBody->containsPoint(pt))
00369               out = INSIDE;
00370     
00371           if (out == OUTSIDE)
00372           {
00373 
00374             // we check it the point is a shadow point 
00375             tf::Vector3 dir(sensor_pos_ - pt);
00376             tfScalar  lng = dir.length();
00377             if (lng < min_sensor_dist_)
00378               out = INSIDE;
00379             else
00380             {
00381               dir /= lng;
00382             
00383               std::vector<tf::Vector3> intersections;
00384               for (unsigned int j = 0 ; out == OUTSIDE && j < bs ; ++j)
00385                 if (bodies_[j].body->intersectsRay(pt, dir, &intersections, 1))
00386                 {
00387                   if (dir.dot(sensor_pos_ - intersections[0]) >= 0.0)
00388                   {
00389                     if (intersectionCallback)
00390                       intersectionCallback(intersections[0]);
00391                     out = SHADOW;
00392                   }
00393                 }
00394             
00395               // if it is not a shadow point, we check if it is inside the scaled body
00396               for (unsigned int j = 0 ; out == OUTSIDE && j < bs ; ++j)
00397                 if (bodies_[j].body->containsPoint(pt))
00398                   out = INSIDE;
00399             }
00400           }
00401           return out;
00402 
00403         }
00404         
00406         void getLinkNames(std::vector<std::string> &frames) const
00407         {
00408           for (unsigned int i = 0 ; i < bodies_.size() ; ++i)
00409             frames.push_back(bodies_[i].name);
00410         }
00411         
00412     private:
00413 
00415         void freeMemory(void)
00416         {
00417           for (unsigned int i = 0 ; i < bodies_.size() ; ++i)
00418             {
00419               if (bodies_[i].body)
00420                 delete bodies_[i].body;
00421               if (bodies_[i].unscaledBody)
00422                 delete bodies_[i].unscaledBody;
00423             }
00424           
00425           bodies_.clear();
00426         }
00427 
00428 
00430         bool configure(const std::vector<LinkInfo> &links)
00431         {
00432           // in case configure was called before, we free the memory
00433           freeMemory();
00434           sensor_pos_.setValue(0, 0, 0);
00435     
00436           std::string content;
00437           boost::shared_ptr<urdf::Model> urdfModel;
00438 
00439           if (nh_.getParam("robot_description", content))
00440           {
00441             urdfModel = boost::shared_ptr<urdf::Model>(new urdf::Model());
00442             if (!urdfModel->initString(content))
00443             {
00444               ROS_ERROR("Unable to parse URDF description!");
00445               return false;
00446             }
00447           }
00448           else
00449           {
00450             ROS_ERROR("Robot model not found! Did you remap 'robot_description'?");
00451             return false;
00452           }
00453           
00454           std::stringstream missing;
00455     
00456           // from the geometric model, find the shape of each link of interest
00457           // and create a body from it, one that knows about poses and can 
00458           // check for point inclusion
00459           for (unsigned int i = 0 ; i < links.size() ; ++i)
00460           {
00461             const urdf::Link *link = urdfModel->getLink(links[i].name).get();
00462             if (!link)
00463             {
00464               missing << " " << links[i].name;
00465               continue;
00466             }
00467             
00468             if (!(link->collision && link->collision->geometry))
00469             {
00470               ROS_WARN("No collision geometry specified for link '%s'", links[i].name.c_str());
00471               continue;
00472             }
00473         
00474             shapes::Shape *shape = constructShape(link->collision->geometry.get());
00475         
00476             if (!shape)
00477             {
00478               ROS_ERROR("Unable to construct collision shape for link '%s'", links[i].name.c_str());
00479               continue;
00480             }
00481         
00482             SeeLink sl;
00483             sl.body = bodies::createBodyFromShape(shape);
00484 
00485             if (sl.body)
00486             {
00487               sl.name = links[i].name;
00488               
00489               // collision models may have an offset, in addition to what TF gives
00490               // so we keep it around
00491               sl.constTransf = urdfPose2TFTransform(link->collision->origin);
00492               
00493               sl.body->setScale(links[i].scale);
00494               sl.body->setPadding(links[i].padding);
00495               ROS_INFO_STREAM("Self see link name " <<  links[i].name << " padding " << links[i].padding);
00496               sl.volume = sl.body->computeVolume();
00497               sl.unscaledBody = bodies::createBodyFromShape(shape);
00498               bodies_.push_back(sl);
00499             }
00500             else
00501               ROS_WARN("Unable to create point inclusion body for link '%s'", links[i].name.c_str());
00502         
00503             delete shape;
00504           }
00505     
00506           if (missing.str().size() > 0)
00507             ROS_WARN("Some links were included for self mask but they do not exist in the model:%s", missing.str().c_str());
00508     
00509           if (bodies_.empty())
00510             ROS_WARN("No robot links will be checked for self mask");
00511     
00512           // put larger volume bodies first -- higher chances of containing a point
00513           std::sort(bodies_.begin(), bodies_.end(), SortBodies());
00514     
00515           bspheres_.resize(bodies_.size());
00516           bspheresRadius2_.resize(bodies_.size());
00517 
00518           for (unsigned int i = 0 ; i < bodies_.size() ; ++i)
00519             ROS_DEBUG("Self mask includes link %s with volume %f", bodies_[i].name.c_str(), bodies_[i].volume);
00520     
00521           //ROS_INFO("Self filter using %f padding and %f scaling", padd, scale);
00522 
00523           return true; 
00524 
00525         }
00526         
00528         void computeBoundingSpheres(void)
00529         {
00530           const unsigned int bs = bodies_.size();
00531           for (unsigned int i = 0 ; i < bs ; ++i)
00532           {
00533             bodies_[i].body->computeBoundingSphere(bspheres_[i]);
00534             bspheresRadius2_[i] = bspheres_[i].radius * bspheres_[i].radius;
00535           }
00536         }
00537 
00538         
00540         void maskAuxContainment(const PointCloud& data_in, std::vector<int> &mask)
00541         {
00542           const unsigned int bs = bodies_.size();
00543           const unsigned int np = data_in.points.size();
00544     
00545           // compute a sphere that bounds the entire robot
00546           bodies::BoundingSphere bound;
00547           bodies::mergeBoundingSpheres(bspheres_, bound);         
00548           tfScalar radiusSquared = bound.radius * bound.radius;
00549     
00550           // we now decide which points we keep
00551           //#pragma omp parallel for schedule(dynamic) 
00552           for (int i = 0 ; i < (int)np ; ++i)
00553           {
00554             tf::Vector3 pt = tf::Vector3(data_in.points[i].x, data_in.points[i].y, data_in.points[i].z);
00555             int out = OUTSIDE;
00556             if (bound.center.distance2(pt) < radiusSquared)
00557               for (unsigned int j = 0 ; out == OUTSIDE && j < bs ; ++j)
00558                 if (bodies_[j].body->containsPoint(pt))
00559                   out = INSIDE;
00560         
00561             mask[i] = out;
00562           }
00563         }
00564 
00566         void maskAuxIntersection(const PointCloud& data_in, std::vector<int> &mask, const boost::function<void(const tf::Vector3&)> &callback)
00567         {
00568           const unsigned int bs = bodies_.size();
00569           const unsigned int np = data_in.points.size();
00570     
00571           // compute a sphere that bounds the entire robot
00572           bodies::BoundingSphere bound;
00573           bodies::mergeBoundingSpheres(bspheres_, bound);         
00574           tfScalar radiusSquared = bound.radius * bound.radius;
00575 
00576           //std::cout << "Testing " << np << " points\n";
00577 
00578           // we now decide which points we keep
00579           //#pragma omp parallel for schedule(dynamic) 
00580           for (int i = 0 ; i < (int)np ; ++i)
00581           {
00582             bool print = false;
00583             //if(i%100 == 0) print = true;
00584             tf::Vector3 pt = tf::Vector3(data_in.points[i].x, data_in.points[i].y, data_in.points[i].z);
00585             int out = OUTSIDE;
00586 
00587             // we first check is the point is in the unscaled body. 
00588             // if it is, the point is definitely inside
00589             if (bound.center.distance2(pt) < radiusSquared)
00590               for (unsigned int j = 0 ; out == OUTSIDE && j < bs ; ++j)
00591                 if (bodies_[j].unscaledBody->containsPoint(pt)) {
00592                   if(print)
00593                     std::cout << "Point " << i << " in unscaled body part " << bodies_[j].name << std::endl;
00594                   out = INSIDE;
00595                 }
00596 
00597             // if the point is not inside the unscaled body,
00598             if (out == OUTSIDE)
00599             {
00600               // we check it the point is a shadow point 
00601               tf::Vector3 dir(sensor_pos_ - pt);
00602               tfScalar  lng = dir.length();
00603               if (lng < min_sensor_dist_) {
00604                 out = INSIDE;
00605                 //std::cout << "Point " << i << " less than min sensor distance away\n";
00606               }
00607               else
00608               {         
00609                 dir /= lng;
00610 
00611                 std::vector<tf::Vector3> intersections;
00612                 for (unsigned int j = 0 ; out == OUTSIDE && j < bs ; ++j) {
00613                   if (bodies_[j].body->intersectsRay(pt, dir, &intersections, 1))
00614                   {
00615                     if (dir.dot(sensor_pos_ - intersections[0]) >= 0.0)
00616                     {
00617                       if (callback)
00618                         callback(intersections[0]);
00619                       out = SHADOW;
00620                       if(print) std::cout << "Point " << i << " shadowed by body part " << bodies_[j].name << std::endl;
00621                     }
00622                   }
00623                 }
00624                 // if it is not a shadow point, we check if it is inside the scaled body
00625                 if (out == OUTSIDE && bound.center.distance2(pt) < radiusSquared)
00626                   for (unsigned int j = 0 ; out == OUTSIDE && j < bs ; ++j)
00627                     if (bodies_[j].body->containsPoint(pt)) {
00628                       if(print) std::cout << "Point " << i << " in scaled body part " << bodies_[j].name << std::endl;
00629                       out = INSIDE;
00630                     }
00631               }
00632             }
00633             mask[i] = out;
00634           }
00635         }
00636         
00637         tf::TransformListener              &tf_;
00638         ros::NodeHandle                     nh_;
00639         
00640         tf::Vector3                           sensor_pos_;
00641         double                              min_sensor_dist_;
00642         
00643         std::vector<SeeLink>                bodies_;
00644         std::vector<double>                 bspheresRadius2_;
00645         std::vector<bodies::BoundingSphere> bspheres_;
00646         
00647     };
00648     
00649 }
00650 
00651 #endif


pr2_navigation_self_filter
Author(s): Eitan Marder-Eppstein
autogenerated on Fri Apr 5 2019 02:18:37