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
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 #include <mapping_msgs/CollisionMap.h>
00049
00050 namespace distance_field
00051 {
00052
00064 template <typename T>
00065 class DistanceField: public VoxelGrid<T>
00066 {
00067 public:
00080 DistanceField(double size_x, double size_y, double size_z, double resolution,
00081 double origin_x, double origin_y, double origin_z, T default_object);
00082
00083 virtual ~DistanceField();
00084
00085
00086
00094 virtual void addPointsToField(const std::vector<btVector3> points)=0;
00095
00099 void addCollisionMapToField(const mapping_msgs::CollisionMap &collision_map);
00100
00104 virtual void reset()=0;
00105
00109 double getDistance(double x, double y, double z) const;
00110
00114 double getDistanceGradient(double x, double y, double z, double& gradient_x, double& gradient_y, double& gradient_z) const;
00115
00119 double getDistanceFromCell(int x, int y, int z) const;
00120
00127 void visualize(double min_radius, double max_radius, std::string frame_id, const btTransform& cur, ros::Time stamp);
00128
00134 void visualizeGradient(double min_radius, double max_radius, std::string frame_id, ros::Time stamp);
00135
00136 protected:
00137 virtual double getDistance(const T& object) const=0;
00138
00139 private:
00140 ros::Publisher pub_viz_;
00141 int inv_twice_resolution_;
00142 };
00143
00145
00146 template <typename T>
00147 DistanceField<T>::~DistanceField()
00148 {
00149
00150 }
00151
00152 template <typename T>
00153 DistanceField<T>::DistanceField(double size_x, double size_y, double size_z, double resolution,
00154 double origin_x, double origin_y, double origin_z, T default_object):
00155 VoxelGrid<T>(size_x, size_y, size_z, resolution, origin_x, origin_y, origin_z, default_object)
00156 {
00157 ros::NodeHandle node;
00158 pub_viz_ = node.advertise<visualization_msgs::Marker>("visualization_marker", 3);
00159 inv_twice_resolution_ = 1.0/(2.0*resolution);
00160 }
00161
00162 template <typename T>
00163 double DistanceField<T>::getDistance(double x, double y, double z) const
00164 {
00165 return getDistance((*this)(x,y,z));
00166 }
00167
00168 template <typename T>
00169 double DistanceField<T>::getDistanceGradient(double x, double y, double z, double& gradient_x, double& gradient_y, double& gradient_z) const
00170 {
00171 int gx, gy, gz;
00172
00173 this->worldToGrid(x, y, z, gx, gy, gz);
00174
00175
00176
00177 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)
00178 {
00179 gradient_x = 0.0;
00180 gradient_y = 0.0;
00181 gradient_z = 0.0;
00182 return 0;
00183 }
00184
00185 gradient_x = (getDistanceFromCell(gx+1,gy,gz) - getDistanceFromCell(gx-1,gy,gz))*inv_twice_resolution_;
00186 gradient_y = (getDistanceFromCell(gx,gy+1,gz) - getDistanceFromCell(gx,gy-1,gz))*inv_twice_resolution_;
00187 gradient_z = (getDistanceFromCell(gx,gy,gz+1) - getDistanceFromCell(gx,gy,gz-1))*inv_twice_resolution_;
00188
00189 return getDistanceFromCell(gx,gy,gz);
00190
00191 }
00192
00193 template <typename T>
00194 double DistanceField<T>::getDistanceFromCell(int x, int y, int z) const
00195 {
00196 return getDistance(this->getCell(x,y,z));
00197 }
00198
00199 template <typename T>
00200 void DistanceField<T>::visualize(double min_radius, double max_radius, std::string frame_id,
00201 const btTransform& cur, ros::Time stamp)
00202 {
00203 visualization_msgs::Marker inf_marker;
00204 inf_marker.header.frame_id = frame_id;
00205 inf_marker.header.stamp = stamp;
00206 inf_marker.ns = "distance_field";
00207 inf_marker.id = 1;
00208 inf_marker.type = visualization_msgs::Marker::CUBE_LIST;
00209 inf_marker.action = 0;
00210 inf_marker.scale.x = this->resolution_[VoxelGrid<T>::DIM_X];
00211 inf_marker.scale.y = this->resolution_[VoxelGrid<T>::DIM_Y];
00212 inf_marker.scale.z = this->resolution_[VoxelGrid<T>::DIM_Z];
00213 inf_marker.color.r = 1.0;
00214 inf_marker.color.g = 0.0;
00215 inf_marker.color.b = 0.0;
00216 inf_marker.color.a = 0.1;
00217
00218
00219 inf_marker.points.reserve(100000);
00220 int num_total_cells =
00221 this->num_cells_[VoxelGrid<T>::DIM_X]*
00222 this->num_cells_[VoxelGrid<T>::DIM_Y]*
00223 this->num_cells_[VoxelGrid<T>::DIM_Z];
00224 for (int x = 0; x < this->num_cells_[VoxelGrid<T>::DIM_X]; ++x)
00225 {
00226 for (int y = 0; y < this->num_cells_[VoxelGrid<T>::DIM_Y]; ++y)
00227 {
00228 for (int z = 0; z < this->num_cells_[VoxelGrid<T>::DIM_Z]; ++z)
00229 {
00230 double dist = getDistanceFromCell(x,y,z);
00231 if (dist >= min_radius && dist <= max_radius)
00232 {
00233 int last = inf_marker.points.size();
00234 inf_marker.points.resize(last + 1);
00235 double nx, ny, nz;
00236 this->gridToWorld(x,y,z,
00237 nx, ny, nz);
00238 btVector3 vec(nx,ny,nz);
00239 vec = cur*vec;
00240 inf_marker.points[last].x = vec.x();
00241 inf_marker.points[last].y = vec.y();
00242 inf_marker.points[last].z = vec.z();
00243 }
00244 }
00245 }
00246 }
00247 ROS_DEBUG("Publishing markers: %u/%d inflated", (unsigned int) inf_marker.points.size(), num_total_cells);
00248 pub_viz_.publish(inf_marker);
00249 }
00250
00251 template <typename T>
00252 void DistanceField<T>::visualizeGradient(double min_radius, double max_radius, std::string frame_id, ros::Time stamp)
00253 {
00254 btVector3 unitX(1, 0, 0);
00255 btVector3 unitY(0, 1, 0);
00256 btVector3 unitZ(0, 0, 1);
00257
00258 int id = 0;
00259
00260 for (int x = 0; x < this->num_cells_[this->DIM_X]; ++x)
00261 {
00262 for (int y = 0; y < this->num_cells_[this->DIM_Y]; ++y)
00263 {
00264 for (int z = 0; z < this->num_cells_[this->DIM_Z]; ++z)
00265 {
00266 double worldX, worldY, worldZ;
00267 this->gridToWorld(x, y, z, worldX, worldY, worldZ);
00268
00269 double gradientX, gradientY, gradientZ;
00270 double distance = getDistanceGradient(worldX, worldY, worldZ, gradientX, gradientY, gradientZ);
00271 btVector3 gradient(gradientX, gradientY, gradientZ);
00272
00273 if (distance >= min_radius && distance <= max_radius && gradient.length() > 0)
00274 {
00275 visualization_msgs::Marker marker;
00276
00277 marker.header.frame_id = frame_id;
00278 marker.header.stamp = stamp;
00279
00280 marker.ns = "distance_field_gradient";
00281 marker.id = id++;
00282 marker.type = visualization_msgs::Marker::ARROW;
00283 marker.action = visualization_msgs::Marker::ADD;
00284
00285 marker.pose.position.x = worldX;
00286 marker.pose.position.y = worldY;
00287 marker.pose.position.z = worldZ;
00288
00289 btVector3 axis = gradient.cross(unitX).length() > 0 ? gradient.cross(unitX) : unitY;
00290 btScalar angle = -gradient.angle(unitX);
00291 btQuaternion rotation = btQuaternion(axis, angle);
00292
00293 marker.pose.orientation.x = rotation.x();
00294 marker.pose.orientation.y = rotation.y();
00295 marker.pose.orientation.z = rotation.z();
00296 marker.pose.orientation.w = rotation.w();
00297
00298 marker.scale.x = this->resolution_[this->DIM_X];
00299 marker.scale.y = this->resolution_[this->DIM_Y];
00300 marker.scale.z = this->resolution_[this->DIM_Z];
00301
00302 marker.color.r = 0.0;
00303 marker.color.g = 0.0;
00304 marker.color.b = 1.0;
00305 marker.color.a = 1.0;
00306
00307
00308
00309 pub_viz_.publish(marker);
00310 }
00311 }
00312 }
00313 }
00314 }
00315
00316 template <typename T>
00317 void DistanceField<T>::addCollisionMapToField(const mapping_msgs::CollisionMap &collision_map)
00318 {
00319 size_t num_boxes = collision_map.boxes.size();
00320 std::vector<btVector3> points;
00321 points.reserve(num_boxes);
00322 for (size_t i=0; i<num_boxes; ++i)
00323 {
00324 points.push_back(btVector3(
00325 collision_map.boxes[i].center.x,
00326 collision_map.boxes[i].center.y,
00327 collision_map.boxes[i].center.z
00328 ));
00329 }
00330 addPointsToField(points);
00331 }
00332
00333
00334 }
00335 #endif