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