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 #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 tf::Transform urdfPose2TFTransform(const urdf::Pose &pose)
00059 {
00060 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));
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 tf::Vector3 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
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
00136
00137
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
00169
00170 sl.constTransf = urdfPose2TFTransform(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
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
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 tf::Vector3&)> &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 tf::Vector3 &sensor_pos, const double min_sensor_dist,
00241 std::vector<int> &mask, const boost::function<void(const tf::Vector3&)> &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 tf::Vector3 &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
00281
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
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
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
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
00335 bodies::BoundingSphere bound;
00336 bodies::mergeBoundingSpheres(bspheres_, bound);
00337 tfScalar radiusSquared = bound.radius * bound.radius;
00338
00339
00340
00341 for (int i = 0 ; i < (int)np ; ++i)
00342 {
00343 tf::Vector3 pt = tf::Vector3(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 tf::Vector3&)> &callback)
00355 {
00356 const unsigned int bs = bodies_.size();
00357 const unsigned int np = data_in.points.size();
00358
00359
00360 bodies::BoundingSphere bound;
00361 bodies::mergeBoundingSpheres(bspheres_, bound);
00362 tfScalar radiusSquared = bound.radius * bound.radius;
00363
00364
00365
00366
00367
00368 for (int i = 0 ; i < (int)np ; ++i)
00369 {
00370 bool print = false;
00371
00372 tf::Vector3 pt = tf::Vector3(data_in.points[i].x, data_in.points[i].y, data_in.points[i].z);
00373 int out = OUTSIDE;
00374
00375
00376
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
00387 if (out == OUTSIDE)
00388 {
00389
00390 tf::Vector3 dir(sensor_pos_ - pt);
00391 tfScalar lng = dir.length();
00392 if (lng < min_sensor_dist_)
00393 {
00394 out = INSIDE;
00395
00396 }
00397 else
00398 {
00399 dir /= lng;
00400 std::vector<tf::Vector3> 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
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 tf::Vector3 &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(tf::Vector3(x, y, z));
00442 }
00443
00444 int robot_self_filter_color::SelfMask::getMaskIntersection(const tf::Vector3 &pt, const boost::function<void(const tf::Vector3&)> &callback) const
00445 {
00446 const unsigned int bs = bodies_.size();
00447
00448
00449
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
00458 tf::Vector3 dir(sensor_pos_ - pt);
00459 tfScalar lng = dir.length();
00460 if (lng < min_sensor_dist_)
00461 out = INSIDE;
00462 else
00463 {
00464 dir /= lng;
00465
00466 std::vector<tf::Vector3> 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
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 tf::Vector3&)> &callback) const
00488 {
00489 return getMaskIntersection(tf::Vector3(x, y, z), callback);
00490 }