self_mask.cpp
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 #include "robot_self_filter/self_mask.h"
00031 #include <urdf/model.h>
00032 #include <resource_retriever/retriever.h>
00033 #include <geometric_shapes/shape_operations.h>
00034 #include <ros/console.h>
00035 #include <algorithm>
00036 #include <sstream>
00037 #include <climits>
00038 
00039 #if defined(IS_ASSIMP3)
00040 #include <assimp/scene.h>
00041 #include <assimp/Importer.hpp>
00042 #include <assimp/postprocess.h>
00043 #else
00044 #include <assimp/aiScene.h>
00045 #include <assimp/assimp.hpp>
00046 #include <assimp/aiPostProcess.h>
00047 #endif
00048 
00049 void robot_self_filter::SelfMask::freeMemory (void)
00050 {
00051   for (unsigned int i = 0 ; i < bodies_.size() ; ++i)
00052   {
00053     if (bodies_[i].body)
00054             delete bodies_[i].body;
00055     if (bodies_[i].unscaledBody)
00056             delete bodies_[i].unscaledBody;
00057   }
00058     
00059   bodies_.clear ();
00060 }
00061 
00062 
00063 namespace robot_self_filter
00064 {
00065     static inline tf::Transform urdfPose2TFTransform(const urdf::Pose &pose)
00066     {
00067       return tf::Transform(tf::Quaternion(pose.rotation.x, pose.rotation.y, pose.rotation.z, pose.rotation.w), 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         {
00079           result = new shapes::Sphere(dynamic_cast<const urdf::Sphere*>(geom)->radius);
00080           break;
00081         }
00082         case urdf::Geometry::BOX:
00083         {
00084           urdf::Vector3 dim = dynamic_cast<const urdf::Box*>(geom)->dim;
00085           result = new shapes::Box(dim.x, dim.y, dim.z);
00086           break;
00087         }
00088         case urdf::Geometry::CYLINDER:
00089         {
00090           result = new shapes::Cylinder(dynamic_cast<const urdf::Cylinder*>(geom)->radius,
00091                   dynamic_cast<const urdf::Cylinder*>(geom)->length);
00092           break;
00093         }
00094         case urdf::Geometry::MESH:
00095         {
00096           const urdf::Mesh *mesh = dynamic_cast<const urdf::Mesh*>(geom);
00097           if (!mesh->filename.empty())
00098           {
00099             tf::Vector3 scale(mesh->scale.x, mesh->scale.y, mesh->scale.z);
00100             result = shapes::createMeshFromFilename(mesh->filename, &scale);
00101           } else
00102             ROS_WARN("Empty mesh filename");
00103           break;
00104         }
00105       default:
00106         {
00107           ROS_ERROR("Unknown geometry type: %d", (int)geom->type);
00108           break;
00109         }
00110       }
00111       return (result);
00112     }
00113 }
00114 
00115 bool robot_self_filter::SelfMask::configure(const std::vector<LinkInfo> &links)
00116 {
00117   // in case configure was called before, we free the memory
00118   freeMemory();
00119   sensor_pos_.setValue(0, 0, 0);
00120     
00121   std::string content;
00122   boost::shared_ptr<urdf::Model> urdfModel;
00123 
00124   if (nh_.getParam("robot_description", content))
00125   {
00126     urdfModel = boost::shared_ptr<urdf::Model>(new urdf::Model());
00127     if (!urdfModel->initString(content))
00128     {
00129         ROS_ERROR("Unable to parse URDF description!");
00130         return false;
00131     }
00132   }
00133   else
00134   {
00135     ROS_ERROR("Robot model not found! Did you remap 'robot_description'?");
00136     return false;
00137   }
00138     
00139   std::stringstream missing;
00140   
00141   // from the geometric model, find the shape of each link of interest
00142   // and create a body from it, one that knows about poses and can 
00143   // check for point inclusion
00144   for (unsigned int i = 0 ; i < links.size() ; ++i)
00145   {
00146     const urdf::Link *link = urdfModel->getLink(links[i].name).get();
00147     if (!link)
00148     {
00149       missing << " " << links[i].name;
00150       continue;
00151     }
00152     
00153     if (!(link->collision && link->collision->geometry))
00154     {
00155         ROS_WARN("No collision geometry specified for link '%s'", links[i].name.c_str());
00156         continue;
00157     }
00158     
00159     shapes::Shape *shape = constructShape(link->collision->geometry.get());
00160     
00161     if (!shape)
00162     {
00163         ROS_ERROR("Unable to construct collision shape for link '%s'", links[i].name.c_str());
00164         continue;
00165     }
00166         
00167     SeeLink sl;
00168     sl.body = bodies::createBodyFromShape(shape);
00169 
00170     if (sl.body)
00171     {
00172       sl.name = links[i].name;
00173       
00174       // collision models may have an offset, in addition to what TF gives
00175       // so we keep it around
00176       sl.constTransf = urdfPose2TFTransform(link->collision->origin);
00177 
00178       sl.body->setScale(links[i].scale);
00179       sl.body->setPadding(links[i].padding);
00180             ROS_DEBUG_STREAM("Self see link name " <<  links[i].name << " padding " << links[i].padding);
00181       sl.volume = sl.body->computeVolume();
00182       sl.unscaledBody = bodies::createBodyFromShape(shape);
00183       bodies_.push_back(sl);
00184     }
00185     else
00186       ROS_WARN("Unable to create point inclusion body for link '%s'", links[i].name.c_str());
00187     
00188     delete shape;
00189   }
00190     
00191   if (missing.str().size() > 0)
00192     ROS_WARN("Some links were included for self mask but they do not exist in the model:%s", missing.str().c_str());
00193     
00194   if (bodies_.empty())
00195     ROS_WARN("No robot links will be checked for self mask");
00196     
00197   // put larger volume bodies first -- higher chances of containing a point
00198   std::sort(bodies_.begin(), bodies_.end(), SortBodies());
00199   
00200   bspheres_.resize(bodies_.size());
00201   bspheresRadius2_.resize(bodies_.size());
00202 
00203   for (unsigned int i = 0 ; i < bodies_.size() ; ++i)
00204     ROS_DEBUG("Self mask includes link %s with volume %f", bodies_[i].name.c_str(), bodies_[i].volume);
00205     
00206   //ROS_INFO("Self filter using %f padding and %f scaling", padd, scale);
00207 
00208   return true; 
00209 }
00210 
00211 void robot_self_filter::SelfMask::getLinkNames(std::vector<std::string> &frames) const
00212 {
00213   for (unsigned int i = 0 ; i < bodies_.size() ; ++i)
00214     frames.push_back(bodies_[i].name);
00215 }
00216 
00217 void robot_self_filter::SelfMask::maskContainment(const pcl::PointCloud<pcl::PointXYZ>& data_in, std::vector<int> &mask)
00218 {
00219   mask.resize(data_in.points.size());
00220   if (bodies_.empty())
00221     std::fill(mask.begin(), mask.end(), (int)OUTSIDE);
00222   else
00223   {
00224       assumeFrame(data_in.header.frame_id,ros::Time(data_in.header.stamp));
00225     maskAuxContainment(data_in, mask);
00226   }
00227 }
00228 
00229 void robot_self_filter::SelfMask::maskIntersection(const pcl::PointCloud<pcl::PointXYZ>& data_in, const std::string &sensor_frame, const double min_sensor_dist,
00230                                                    std::vector<int> &mask, const boost::function<void(const tf::Vector3&)> &callback)
00231 {
00232   mask.resize(data_in.points.size());
00233   if (bodies_.empty()) {
00234     std::fill(mask.begin(), mask.end(), (int)OUTSIDE);
00235   }
00236   else
00237   {
00238       assumeFrame(data_in.header.frame_id, ros::Time(data_in.header.stamp), sensor_frame, min_sensor_dist);
00239     if (sensor_frame.empty())
00240         maskAuxContainment(data_in, mask);
00241     else
00242         maskAuxIntersection(data_in, mask, callback);
00243   }
00244 }
00245 
00246 void robot_self_filter::SelfMask::maskIntersection(const pcl::PointCloud<pcl::PointXYZ>& data_in, const tf::Vector3 &sensor_pos, const double min_sensor_dist,
00247                                                    std::vector<int> &mask, const boost::function<void(const tf::Vector3&)> &callback)
00248 {
00249   mask.resize(data_in.points.size());
00250   if (bodies_.empty())
00251     std::fill(mask.begin(), mask.end(), (int)OUTSIDE);
00252   else
00253   {
00254       assumeFrame(data_in.header.frame_id, ros::Time(data_in.header.stamp), sensor_pos, min_sensor_dist);
00255     maskAuxIntersection(data_in, mask, callback);
00256   }
00257 }
00258 
00259 void robot_self_filter::SelfMask::computeBoundingSpheres(void)
00260 {
00261   const unsigned int bs = bodies_.size();
00262   for (unsigned int i = 0 ; i < bs ; ++i)
00263   {
00264     bodies_[i].body->computeBoundingSphere(bspheres_[i]);
00265     bspheresRadius2_[i] = bspheres_[i].radius * bspheres_[i].radius;
00266   }
00267 }
00268 
00269 void robot_self_filter::SelfMask::assumeFrame(const std::string& frame_id, const ros::Time& stamp, const tf::Vector3 &sensor_pos, double min_sensor_dist)
00270 {
00271     assumeFrame(frame_id,ros::Time(stamp));
00272   sensor_pos_ = sensor_pos;
00273   min_sensor_dist_ = min_sensor_dist;
00274 }
00275 
00276 void robot_self_filter::SelfMask::assumeFrame(const std::string& frame_id, const ros::Time& stamp, const std::string &sensor_frame, double min_sensor_dist)
00277 {
00278     assumeFrame(frame_id,ros::Time(stamp));
00279 
00280   std::string err;
00281   if(!tf_.waitForTransform(frame_id, sensor_frame, stamp, ros::Duration(.1), ros::Duration(.01), &err)) {
00282     ROS_ERROR("WaitForTransform timed out from %s to %s after 100ms.  Error string: %s", sensor_frame.c_str(), frame_id.c_str(), err.c_str());
00283     sensor_pos_.setValue(0, 0, 0);
00284   } 
00285 
00286   //transform should be there
00287   // compute the origin of the sensor in the frame of the cloud
00288   try
00289   {
00290     tf::StampedTransform transf;
00291     tf_.lookupTransform(frame_id, sensor_frame, stamp, transf);
00292     sensor_pos_ = transf.getOrigin();
00293   }
00294   catch(tf::TransformException& ex)
00295   {
00296     sensor_pos_.setValue(0, 0, 0);
00297     ROS_ERROR("Unable to lookup transform from %s to %s.  Exception: %s", sensor_frame.c_str(), frame_id.c_str(), ex.what());
00298   }
00299   
00300   min_sensor_dist_ = min_sensor_dist;
00301 }
00302 
00303 void robot_self_filter::SelfMask::assumeFrame(const std::string &frame_id, const ros::Time &stamp)
00304 {
00305   const unsigned int bs = bodies_.size();
00306   
00307   // place the links in the assumed frame 
00308   for (unsigned int i = 0 ; i < bs ; ++i)
00309   {
00310     std::string err;
00311     if(!tf_.waitForTransform(frame_id, bodies_[i].name, stamp, ros::Duration(.1), ros::Duration(.01), &err)) {
00312       ROS_ERROR("WaitForTransform timed out from %s to %s after 100ms.  Error string: %s", bodies_[i].name.c_str(), frame_id.c_str(), err.c_str());      
00313     } 
00314     
00315     // find the transform between the link's frame and the pointcloud frame
00316     tf::StampedTransform transf;
00317     try
00318     {
00319       tf_.lookupTransform(frame_id, bodies_[i].name, stamp, transf);
00320     }
00321     catch(tf::TransformException& ex)
00322     {
00323       transf.setIdentity();
00324       ROS_ERROR("Unable to lookup transform from %s to %s. Exception: %s", bodies_[i].name.c_str(), frame_id.c_str(), ex.what());       
00325     }
00326     
00327     // set it for each body; we also include the offset specified in URDF
00328     bodies_[i].body->setPose(transf * bodies_[i].constTransf);
00329     bodies_[i].unscaledBody->setPose(transf * bodies_[i].constTransf);
00330   }
00331   
00332   computeBoundingSpheres();
00333 }
00334 
00335 void robot_self_filter::SelfMask::maskAuxContainment(const pcl::PointCloud<pcl::PointXYZ>& data_in, std::vector<int> &mask)
00336 {
00337     const unsigned int bs = bodies_.size();
00338     const unsigned int np = data_in.points.size();
00339     
00340     // compute a sphere that bounds the entire robot
00341     bodies::BoundingSphere bound;
00342     bodies::mergeBoundingSpheres(bspheres_, bound);       
00343     tfScalar radiusSquared = bound.radius * bound.radius;
00344     
00345     // we now decide which points we keep
00346     //#pragma omp parallel for schedule(dynamic) 
00347     for (int i = 0 ; i < (int)np ; ++i)
00348     {
00349       tf::Vector3 pt = tf::Vector3(data_in.points[i].x, data_in.points[i].y, data_in.points[i].z);
00350       int out = OUTSIDE;
00351       if (bound.center.distance2(pt) < radiusSquared)
00352           for (unsigned int j = 0 ; out == OUTSIDE && j < bs ; ++j)
00353         if (bodies_[j].body->containsPoint(pt))
00354             out = INSIDE;
00355       
00356       mask[i] = out;
00357     }
00358 }
00359 
00360 void robot_self_filter::SelfMask::maskAuxIntersection(const pcl::PointCloud<pcl::PointXYZ>& data_in, std::vector<int> &mask, const boost::function<void(const tf::Vector3&)> &callback)
00361 {
00362   const unsigned int bs = bodies_.size();
00363   const unsigned int np = data_in.points.size();
00364   
00365   // compute a sphere that bounds the entire robot
00366   bodies::BoundingSphere bound;
00367   bodies::mergeBoundingSpheres(bspheres_, bound);         
00368   tfScalar radiusSquared = bound.radius * bound.radius;
00369 
00370   //std::cout << "Testing " << np << " points\n";
00371 
00372   // we now decide which points we keep
00373   //#pragma omp parallel for schedule(dynamic) 
00374   for (int i = 0 ; i < (int)np ; ++i)
00375   {
00376     bool print = false;
00377     //if(i%100 == 0) print = true;
00378     tf::Vector3 pt = tf::Vector3(data_in.points[i].x, data_in.points[i].y, data_in.points[i].z);
00379     int out = OUTSIDE;
00380 
00381     // we first check is the point is in the unscaled body. 
00382     // if it is, the point is definitely inside
00383     if (bound.center.distance2(pt) < radiusSquared)
00384       for (unsigned int j = 0 ; out == OUTSIDE && j < bs ; ++j)
00385         if (bodies_[j].unscaledBody->containsPoint(pt)) 
00386         {
00387           if(print)
00388           std::cout << "Point " << i << " in unscaled body part " << bodies_[j].name << std::endl;
00389         out = INSIDE;
00390         }
00391 
00392         // if the point is not inside the unscaled body,
00393         if (out == OUTSIDE)
00394         {
00395           // we check it the point is a shadow point 
00396           tf::Vector3 dir(sensor_pos_ - pt);
00397           tfScalar  lng = dir.length();
00398           if (lng < min_sensor_dist_) 
00399           {
00400             out = INSIDE;
00401             //std::cout << "Point " << i << " less than min sensor distance away\n";
00402           }
00403           else
00404           {             
00405             dir /= lng;
00406             std::vector<tf::Vector3> intersections;
00407             for (unsigned int j = 0 ; out == OUTSIDE && j < bs ; ++j) 
00408             {
00409               if (bodies_[j].body->intersectsRay(pt, dir, &intersections, 1))
00410               {
00411                 if (dir.dot(sensor_pos_ - intersections[0]) >= 0.0)
00412                 {
00413                   if (callback)
00414                     callback(intersections[0]);
00415                   out = SHADOW;
00416                   if(print) std::cout << "Point " << i << " shadowed by body part " << bodies_[j].name << std::endl;
00417                 }
00418              }
00419                        }
00420            // if it is not a shadow point, we check if it is inside the scaled body
00421            if (out == OUTSIDE && bound.center.distance2(pt) < radiusSquared)
00422              for (unsigned int j = 0 ; out == OUTSIDE && j < bs ; ++j)
00423                if (bodies_[j].body->containsPoint(pt)) 
00424                {
00425                  if(print) 
00426                    std::cout << "Point " << i << " in scaled body part " << bodies_[j].name << std::endl;
00427                  out = INSIDE;
00428                }
00429           }
00430         }
00431         mask[i] = out;
00432   }
00433 }
00434 
00435 int robot_self_filter::SelfMask::getMaskContainment(const tf::Vector3 &pt) const
00436 {
00437   const unsigned int bs = bodies_.size();
00438   int out = OUTSIDE;
00439   for (unsigned int j = 0 ; out == OUTSIDE && j < bs ; ++j)
00440     if (bodies_[j].body->containsPoint(pt))
00441             out = INSIDE;
00442   return out;
00443 }
00444 
00445 int robot_self_filter::SelfMask::getMaskContainment(double x, double y, double z) const
00446 {
00447   return getMaskContainment(tf::Vector3(x, y, z));
00448 }
00449 
00450 int robot_self_filter::SelfMask::getMaskIntersection(const tf::Vector3 &pt, const boost::function<void(const tf::Vector3&)> &callback) const
00451 {  
00452   const unsigned int bs = bodies_.size();
00453 
00454   // we first check is the point is in the unscaled body. 
00455   // if it is, the point is definitely inside
00456   int out = OUTSIDE;
00457   for (unsigned int j = 0 ; out == OUTSIDE && j < bs ; ++j)
00458     if (bodies_[j].unscaledBody->containsPoint(pt))
00459       out = INSIDE;
00460   
00461   if (out == OUTSIDE)
00462   {
00463     // we check it the point is a shadow point 
00464     tf::Vector3 dir(sensor_pos_ - pt);
00465     tfScalar  lng = dir.length();
00466     if (lng < min_sensor_dist_)
00467         out = INSIDE;
00468     else
00469     {
00470       dir /= lng;
00471       
00472       std::vector<tf::Vector3> intersections;
00473       for (unsigned int j = 0 ; out == OUTSIDE && j < bs ; ++j)
00474         if (bodies_[j].body->intersectsRay(pt, dir, &intersections, 1))
00475         {
00476           if (dir.dot(sensor_pos_ - intersections[0]) >= 0.0)
00477           {
00478             if (callback)
00479               callback(intersections[0]);
00480             out = SHADOW;
00481           }
00482         }
00483         
00484         // if it is not a shadow point, we check if it is inside the scaled body
00485         for (unsigned int j = 0 ; out == OUTSIDE && j < bs ; ++j)
00486           if (bodies_[j].body->containsPoint(pt))
00487             out = INSIDE;
00488     }
00489   }
00490   return (out);
00491 }
00492 
00493 int robot_self_filter::SelfMask::getMaskIntersection(double x, double y, double z, const boost::function<void(const tf::Vector3&)> &callback) const
00494 {
00495   return getMaskIntersection(tf::Vector3(x, y, z), callback);
00496 }


robot_self_filter
Author(s): Ioan Sucan
autogenerated on Mon Dec 2 2013 12:33:58