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