occupancy_grid_utils: occupancy_grid_utils: grid

00001 /*
00002  * Copyright (c) 2008, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00038 #include <occupancy_grid_utils/ray_tracer.h>
00039 #include <ros/assert.h>
00040 #include <tf/transform_datatypes.h>
00041 #include <boost/foreach.hpp>
00042 #include <boost/bind.hpp>
00043 #include <boost/ref.hpp>
00044 #include <boost/optional.hpp>
00045 #include "gcc_version.h"
00046 
00047 namespace occupancy_grid_utils
00048 {
00049 
00050 namespace gm=geometry_msgs;
00051 namespace nm=nav_msgs;
00052 
00053 using boost::bind;
00054 using boost::ref;
00055 using std::vector;
00056 
00057 typedef boost::shared_ptr<nm::OccupancyGrid> GridPtr;
00058 typedef boost::shared_ptr<nm::OccupancyGrid const> GridConstPtr;
00059 
00060 
00061 inline gm::Point transformPt (const tf::Pose& trans, const gm::Point32& p)
00062 {
00063   const btVector3 pt(p.x, p.y, p.z);
00064   gm::Point transformed;
00065   tf::pointTFToMsg(trans*pt, transformed);
00066   return transformed;
00067 }
00068 
00069 inline double euclideanDistance (const gm::Point& p1, const gm::Point& p2)
00070 {
00071   const double dx=p1.x-p2.x;
00072   const double dy=p1.y-p2.y;
00073   const double dz=p1.z-p2.z;
00074   return sqrt(dx*dx + dy*dy + dz*dz);
00075 }
00076 
00077 
00078 // This is our policy for computing the occupancy of a cell based on hit and pass through counts
00079 inline int8_t determineOccupancy (const unsigned hit_count, const unsigned pass_through_count, 
00080                                   const double occupancy_threshold, const double min_pass_through)
00081 {
00082   int8_t ret;
00083   if (pass_through_count < min_pass_through)
00084     ret=UNKNOWN;
00085   else if (hit_count > pass_through_count*occupancy_threshold)
00086     ret=OCCUPIED;
00087   else 
00088     ret=UNOCCUPIED;
00089   ROS_DEBUG_NAMED ("overlay_get_grid", " Hit count is %u, pass through count is %u, occupancy is %d",
00090                    hit_count, pass_through_count, ret);
00091   return ret;
00092 }
00093 
00094 OverlayClouds createCloudOverlay (const nm::OccupancyGrid& grid, const std::string& frame_id,
00095                                   double occupancy_threshold,
00096                                   double max_distance, double min_pass_through)
00097 {
00098   OverlayClouds overlay;
00099   overlay.grid = grid;
00100   overlay.frame_id = frame_id;
00101   overlay.occupancy_threshold = occupancy_threshold;
00102   overlay.max_distance = max_distance;
00103   overlay.min_pass_through = min_pass_through;
00104   overlay.hit_counts.resize(grid.info.height*grid.info.width);
00105   overlay.pass_through_counts.resize(grid.info.height*grid.info.width);
00106   ROS_ASSERT(min_pass_through > 0);
00107   return overlay;
00108 }
00109 
00110 void addKnownFreePoint (OverlayClouds* overlay, const gm::Point& p, const double r)
00111 {
00112   const nm::MapMetaData& geom = overlay->grid.info;
00113   const int cell_radius = floor(r/geom.resolution);
00114   const Cell c = pointCell(geom, p);
00115   for (int x= c.x-cell_radius; x<=c.x+cell_radius; x++)
00116   {
00117     for (int y=c.y-cell_radius; y<=c.y+cell_radius; y++)
00118     {
00119       const Cell c2(x, y);
00120       if (withinBounds(geom, c2))
00121       {
00122         const index_t ind = cellIndex(geom, c2);
00123         overlay->hit_counts[ind] = 0;
00124         overlay->pass_through_counts[ind] = overlay->min_pass_through+1;
00125       }
00126     }
00127   }
00128 }
00129 
00130 
00131 void addCloud (OverlayClouds* overlay, LocalizedCloud::ConstPtr cloud, const int inc)
00132 {
00133   
00134   ROS_ASSERT_MSG(overlay->frame_id==cloud->header.frame_id,
00135                  "Frame id %s of overlayed cloud didn't match existing one %s",
00136                  cloud->header.frame_id.c_str(), overlay->frame_id.c_str());
00137   const index_t num_cells = overlay->grid.info.width*overlay->grid.info.height;
00138   ROS_ASSERT(num_cells>0);
00139 
00140   const gm::Point& sensor_pos = cloud->sensor_pose.position;
00141   ROS_DEBUG_NAMED ("overlay", "Ray tracing from %.2f, %.2f", sensor_pos.x, sensor_pos.y);
00142 
00143   // Transform points to world frame
00144   tf::Pose sensor_to_world;
00145   tf::poseMsgToTF(cloud->sensor_pose, sensor_to_world);
00146   vector<gm::Point> transformed_points(cloud->cloud.points.size());
00147   transform(cloud->cloud.points.begin(), 
00148             cloud->cloud.points.end(), 
00149             transformed_points.begin(),
00150             bind(transformPt, ref(sensor_to_world), _1));
00151 
00152   // Iterate over points in this cloud
00153   BOOST_FOREACH (const gm::Point& p, transformed_points) {
00154   
00155     // Skip point unless it's within the distance threshold
00156     if (euclideanDistance(sensor_pos, p) < overlay->max_distance) {
00157       ROS_DEBUG_NAMED ("overlay_counts", " Ray tracing to point %.2f, %.2f", p.x, p.y);
00158       boost::optional<index_t> last_ind;
00159 
00160       const bool have_existing_grid = !overlay->grid.data.empty();
00161       
00162       // Inner loop: raytrace along the line and update counts
00163       // We allow both the sensor pose and the target to be off the grid
00164       BOOST_FOREACH (const Cell& c, rayTrace(overlay->grid.info, sensor_pos, p, true, true)) {
00165         last_ind = cellIndex(overlay->grid.info, c);
00166         overlay->pass_through_counts[*last_ind] += inc;
00167         if (have_existing_grid && overlay->grid.data[*last_ind] == OCCUPIED)
00168             break;
00169         ROS_ASSERT(overlay->pass_through_counts[*last_ind]>=0);
00170         ROS_DEBUG_NAMED ("overlay_counts", "  Pass-through counts for %d, %d are now %u", c.x, c.y, 
00171                          overlay->pass_through_counts[*last_ind]);
00172       }
00173 
00174       if (last_ind) {
00175         // If the last cell equals the point (i.e., point is not off the grid), update hit counts
00176         const Cell last_cell = indexCell(overlay->grid.info, *last_ind);
00177         if (last_cell == pointCell(overlay->grid.info, p)) {
00178           
00179 #ifdef GRID_UTILS_GCC_46
00180 #pragma GCC diagnostic push
00181 #pragma GCC diagnostic ignored "-Wuninitialized"
00182 #endif
00183           
00184           overlay->hit_counts[*last_ind] += inc;
00185           
00186 #ifdef GRID_UTILS_GCC_46
00187 #pragma GCC diagnostic pop
00188 #endif
00189           
00190           ROS_ASSERT(overlay->hit_counts[*last_ind]>=0);
00191           ROS_DEBUG_NAMED ("overlay_counts", "  Hit counts for %d, %d are now %u", last_cell.x, last_cell.y, 
00192                            overlay->hit_counts[*last_ind]);
00193         }
00194       }
00195     }
00196   }
00197   ROS_DEBUG_NAMED ("overlay", "Done ray tracing from %.2f, %.2f", sensor_pos.x, sensor_pos.y);
00198 }
00199 
00200 
00201 void addCloud (OverlayClouds* overlay, LocalizedCloud::ConstPtr cloud)
00202 {
00203   addCloud(overlay, cloud, 1);
00204 }
00205 
00206 void removeCloud (OverlayClouds* overlay, LocalizedCloud::ConstPtr cloud)
00207 {
00208   addCloud(overlay, cloud, -1);
00209 }
00210 
00211 
00212 
00213 GridPtr getGrid (const OverlayClouds& overlay) 
00214 {
00215   GridPtr grid(new nm::OccupancyGrid());
00216   grid->info = overlay.grid.info;
00217   grid->header.frame_id = overlay.frame_id;
00218   ROS_DEBUG_STREAM_NAMED ("overlay_get_grid", "Getting overlaid grid with geometry " << overlay.grid.info);
00219   grid->data.resize(overlay.grid.info.width*overlay.grid.info.height);
00220   transform(overlay.hit_counts.begin(), overlay.hit_counts.end(), overlay.pass_through_counts.begin(), 
00221             grid->data.begin(), bind(determineOccupancy, _1, _2, overlay.occupancy_threshold,
00222                                      overlay.min_pass_through));
00223   return grid;
00224 }
00225 
00226 void resetCounts (OverlayClouds* overlay)
00227 {
00228   fill(overlay->hit_counts.begin(), overlay->hit_counts.end(), 0);
00229   fill(overlay->pass_through_counts.begin(), overlay->pass_through_counts.end(), 0);
00230 }
00231 
00232 nm::MapMetaData gridInfo (const OverlayClouds& overlay) 
00233 {
00234   return overlay.grid.info;
00235 }
00236 
00237 } // namespace occupancy_grid_utils
grid_overlay.cpp
