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