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