occupancy_grid_utils: occupancy_grid_utils: ray

00001 #include <occupancy_grid_utils/ray_tracer.h>
00002 #include <occupancy_grid_utils/exceptions.h>
00003 #include <tf/transform_datatypes.h>
00004 #include <boost/bind.hpp>
00005 #include <boost/ref.hpp>
00006 #include <boost/foreach.hpp>
00007 #include <boost/optional.hpp>
00008 #include <algorithm>
00009 #include <ros/assert.h>
00010 
00011 namespace occupancy_grid_utils
00012 {
00013 
00014 namespace nm=nav_msgs;
00015 namespace gm=geometry_msgs;
00016 namespace sm=sensor_msgs;
00017 
00018 using boost::bind;
00019 using boost::ref;
00020 using boost::optional;
00021 
00022 RayTraceIterRange rayTrace (const Cell& c1, const Cell& c2)
00023 {
00024   return RayTraceIterRange(RayTraceIterator(c1,c2), RayTraceIterator(c2,c2,true));
00025 }
00026 
00027 inline
00028 bool cellWithinBounds (const nm::MapMetaData& info, const Cell& c)
00029 {
00030   return withinBounds(info, c);
00031 }
00032 
00033 optional<Cell> rayTraceOntoGrid (const nm::MapMetaData& info, const Cell& c1, const Cell& c2)
00034 {
00035   RayTraceIterRange r = rayTrace(c2, c1);
00036   RayTraceIterator pos = std::find_if (r.first, r.second, bind(cellWithinBounds, ref(info), _1));
00037   optional<Cell> c;
00038   if (pos!=r.second)
00039     c = *pos;
00040   return c;
00041 }
00042 
00043 RayTraceIterRange rayTrace (const nm::MapMetaData& info, const gm::Point& p1, const gm::Point& p2,
00044                             bool project_onto_grid, bool project_source_onto_grid)
00045 {
00046   Cell c1 = pointCell(info, p1);
00047   Cell c2 = pointCell(info, p2);
00048   ROS_DEBUG_STREAM_NAMED ("ray_trace", "Ray tracing between " << c1.x <<
00049                           ", " << c1.y << " and " << c2.x << ", " << c2.y);
00050   const RayTraceIterator done(c1, c1, true);
00051   const RayTraceIterRange empty_range(done, done);
00052   if (!withinBounds(info, c1)) {
00053     if (project_source_onto_grid) {
00054       const optional<Cell> c = rayTraceOntoGrid(info, c2, c1);
00055       if (c)
00056         c1 = *c;
00057       else
00058         return empty_range;
00059     }
00060     else
00061       throw PointOutOfBoundsException(p1);
00062   }
00063   
00064   if (!withinBounds(info, p2)) {
00065     if (project_onto_grid) {
00066       const optional<Cell> c = rayTraceOntoGrid(info, c1, c2);
00067       if (c)
00068         c2 = *c;
00069       else
00070         return empty_range;
00071     }
00072     else {
00073       throw PointOutOfBoundsException(p2);
00074     }
00075   }
00076 
00077   ROS_DEBUG_STREAM_NAMED ("ray_trace", "Projected ray trace endpoints to " << c1.x <<
00078                           ", " << c1.y << " and " << c2.x << ", " << c2.y);
00079   return rayTrace(c1, c2);
00080 }
00081 
00082 
00083 gm::Point rayEndPoint (const gm::Point& p0, const double theta, const double d)
00084 {
00085   gm::Point p;
00086   p.x = p0.x + cos(theta)*d;
00087   p.y = p0.y + sin(theta)*d;
00088   return p;
00089 }                       
00090 
00091 sm::LaserScan::Ptr
00092 simulateRangeScan (const nm::OccupancyGrid& grid, const gm::Pose& sensor_pose,
00093                    const sm::LaserScan& scanner_info, const bool unknown_obstacles)
00094 {
00095   sm::LaserScan::Ptr result(new sm::LaserScan(scanner_info));
00096 
00097   const double angle_range = scanner_info.angle_max - scanner_info.angle_min;
00098   const unsigned n =
00099     (unsigned) round(1+angle_range/scanner_info.angle_increment);
00100   const gm::Point& p0 = sensor_pose.position;
00101   const Cell c0 = pointCell(grid.info, p0);
00102   const double theta0 = tf::getYaw(sensor_pose.orientation);
00103   result->ranges.resize(n);
00104 
00105   for (unsigned i=0; i<n; i++)
00106   {
00107     const double theta = scanner_info.angle_min+i*scanner_info.angle_increment;
00108     const gm::Point scan_max =
00109       rayEndPoint(p0, theta0 + theta, scanner_info.range_max+1);
00110     
00111     result->ranges[i] = scanner_info.range_max+1; // Default if loop terminates
00112     BOOST_FOREACH (const Cell& c, rayTrace(grid.info, p0, scan_max, true))
00113     {
00114       const gm::Point p = cellCenter(grid.info, c);
00115       const double d = sqrt(pow(p.x-p0.x, 2) + pow(p.y-p0.y, 2));
00116       char data = grid.data[cellIndex(grid.info, c)];
00117       if (d > scanner_info.range_max)
00118         break;
00119       else if (data == OCCUPIED && !(c==c0))
00120       {
00121         result->ranges[i] = d;
00122         break;
00123       }
00124       else if (data == UNKNOWN && !(c==c0))
00125       {
00126         result->ranges[i] = unknown_obstacles ? d : scanner_info.range_max+1;
00127         break;
00128       }
00129     }
00130   }
00131 
00132   return result;
00133 }
00134 
00135 
00136 } // namespace occupancy_grid_utils
ray_tracer.cpp
