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