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
00031
00032
00033
00034
00035
00036
00037 #include <moveit/distance_field/distance_field.h>
00038 #include <moveit/distance_field/distance_field_common.h>
00039 #include <geometric_shapes/body_operations.h>
00040 #include <eigen_conversions/eigen_msg.h>
00041 #include <console_bridge/console.h>
00042 #include <octomap/octomap.h>
00043 #include <octomap/OcTree.h>
00044
00045 namespace distance_field
00046 {
00047
00048 DistanceField::DistanceField(double size_x, double size_y, double size_z, double resolution,
00049 double origin_x, double origin_y, double origin_z) :
00050 size_x_(size_x),
00051 size_y_(size_y),
00052 size_z_(size_z),
00053 origin_x_(origin_x),
00054 origin_y_(origin_y),
00055 origin_z_(origin_z),
00056 resolution_(resolution),
00057 inv_twice_resolution_(1.0/(2.0*resolution_))
00058 {
00059 }
00060
00061 DistanceField::~DistanceField()
00062 {
00063 }
00064
00065 double DistanceField::getDistanceGradient(double x, double y, double z, double& gradient_x, double& gradient_y, double& gradient_z,
00066 bool& in_bounds) const
00067 {
00068 int gx, gy, gz;
00069
00070 worldToGrid(x, y, z, gx, gy, gz);
00071
00072
00073
00074 if (gx<1 || gy<1 || gz<1 || gx>=getXNumCells()-1 || gy>=getYNumCells()-1 || gz>=getZNumCells()-1)
00075 {
00076 gradient_x = 0.0;
00077 gradient_y = 0.0;
00078 gradient_z = 0.0;
00079 in_bounds = false;
00080 return getUninitializedDistance();
00081 }
00082
00083 gradient_x = (getDistance(gx+1,gy,gz) - getDistance(gx-1,gy,gz))*inv_twice_resolution_;
00084 gradient_y = (getDistance(gx,gy+1,gz) - getDistance(gx,gy-1,gz))*inv_twice_resolution_;
00085 gradient_z = (getDistance(gx,gy,gz+1) - getDistance(gx,gy,gz-1))*inv_twice_resolution_;
00086
00087 in_bounds = true;
00088 return getDistance(gx,gy,gz);
00089 }
00090
00091 void DistanceField::getIsoSurfaceMarkers(double min_distance, double max_distance,
00092 const std::string & frame_id, const ros::Time stamp,
00093 visualization_msgs::Marker& inf_marker) const
00094 {
00095 inf_marker.points.clear();
00096 inf_marker.header.frame_id = frame_id;
00097 inf_marker.header.stamp = stamp;
00098 inf_marker.ns = "distance_field";
00099 inf_marker.id = 1;
00100 inf_marker.type = visualization_msgs::Marker::CUBE_LIST;
00101 inf_marker.action = visualization_msgs::Marker::MODIFY;
00102 inf_marker.scale.x = resolution_;
00103 inf_marker.scale.y = resolution_;
00104 inf_marker.scale.z = resolution_;
00105 inf_marker.color.r = 1.0;
00106 inf_marker.color.g = 0.0;
00107 inf_marker.color.b = 0.0;
00108 inf_marker.color.a = 0.1;
00109
00110
00111 inf_marker.points.reserve(100000);
00112 for (int x = 0; x < getXNumCells(); ++x)
00113 {
00114 for (int y = 0; y < getYNumCells(); ++y)
00115 {
00116 for (int z = 0; z < getZNumCells(); ++z)
00117 {
00118 double dist = getDistance(x,y,z);
00119
00120 if (dist >= min_distance && dist <= max_distance)
00121 {
00122 int last = inf_marker.points.size();
00123 inf_marker.points.resize(last + 1);
00124 double nx, ny, nz;
00125 gridToWorld(x,y,z,
00126 nx, ny, nz);
00127 Eigen::Translation3d vec(nx,ny,nz);
00128 inf_marker.points[last].x = vec.x();
00129 inf_marker.points[last].y = vec.y();
00130 inf_marker.points[last].z = vec.z();
00131 }
00132 }
00133 }
00134 }
00135 }
00136
00137 void DistanceField::getGradientMarkers(double min_distance,
00138 double max_distance,
00139 const std::string& frame_id,
00140 const ros::Time& stamp,
00141 visualization_msgs::MarkerArray& marker_array) const
00142 {
00143 Eigen::Vector3d unitX(1, 0, 0);
00144 Eigen::Vector3d unitY(0, 1, 0);
00145 Eigen::Vector3d unitZ(0, 0, 1);
00146
00147 int id = 0;
00148
00149 for (int x = 0; x < getXNumCells(); ++x)
00150 {
00151 for (int y = 0; y < getYNumCells(); ++y)
00152 {
00153 for (int z = 0; z < getZNumCells(); ++z)
00154 {
00155 double worldX, worldY, worldZ;
00156 gridToWorld(x, y, z, worldX, worldY, worldZ);
00157
00158 double gradientX, gradientY, gradientZ;
00159 bool in_bounds;
00160 double distance = getDistanceGradient(worldX, worldY, worldZ, gradientX, gradientY, gradientZ, in_bounds);
00161 Eigen::Vector3d gradient(gradientX, gradientY, gradientZ);
00162
00163 if (in_bounds && distance >= min_distance && distance <= max_distance && gradient.norm() > 0)
00164 {
00165 visualization_msgs::Marker marker;
00166
00167 marker.header.frame_id = frame_id;
00168 marker.header.stamp = stamp;
00169
00170 marker.ns = "distance_field_gradient";
00171 marker.id = id++;
00172 marker.type = visualization_msgs::Marker::ARROW;
00173 marker.action = visualization_msgs::Marker::ADD;
00174
00175 marker.pose.position.x = worldX;
00176 marker.pose.position.y = worldY;
00177 marker.pose.position.z = worldZ;
00178
00179 Eigen::Vector3d axis = gradient.cross(unitX).norm() > 0 ? gradient.cross(unitX) : unitY;
00180
00181
00182
00183
00184
00185
00186
00187
00188 marker.scale.x = getResolution();
00189 marker.scale.y = getResolution();
00190 marker.scale.z = getResolution();
00191
00192 marker.color.r = 0.0;
00193 marker.color.g = 0.0;
00194 marker.color.b = 1.0;
00195 marker.color.a = 1.0;
00196
00197 marker_array.markers.push_back(marker);
00198 }
00199 }
00200 }
00201 }
00202 }
00203
00204 void DistanceField::addShapeToField(const shapes::Shape* shape,
00205 const geometry_msgs::Pose& pose)
00206 {
00207 if(shape->type == shapes::OCTREE) {
00208 const shapes::OcTree* oc = dynamic_cast<const shapes::OcTree*>(shape);
00209 if(!oc) {
00210 logError("Problem dynamic casting shape that claims to be OcTree");
00211 return;
00212 }
00213 addOcTreeToField(oc->octree.get());
00214 } else {
00215 bodies::Body* body = bodies::createBodyFromShape(shape);
00216 Eigen::Affine3d pose_e;
00217 tf::poseMsgToEigen(pose, pose_e);
00218 body->setPose(pose_e);
00219 EigenSTL::vector_Vector3d point_vec = determineCollisionPoints(body, resolution_);
00220 delete body;
00221 addPointsToField(point_vec);
00222 }
00223 }
00224
00225 void DistanceField::addOcTreeToField(const octomap::OcTree* octree)
00226 {
00227
00228 double min_x, min_y, min_z;
00229 gridToWorld(0,0,0,
00230 min_x, min_y, min_z);
00231
00232 octomap::point3d bbx_min(min_x, min_y, min_z);
00233
00234 int num_x = getXNumCells();
00235 int num_y = getYNumCells();
00236 int num_z = getZNumCells();
00237
00238
00239 double max_x, max_y, max_z;
00240 gridToWorld(num_x, num_y, num_z,
00241 max_x, max_y, max_z);
00242
00243 octomap::point3d bbx_max(max_x, max_y, max_z);
00244
00245 EigenSTL::vector_Vector3d points;
00246
00247 for(octomap::OcTree::leaf_bbx_iterator it = octree->begin_leafs_bbx(bbx_min,bbx_max),
00248 end=octree->end_leafs_bbx(); it!= end; ++it)
00249 {
00250 if (octree->isNodeOccupied(*it))
00251 {
00252 if(it.getSize() <= resolution_) {
00253 Eigen::Vector3d point(it.getX(), it.getY(), it.getZ());
00254 points.push_back(point);
00255 } else {
00256 double ceil_val = ceil(it.getSize()/resolution_)*resolution_;
00257 for(double x = it.getX()-ceil_val; x < it.getX()+ceil_val; x += resolution_) {
00258 for(double y = it.getY()-ceil_val; y < it.getY()+ceil_val; y += resolution_) {
00259 for(double z = it.getZ()-ceil_val; z < it.getZ()+ceil_val; z += resolution_) {
00260 points.push_back(Eigen::Vector3d(x,y,z));
00261 }
00262 }
00263 }
00264 }
00265 }
00266 }
00267 addPointsToField(points);
00268 }
00269
00270 void DistanceField::moveShapeInField(const shapes::Shape* shape,
00271 const geometry_msgs::Pose& old_pose,
00272 const geometry_msgs::Pose& new_pose)
00273 {
00274 if(shape->type == shapes::OCTREE) {
00275 logWarn("Move shape not supported for Octree");
00276 return;
00277 }
00278 bodies::Body* body = bodies::createBodyFromShape(shape);
00279 Eigen::Affine3d old_pose_e;
00280 tf::poseMsgToEigen(old_pose, old_pose_e);
00281 body->setPose(old_pose_e);
00282 EigenSTL::vector_Vector3d old_point_vec = determineCollisionPoints(body, resolution_);
00283 Eigen::Affine3d new_pose_e;
00284 tf::poseMsgToEigen(new_pose, new_pose_e);
00285 body->setPose(new_pose_e);
00286 EigenSTL::vector_Vector3d new_point_vec = determineCollisionPoints(body, resolution_);
00287 delete body;
00288 updatePointsInField(old_point_vec,
00289 new_point_vec);
00290 }
00291
00292 void DistanceField::removeShapeFromField(const shapes::Shape* shape,
00293 const geometry_msgs::Pose& pose)
00294 {
00295 bodies::Body* body = bodies::createBodyFromShape(shape);
00296 Eigen::Affine3d pose_e;
00297 tf::poseMsgToEigen(pose, pose_e);
00298 body->setPose(pose_e);
00299 EigenSTL::vector_Vector3d point_vec = determineCollisionPoints(body, resolution_);
00300 delete body;
00301 removePointsFromField(point_vec);
00302 }
00303
00304 void DistanceField::getPlaneMarkers(distance_field::PlaneVisualizationType type, double length, double width,
00305 double height, const Eigen::Vector3d& origin,
00306 const std::string & frame_id, const ros::Time stamp,
00307 visualization_msgs::Marker& plane_marker ) const
00308 {
00309 plane_marker.header.frame_id = frame_id;
00310 plane_marker.header.stamp = stamp;
00311 plane_marker.ns = "distance_field_plane";
00312 plane_marker.id = 1;
00313 plane_marker.type = visualization_msgs::Marker::CUBE_LIST;
00314 plane_marker.action = visualization_msgs::Marker::ADD;
00315 plane_marker.scale.x = resolution_;
00316 plane_marker.scale.y = resolution_;
00317 plane_marker.scale.z = resolution_;
00318
00319
00320 plane_marker.points.reserve(100000);
00321 plane_marker.colors.reserve(100000);
00322
00323 double minX = 0;
00324 double maxX = 0;
00325 double minY = 0;
00326 double maxY = 0;
00327 double minZ = 0;
00328 double maxZ = 0;
00329
00330 switch(type)
00331 {
00332 case XYPlane:
00333 minZ = height;
00334 maxZ = height;
00335
00336 minX = -length/2.0;
00337 maxX = length/2.0;
00338 minY = -width/2.0;
00339 maxY = width/2.0;
00340 break;
00341 case XZPlane:
00342 minY = height;
00343 maxY = height;
00344
00345 minX = -length/2.0;
00346 maxX = length/2.0;
00347 minZ = -width/2.0;
00348 maxZ = width/2.0;
00349 break;
00350 case YZPlane:
00351 minX = height;
00352 maxX = height;
00353
00354 minY = -length/2.0;
00355 maxY = length/2.0;
00356 minZ = -width/2.0;
00357 maxZ = width/2.0;
00358 break;
00359 }
00360
00361 minX += origin.x();
00362 minY += origin.y();
00363 minZ += origin.z();
00364
00365 maxX += origin.x();
00366 maxY += origin.y();
00367 maxZ += origin.z();
00368
00369 int minXCell = 0;
00370 int maxXCell = 0;
00371 int minYCell = 0;
00372 int maxYCell = 0;
00373 int minZCell = 0;
00374 int maxZCell = 0;
00375
00376 worldToGrid(minX,minY,minZ, minXCell, minYCell, minZCell);
00377 worldToGrid(maxX,maxY,maxZ, maxXCell, maxYCell, maxZCell);
00378 plane_marker.color.a = 1.0;
00379 for(int x = minXCell; x <= maxXCell; ++x)
00380 {
00381 for(int y = minYCell; y <= maxYCell; ++y)
00382 {
00383 for(int z = minZCell; z <= maxZCell; ++z)
00384 {
00385 if(!isCellValid(x,y,z))
00386 {
00387 continue;
00388 }
00389 double dist = getDistance(x, y, z);
00390 int last = plane_marker.points.size();
00391 plane_marker.points.resize(last + 1);
00392 plane_marker.colors.resize(last + 1);
00393 double nx, ny, nz;
00394 gridToWorld(x, y, z, nx, ny, nz);
00395 Eigen::Vector3d vec(nx, ny, nz);
00396 plane_marker.points[last].x = vec.x();
00397 plane_marker.points[last].y = vec.y();
00398 plane_marker.points[last].z = vec.z();
00399 if(dist < 0.0)
00400 {
00401 plane_marker.colors[last].r = fmax(fmin(0.1/fabs(dist), 1.0), 0.0);
00402 plane_marker.colors[last].g = fmax(fmin(0.05/fabs(dist), 1.0), 0.0);
00403 plane_marker.colors[last].b = fmax(fmin(0.01/fabs(dist), 1.0), 0.0);
00404
00405 }
00406 else
00407 {
00408 plane_marker.colors[last].b = fmax(fmin(0.1/(dist+0.001), 1.0),0.0);
00409 plane_marker.colors[last].g = fmax(fmin(0.05/(dist+0.001), 1.0),0.0);
00410 plane_marker.colors[last].r = fmax(fmin(0.01/(dist+0.001), 1.0),0.0);
00411 }
00412 }
00413 }
00414 }
00415 }
00416
00417
00418
00419 void DistanceField::setPoint(int xCell, int yCell, int zCell,
00420 double dist, geometry_msgs::Point& point,
00421 std_msgs::ColorRGBA& color,
00422 double max_distance) const
00423 {
00424 double wx,wy,wz;
00425 gridToWorld(xCell,yCell,zCell, wx,wy,wz);
00426
00427 point.x = wx;
00428 point.y = wy;
00429 point.z = wz;
00430
00431 color.r = 1.0;
00432 color.g = dist/max_distance;
00433 color.b = dist/max_distance;
00434 }
00435
00436
00437 void DistanceField::getProjectionPlanes(const std::string& frame_id,
00438 const ros::Time& stamp,
00439 double max_dist,
00440 visualization_msgs::Marker& marker) const
00441 {
00442 int maxXCell = getXNumCells();
00443 int maxYCell = getYNumCells();
00444 int maxZCell = getZNumCells();
00445
00446 double * x_projection = new double[maxYCell*maxZCell];
00447 double * y_projection = new double[maxZCell*maxXCell];
00448 double * z_projection = new double[maxXCell*maxYCell];
00449 double initial_val = sqrt(INT_MAX);
00450
00451
00452 for( int y = 0; y < maxYCell; y++ )
00453 for( int x = 0; x < maxXCell; x++ )
00454 z_projection[x+y*maxXCell] = initial_val;
00455
00456 for( int z = 0; z < maxZCell; z++ )
00457 for( int y = 0; y < maxYCell; y++ )
00458 x_projection[y+z*maxYCell] = initial_val;
00459
00460 for( int z = 0; z < maxZCell; z++ )
00461 for( int x = 0; x < maxXCell; x++ )
00462 y_projection[x+z*maxXCell] = initial_val;
00463
00464
00465 for( int z = 0; z < maxZCell; z++ ) {
00466 for( int y = 0; y < maxYCell; y++ ) {
00467 for( int x = 0; x < maxXCell; x++ ) {
00468 double dist = getDistance(x,y,z);
00469 z_projection[x+y*maxXCell] = std::min( dist, z_projection[x+y*maxXCell]);
00470 x_projection[y+z*maxYCell] = std::min( dist, x_projection[y+z*maxYCell]);
00471 y_projection[x+z*maxXCell] = std::min( dist, y_projection[x+z*maxXCell]);
00472 }
00473 }
00474 }
00475
00476
00477 marker.points.clear();
00478 marker.header.frame_id = frame_id;
00479 marker.header.stamp = stamp;
00480 marker.ns = "distance_field_projection_plane";
00481 marker.id = 1;
00482 marker.type = visualization_msgs::Marker::CUBE_LIST;
00483 marker.action = visualization_msgs::Marker::MODIFY;
00484 marker.scale.x = getResolution();
00485 marker.scale.y = getResolution();
00486 marker.scale.z = getResolution();
00487 marker.color.a = 1.0;
00488
00489
00490 int x, y, z;
00491 int index = 0;
00492 marker.points.resize(maxXCell*maxYCell + maxYCell*maxZCell + maxZCell*maxXCell);
00493 marker.colors.resize(maxXCell*maxYCell + maxYCell*maxZCell + maxZCell*maxXCell);
00494
00495 z = 0;
00496 for( y = 0; y < maxYCell; y++ ) {
00497 for( x = 0; x < maxXCell; x++ ) {
00498 double dist = z_projection[x+y*maxXCell];
00499 setPoint( x, y, z, dist, marker.points[index], marker.colors[index], max_dist );
00500 index++;
00501 }
00502 }
00503
00504 x = 0;
00505 for( z = 0; z < maxZCell; z++ ) {
00506 for( y = 0; y < maxYCell; y++ ) {
00507 double dist = x_projection[y+z*maxYCell];
00508 setPoint( x, y, z, dist, marker.points[index], marker.colors[index], max_dist );
00509 index++;
00510 }
00511 }
00512
00513 y = 0;
00514 for( z = 0; z < maxZCell; z++ ) {
00515 for( x = 0; x < maxXCell; x++ ) {
00516 double dist = y_projection[x+z*maxXCell];
00517 setPoint( x, y, z, dist, marker.points[index], marker.colors[index], max_dist );
00518 index++;
00519 }
00520 }
00521
00522 if( x_projection)
00523 delete[] x_projection;
00524 if( y_projection )
00525 delete[] y_projection;
00526 if( z_projection )
00527 delete[] z_projection;
00528 }
00529
00530
00531
00532 }