distance_field: distance_field: distance

00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2009, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00037 #ifndef DF_DISTANCE_FIELD_H_
00038 #define DF_DISTANCE_FIELD_H_
00039 
00040 #include <distance_field/voxel_grid.h>
00041 #include <LinearMath/btVector3.h>
00042 #include <LinearMath/btQuaternion.h>
00043 #include <LinearMath/btTransform.h>
00044 #include <vector>
00045 #include <list>
00046 #include <ros/ros.h>
00047 #include <visualization_msgs/Marker.h>
00048 
00049 #include <arm_navigation_msgs/CollisionMap.h>
00050 
00051 namespace distance_field
00052 {
00053   enum PlaneVisualizationType
00054   {
00055     XYPlane,
00056     XZPlane,
00057     YZPlane
00058   };
00070 template <typename T>
00071 class DistanceField: public VoxelGrid<T>
00072 {
00073 public:
00074 
00087   DistanceField(double size_x, double size_y, double size_z, double resolution,
00088       double origin_x, double origin_y, double origin_z, T default_object);
00089 
00090   virtual ~DistanceField();
00091 
00092 
00093 
00101   virtual void addPointsToField(const std::vector<btVector3> points)=0;
00102 
00106   void addCollisionMapToField(const arm_navigation_msgs::CollisionMap &collision_map);
00107 
00111   virtual void reset()=0;
00112 
00116   double getDistance(double x, double y, double z) const;
00117 
00121   double getDistanceGradient(double x, double y, double z, double& gradient_x, double& gradient_y, double& gradient_z) const;
00122 
00126   double getDistanceFromCell(int x, int y, int z) const;
00127 
00134   void visualize(double min_radius, double max_radius, std::string frame_id, const btTransform& cur, ros::Time stamp);
00135 
00141   void visualizeGradient(double min_radius, double max_radius, std::string frame_id, ros::Time stamp);
00142 
00152   void visualizePlane(PlaneVisualizationType type, double length, double width, double height, btVector3 origin, std::string frame_id, ros::Time stamp);
00153 
00154 protected:
00155   virtual double getDistance(const T& object) const=0;
00156 
00157 private:
00158   ros::Publisher pub_viz_;
00159   int inv_twice_resolution_;
00160 };
00161 
00163 
00164 template <typename T>
00165 DistanceField<T>::~DistanceField()
00166 {
00167 
00168 }
00169 
00170 template <typename T>
00171 DistanceField<T>::DistanceField(double size_x, double size_y, double size_z, double resolution,
00172     double origin_x, double origin_y, double origin_z, T default_object):
00173       VoxelGrid<T>(size_x, size_y, size_z, resolution, origin_x, origin_y, origin_z, default_object)
00174 {
00175   ros::NodeHandle node;
00176   pub_viz_ = node.advertise<visualization_msgs::Marker>("visualization_marker", 3);
00177   inv_twice_resolution_ = 1.0/(2.0*resolution);
00178 }
00179 
00180 template <typename T>
00181 double DistanceField<T>::getDistance(double x, double y, double z) const
00182 {
00183   return getDistance((*this)(x,y,z));
00184 }
00185 
00186 template <typename T>
00187 double DistanceField<T>::getDistanceGradient(double x, double y, double z, double& gradient_x, double& gradient_y, double& gradient_z) const
00188 {
00189   int gx, gy, gz;
00190 
00191   this->worldToGrid(x, y, z, gx, gy, gz);
00192 
00193   // if out of bounds, return 0 distance, and 0 gradient
00194   // we need extra padding of 1 to get gradients
00195   if (gx<1 || gy<1 || gz<1 || gx>=this->num_cells_[this->DIM_X]-1 || gy>=this->num_cells_[this->DIM_Y]-1 || gz>=this->num_cells_[this->DIM_Z]-1)
00196   {
00197     gradient_x = 0.0;
00198     gradient_y = 0.0;
00199     gradient_z = 0.0;
00200     return 0;
00201   }
00202 
00203   gradient_x = (getDistanceFromCell(gx+1,gy,gz) - getDistanceFromCell(gx-1,gy,gz))*inv_twice_resolution_;
00204   gradient_y = (getDistanceFromCell(gx,gy+1,gz) - getDistanceFromCell(gx,gy-1,gz))*inv_twice_resolution_;
00205   gradient_z = (getDistanceFromCell(gx,gy,gz+1) - getDistanceFromCell(gx,gy,gz-1))*inv_twice_resolution_;
00206 
00207   return getDistanceFromCell(gx,gy,gz);
00208 
00209 }
00210 
00211 template <typename T>
00212 double DistanceField<T>::getDistanceFromCell(int x, int y, int z) const
00213 {
00214   return getDistance(this->getCell(x,y,z));
00215 }
00216 
00217 template <typename T>
00218 void DistanceField<T>::visualize(double min_radius, double max_radius, std::string frame_id, 
00219                                  const btTransform& cur, ros::Time stamp)
00220 {
00221   visualization_msgs::Marker inf_marker; // Marker for the inflation
00222   inf_marker.header.frame_id = frame_id;
00223   inf_marker.header.stamp = stamp;
00224   inf_marker.ns = "distance_field";
00225   inf_marker.id = 1;
00226   inf_marker.type = visualization_msgs::Marker::CUBE_LIST;
00227   inf_marker.action = 0;
00228   inf_marker.scale.x = this->resolution_[VoxelGrid<T>::DIM_X];
00229   inf_marker.scale.y = this->resolution_[VoxelGrid<T>::DIM_Y];
00230   inf_marker.scale.z = this->resolution_[VoxelGrid<T>::DIM_Z];
00231   inf_marker.color.r = 1.0;
00232   inf_marker.color.g = 0.0;
00233   inf_marker.color.b = 0.0;
00234   inf_marker.color.a = 0.1;
00235   //inf_marker.lifetime = ros::Duration(30.0);
00236 
00237   inf_marker.points.reserve(100000);
00238   int num_total_cells =
00239     this->num_cells_[VoxelGrid<T>::DIM_X]*
00240     this->num_cells_[VoxelGrid<T>::DIM_Y]*
00241     this->num_cells_[VoxelGrid<T>::DIM_Z];
00242   for (int x = 0; x < this->num_cells_[VoxelGrid<T>::DIM_X]; ++x)
00243   {
00244     for (int y = 0; y < this->num_cells_[VoxelGrid<T>::DIM_Y]; ++y)
00245     {
00246       for (int z = 0; z < this->num_cells_[VoxelGrid<T>::DIM_Z]; ++z)
00247       {
00248         double dist = getDistanceFromCell(x,y,z);
00249         if (dist >= min_radius && dist <= max_radius)
00250         {
00251           int last = inf_marker.points.size();
00252           inf_marker.points.resize(last + 1);
00253           double nx, ny, nz;
00254           this->gridToWorld(x,y,z,
00255                             nx, ny, nz);
00256           btVector3 vec(nx,ny,nz);
00257           vec = cur*vec;
00258           inf_marker.points[last].x = vec.x();
00259           inf_marker.points[last].y = vec.y();
00260           inf_marker.points[last].z = vec.z();
00261         }
00262       }
00263     }
00264   }
00265   ROS_DEBUG("Publishing markers: %u/%d inflated", (unsigned int) inf_marker.points.size(), num_total_cells);
00266   pub_viz_.publish(inf_marker);
00267 }
00268 
00269 template <typename T>
00270 void DistanceField<T>::visualizeGradient(double min_radius, double max_radius, std::string frame_id, ros::Time stamp)
00271 {
00272   btVector3 unitX(1, 0, 0);
00273   btVector3 unitY(0, 1, 0);
00274   btVector3 unitZ(0, 0, 1);
00275 
00276   int id = 0;
00277 
00278   for (int x = 0; x < this->num_cells_[this->DIM_X]; ++x)
00279   {
00280     for (int y = 0; y < this->num_cells_[this->DIM_Y]; ++y)
00281     {
00282       for (int z = 0; z < this->num_cells_[this->DIM_Z]; ++z)
00283       {
00284         double worldX, worldY, worldZ;
00285         this->gridToWorld(x, y, z, worldX, worldY, worldZ);
00286 
00287         double gradientX, gradientY, gradientZ;
00288         double distance = getDistanceGradient(worldX, worldY, worldZ, gradientX, gradientY, gradientZ);
00289         btVector3 gradient(gradientX, gradientY, gradientZ);
00290 
00291         if (distance >= min_radius && distance <= max_radius && gradient.length() > 0)
00292         {
00293           visualization_msgs::Marker marker;
00294 
00295           marker.header.frame_id = frame_id;
00296           marker.header.stamp = stamp;
00297 
00298           marker.ns = "distance_field_gradient";
00299           marker.id = id++;
00300           marker.type = visualization_msgs::Marker::ARROW;
00301           marker.action = visualization_msgs::Marker::ADD;
00302 
00303           marker.pose.position.x = worldX;
00304           marker.pose.position.y = worldY;
00305           marker.pose.position.z = worldZ;
00306 
00307           btVector3 axis = gradient.cross(unitX).length() > 0 ? gradient.cross(unitX) : unitY;
00308           btScalar angle = -gradient.angle(unitX);
00309           btQuaternion rotation = btQuaternion(axis, angle);
00310 
00311           marker.pose.orientation.x = rotation.x();
00312           marker.pose.orientation.y = rotation.y();
00313           marker.pose.orientation.z = rotation.z();
00314           marker.pose.orientation.w = rotation.w();
00315 
00316           marker.scale.x = this->resolution_[this->DIM_X];
00317           marker.scale.y = this->resolution_[this->DIM_Y];
00318           marker.scale.z = this->resolution_[this->DIM_Z];
00319 
00320           marker.color.r = 0.0;
00321           marker.color.g = 0.0;
00322           marker.color.b = 1.0;
00323           marker.color.a = 1.0;
00324 
00325           //marker.lifetime = ros::Duration(30.0);
00326 
00327           pub_viz_.publish(marker);
00328         }
00329       }
00330     }
00331   }
00332 }
00333 
00334 template <typename T>
00335 void DistanceField<T>::addCollisionMapToField(const arm_navigation_msgs::CollisionMap &collision_map)
00336 {
00337   size_t num_boxes = collision_map.boxes.size();
00338   std::vector<btVector3> points;
00339   points.reserve(num_boxes);
00340   for (size_t i=0; i<num_boxes; ++i)
00341   {
00342     points.push_back(btVector3(
00343         collision_map.boxes[i].center.x,
00344         collision_map.boxes[i].center.y,
00345         collision_map.boxes[i].center.z
00346         ));
00347   }
00348   addPointsToField(points);
00349 }
00350 
00351 template <typename T>
00352 void DistanceField<T>::visualizePlane(distance_field::PlaneVisualizationType type, double length, double width,
00353                                       double height, btVector3 origin, std::string frame_id, ros::Time stamp)
00354 {
00355   visualization_msgs::Marker plane_marker;
00356   plane_marker.header.frame_id = frame_id;
00357   plane_marker.header.stamp = stamp;
00358   plane_marker.ns = "distance_field_plane";
00359   plane_marker.id = 1;
00360   plane_marker.type = visualization_msgs::Marker::CUBE_LIST;
00361   plane_marker.action = visualization_msgs::Marker::ADD;
00362   plane_marker.scale.x = this->resolution_[VoxelGrid<T>::DIM_X];
00363   plane_marker.scale.y = this->resolution_[VoxelGrid<T>::DIM_Y];
00364   plane_marker.scale.z = this->resolution_[VoxelGrid<T>::DIM_Z];
00365   //plane_marker.lifetime = ros::Duration(30.0);
00366 
00367   plane_marker.points.reserve(100000);
00368   plane_marker.colors.reserve(100000);
00369 
00370   double minX = 0;
00371   double maxX = 0;
00372   double minY = 0;
00373   double maxY = 0;
00374   double minZ = 0;
00375   double maxZ = 0;
00376 
00377   switch(type)
00378   {
00379     case XYPlane:
00380       minZ = height;
00381       maxZ = height;
00382 
00383       minX = -length/2.0;
00384       maxX = length/2.0;
00385       minY = -width/2.0;
00386       maxY = width/2.0;
00387       break;
00388     case XZPlane:
00389       minY = height;
00390       maxY = height;
00391 
00392       minX = -length/2.0;
00393       maxX = length/2.0;
00394       minZ = -width/2.0;
00395       maxZ = width/2.0;
00396       break;
00397     case YZPlane:
00398       minX = height;
00399       maxX = height;
00400 
00401       minY = -length/2.0;
00402       maxY = length/2.0;
00403       minZ = -width/2.0;
00404       maxZ = width/2.0;
00405       break;
00406   }
00407 
00408   minX += origin.getX();
00409   minY += origin.getY();
00410   minZ += origin.getZ();
00411 
00412   maxX += origin.getX();
00413   maxY += origin.getY();
00414   maxZ += origin.getZ();
00415 
00416   int minXCell = 0;
00417   int maxXCell = 0;
00418   int minYCell = 0;
00419   int maxYCell = 0;
00420   int minZCell = 0;
00421   int maxZCell = 0;
00422 
00423   this->worldToGrid(minX,minY,minZ, minXCell, minYCell, minZCell);
00424   this->worldToGrid(maxX,maxY,maxZ, maxXCell, maxYCell, maxZCell);
00425   plane_marker.color.a = 1.0;
00426   for(int x = minXCell; x <= maxXCell; ++x)
00427   {
00428     for(int y = minYCell; y <= maxYCell; ++y)
00429     {
00430       for(int z = minZCell; z <= maxZCell; ++z)
00431       {
00432         if(!this->isCellValid(x,y,z))
00433         {
00434           continue;
00435         }
00436         double dist = getDistanceFromCell(x, y, z);
00437         int last = plane_marker.points.size();
00438         plane_marker.points.resize(last + 1);
00439         plane_marker.colors.resize(last + 1);
00440         double nx, ny, nz;
00441         this->gridToWorld(x, y, z, nx, ny, nz);
00442         btVector3 vec(nx, ny, nz);
00443         plane_marker.points[last].x = vec.x();
00444         plane_marker.points[last].y = vec.y();
00445         plane_marker.points[last].z = vec.z();
00446         if(dist < 0.0)
00447         {
00448           plane_marker.colors[last].r = fmax(fmin(0.1/fabs(dist), 1.0), 0.0);
00449           plane_marker.colors[last].g = fmax(fmin(0.05/fabs(dist), 1.0), 0.0);
00450           plane_marker.colors[last].b = fmax(fmin(0.01/fabs(dist), 1.0), 0.0);
00451 
00452         }
00453         else
00454         {
00455           plane_marker.colors[last].b = fmax(fmin(0.1/(dist+0.001), 1.0),0.0);
00456           plane_marker.colors[last].g = fmax(fmin(0.05/(dist+0.001), 1.0),0.0);
00457           plane_marker.colors[last].r = fmax(fmin(0.01/(dist+0.001), 1.0),0.0);
00458         }
00459       }
00460     }
00461   }
00462   pub_viz_.publish(plane_marker);
00463 }
00464 
00465 }
00466 #endif /* DF_DISTANCE_FIELD_H_ */
distance_field.h
