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