示例#1
0
RayTraceIterRange rayTrace (const nm::MapMetaData& info, const gm::Point& p1, const gm::Point& p2,
                            bool project_onto_grid, bool project_source_onto_grid, 
                            float max_range)
{

  gm::Point np2 = p2;
  if (max_range > 0) {
    double distance = euclideanDistance(p1,p2);
    if (distance > max_range) {
      np2.x = ((p2.x - p1.x) * max_range/distance) + p1.x;
      np2.y = ((p2.y - p1.y) * max_range/distance) + p1.y;
    }
  }

  Cell c1 = pointCell(info, p1);
  Cell c2 = pointCell(info, np2);
  ROS_DEBUG_STREAM_NAMED ("ray_trace", "Ray tracing between " << c1.x <<
                          ", " << c1.y << " and " << c2.x << ", " << c2.y);
  const RayTraceIterator done(c1, c1, true);
  const RayTraceIterRange empty_range(done, done);
  if (!withinBounds(info, c1)) {
    if (project_source_onto_grid) {
      const optional<Cell> c = rayTraceOntoGrid(info, c2, c1);
      if (c)
        c1 = *c;
      else
        return empty_range;
    }
    else
      throw PointOutOfBoundsException(p1);
  }
  
  if (!withinBounds(info, np2)) {
    if (project_onto_grid) {
      const optional<Cell> c = rayTraceOntoGrid(info, c1, c2);
      if (c)
        c2 = *c;
      else
        return empty_range;
    }
    else {
      throw PointOutOfBoundsException(np2);
    }
  }

  ROS_DEBUG_STREAM_NAMED ("ray_trace", "Projected ray trace endpoints to " << c1.x <<
                          ", " << c1.y << " and " << c2.x << ", " << c2.y);
  return rayTrace(c1, c2);
}
示例#2
0
RayTraceIterRange rayTrace (const nm::MapMetaData& info, const gm::Point& p1, const gm::Point& p2,
                            bool project_onto_grid, bool project_source_onto_grid)
{
  Cell c1 = pointCell(info, p1);
  Cell c2 = pointCell(info, p2);
  ROS_DEBUG_STREAM_NAMED ("ray_trace", "Ray tracing between " << c1.x <<
                          ", " << c1.y << " and " << c2.x << ", " << c2.y);
  const RayTraceIterator done(c1, c1, true);
  const RayTraceIterRange empty_range(done, done);
  if (!withinBounds(info, c1)) {
    if (project_source_onto_grid) {
      const optional<Cell> c = rayTraceOntoGrid(info, c2, c1);
      if (c)
        c1 = *c;
      else
        return empty_range;
    }
    else
      throw PointOutOfBoundsException(p1);
  }
  
  if (!withinBounds(info, p2)) {
    if (project_onto_grid) {
      const optional<Cell> c = rayTraceOntoGrid(info, c1, c2);
      if (c)
        c2 = *c;
      else
        return empty_range;
    }
    else {
      throw PointOutOfBoundsException(p2);
    }
  }

  ROS_DEBUG_STREAM_NAMED ("ray_trace", "Projected ray trace endpoints to " << c1.x <<
                          ", " << c1.y << " and " << c2.x << ", " << c2.y);
  return rayTrace(c1, c2);
}
示例#3
0
sm::LaserScan::Ptr
simulateRangeScan (const nm::OccupancyGrid& grid, const gm::Pose& sensor_pose,
                   const sm::LaserScan& scanner_info, const bool unknown_obstacles)
{
  sm::LaserScan::Ptr result(new sm::LaserScan(scanner_info));

  const double angle_range = scanner_info.angle_max - scanner_info.angle_min;
  const unsigned n =
    (unsigned) round(1+angle_range/scanner_info.angle_increment);
  const gm::Point& p0 = sensor_pose.position;
  const Cell c0 = pointCell(grid.info, p0);
  const double theta0 = tf::getYaw(sensor_pose.orientation);
  result->ranges.resize(n);

  for (unsigned i=0; i<n; i++)
  {
    const double theta = scanner_info.angle_min+i*scanner_info.angle_increment;
    const gm::Point scan_max =
      rayEndPoint(p0, theta0 + theta, scanner_info.range_max+1);
    
    result->ranges[i] = scanner_info.range_max+1; // Default if loop terminates
    BOOST_FOREACH (const Cell& c, rayTrace(grid.info, p0, scan_max, true))
    {
      const gm::Point p = cellCenter(grid.info, c);
      const double d = sqrt(pow(p.x-p0.x, 2) + pow(p.y-p0.y, 2));
      char data = grid.data[cellIndex(grid.info, c)];
      if (d > scanner_info.range_max)
        break;
      else if (data == OCCUPIED && !(c==c0))
      {
        result->ranges[i] = d;
        break;
      }
      else if (data == UNKNOWN && !(c==c0))
      {
        result->ranges[i] = unknown_obstacles ? d : scanner_info.range_max+1;
        break;
      }
    }
  }

  return result;
}
示例#4
0
void addKnownFreePoint (OverlayClouds* overlay, const gm::Point& p, const double r)
{
    const nm::MapMetaData& geom = overlay->grid.info;
    const int cell_radius = floor(r/geom.resolution);
    const Cell c = pointCell(geom, p);
    for (int x= c.x-cell_radius; x<=c.x+cell_radius; x++)
    {
        for (int y=c.y-cell_radius; y<=c.y+cell_radius; y++)
        {
            const Cell c2(x, y);
            if (withinBounds(geom, c2))
            {
                const index_t ind = cellIndex(geom, c2);
                overlay->hit_counts[ind] = 0;
                overlay->pass_through_counts[ind] = overlay->min_pass_through+1;
            }
        }
    }
}
示例#5
0
void addCloud (OverlayClouds* overlay, LocalizedCloud::ConstPtr cloud, const int inc)
{
    ROS_ASSERT_MSG(overlay->frame_id==cloud->header.frame_id,
                   "Frame id %s of overlayed cloud didn't match existing one %s",
                   cloud->header.frame_id.c_str(), overlay->frame_id.c_str());
    ROS_ASSERT(overlay->grid.info.width*overlay->grid.info.height > 0);

    const gm::Point& sensor_pos = cloud->sensor_pose.position;
    ROS_DEBUG_NAMED ("overlay", "Ray tracing from %.2f, %.2f", sensor_pos.x, sensor_pos.y);

    // Transform points to world frame
    tf::Pose sensor_to_world;
    tf::poseMsgToTF(cloud->sensor_pose, sensor_to_world);
    vector<gm::Point> transformed_points(cloud->cloud.points.size());
    transform(cloud->cloud.points.begin(),
              cloud->cloud.points.end(),
              transformed_points.begin(),
              bind(transformPt, ref(sensor_to_world), _1));

    // Iterate over points in this cloud
    BOOST_FOREACH (const gm::Point& p, transformed_points) {

        ROS_DEBUG_NAMED ("overlay_counts", " Ray tracing to point %.2f, %.2f", p.x, p.y);
        boost::optional<index_t> last_ind;

        const bool have_existing_grid = !overlay->grid.data.empty();

        // Inner loop: raytrace along the line and update counts
        // We allow both the sensor pose and the target to be off the grid
        BOOST_FOREACH (const Cell& c, rayTrace(overlay->grid.info, sensor_pos, p, true, true, overlay->max_distance)) {
            last_ind = cellIndex(overlay->grid.info, c);
            overlay->pass_through_counts[*last_ind] += inc;
            if (have_existing_grid && overlay->grid.data[*last_ind] == OCCUPIED)
                break;
            ROS_ASSERT(overlay->pass_through_counts[*last_ind]>=0);
            ROS_DEBUG_NAMED ("overlay_counts", "  Pass-through counts for %d, %d are now %u", c.x, c.y,
                             overlay->pass_through_counts[*last_ind]);
        }

        if (last_ind) {
            // If the last cell equals the point (i.e., point is not off the grid), update hit counts
            const Cell last_cell = indexCell(overlay->grid.info, *last_ind);
            if (last_cell == pointCell(overlay->grid.info, p)) {

#ifdef GRID_UTILS_GCC_46
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wuninitialized"
#endif

                overlay->hit_counts[*last_ind] += inc;

#ifdef GRID_UTILS_GCC_46
#pragma GCC diagnostic pop
#endif

                ROS_ASSERT(overlay->hit_counts[*last_ind]>=0);
                ROS_DEBUG_NAMED ("overlay_counts", "  Hit counts for %d, %d are now %u", last_cell.x, last_cell.y,
                                 overlay->hit_counts[*last_ind]);
            }
        }
    }