00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
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
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
00142
00143
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
00175
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
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
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
00287
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
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
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
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
00341 bodies::BoundingSphere bound;
00342 bodies::mergeBoundingSpheres(bspheres_, bound);
00343 tfScalar radiusSquared = bound.radius * bound.radius;
00344
00345
00346
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
00366 bodies::BoundingSphere bound;
00367 bodies::mergeBoundingSpheres(bspheres_, bound);
00368 tfScalar radiusSquared = bound.radius * bound.radius;
00369
00370
00371
00372
00373
00374 for (int i = 0 ; i < (int)np ; ++i)
00375 {
00376 bool print = false;
00377
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
00382
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
00393 if (out == OUTSIDE)
00394 {
00395
00396 tf::Vector3 dir(sensor_pos_ - pt);
00397 tfScalar lng = dir.length();
00398 if (lng < min_sensor_dist_)
00399 {
00400 out = INSIDE;
00401
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
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
00455
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
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
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 }