void VoxelGridModel::removePointsInScanBoundry(const PlanarLaserScan& laser_scan, double raytrace_range){
    if(laser_scan.cloud.points.size() == 0)
      return;

    unsigned int sensor_x, sensor_y, sensor_z;
    double ox = laser_scan.origin.x;
    double oy = laser_scan.origin.y;
    double oz = laser_scan.origin.z;
    
    if(!worldToMap3D(ox, oy, oz, sensor_x, sensor_y, sensor_z))
      return;

    for(unsigned int i = 0; i < laser_scan.cloud.points.size(); ++i){
      double wpx = laser_scan.cloud.points[i].x;
      double wpy = laser_scan.cloud.points[i].y;
      double wpz = laser_scan.cloud.points[i].z;

      double distance = dist(ox, oy, oz, wpx, wpy, wpz);
      double scaling_fact = raytrace_range / distance;
      scaling_fact = scaling_fact > 1.0 ? 1.0 : scaling_fact;
      wpx = scaling_fact * (wpx - ox) + ox;
      wpy = scaling_fact * (wpy - oy) + oy;
      wpz = scaling_fact * (wpz - oz) + oz;

      //we can only raytrace to a maximum z height
      if(wpz >= max_z_){
        //we know we want the vector's z value to be max_z
        double a = wpx - ox;
        double b = wpy - oy;
        double c = wpz - oz;
        double t = (max_z_ - .01 - oz) / c;
        wpx = ox + a * t;
        wpy = oy + b * t;
        wpz = oz + c * t;
      }
      //and we can only raytrace down to the floor
      else if(wpz < 0.0){
        //we know we want the vector's z value to be 0.0
        double a = wpx - ox;
        double b = wpy - oy;
        double c = wpz - oz;
        double t = (0.0 - oz) / c;
        wpx = ox + a * t;
        wpy = oy + b * t;
        wpz = oz + c * t;
      }

      unsigned int point_x, point_y, point_z;
      if(worldToMap3D(wpx, wpy, wpz, point_x, point_y, point_z)){
        obstacle_grid_.clearVoxelLine(sensor_x, sensor_y, sensor_z, point_x, point_y, point_z);
      }
    }
  }
Exemple #2
0
void VoxelLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x,
                                       double* min_y, double* max_x, double* max_y)
{
  if (rolling_window_)
    updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2);
  if (!enabled_)
    return;
  useExtraBounds(min_x, min_y, max_x, max_y);

  bool current = true;
  std::vector<ObservationSet> observations;
  std::vector<Observation> clearing_observations;

  if(clear_old_){
    resetOldCosts(min_x, min_y, max_x, max_y);
  }

  //get the marking observations
  current = current && getMarkingObservations(observations);

  //get the clearing observations
  current = current && getClearingObservations(clearing_observations);

  //update the global current status
  current_ = current;

  //raytrace freespace
  for (unsigned int i = 0; i < clearing_observations.size(); ++i)
  {
    raytraceFreespace(clearing_observations[i], min_x, min_y, max_x, max_y);
  }
   
  //place the new obstacles into a priority queue... each with a priority of zero to begin with
  for (std::vector<ObservationSet>::const_iterator it = observations.begin(); it != observations.end(); ++it)
  {
    const ObservationSet& obs_set = *it; 

    //check if this is in the max_timeout list 
    bool max_timeout = false; 
    
    if(observation_timeout.find(obs_set.topic)!= observation_timeout.end()){
      max_timeout = true;
      ROS_DEBUG("Observation set for topic %s - clearing\n", obs_set.topic.c_str());
    }

    for(std::vector<Observation>::const_iterator it_o = obs_set.marking_observations.begin(); 
        it_o != obs_set.marking_observations.end(); it_o++){
      const Observation& obs = *it_o;

      //we should throw out stale observations also 

      CostMapList cm_list; 
      cm_list.topic = obs_set.topic;
      cm_list.obs_timestamp = obs.cloud_->header.stamp; 
      
      double obs_ts = cm_list.obs_timestamp / 1.0e6; 

      const pcl::PointCloud<pcl::PointXYZ>& cloud = *(obs.cloud_);

      double sq_obstacle_range = obs.obstacle_range_ * obs.obstacle_range_;

      int count = 0; 

      for (unsigned int i = 0; i < cloud.points.size(); ++i)
      {
          //if the obstacle is too high or too far away from the robot we won't add it
          if (cloud.points[i].z > max_obstacle_height_)
            continue;          

          //compute the squared distance from the hitpoint to the pointcloud's origin
          double sq_dist = (cloud.points[i].x - obs.origin_.x) * (cloud.points[i].x - obs.origin_.x)
            + (cloud.points[i].y - obs.origin_.y) * (cloud.points[i].y - obs.origin_.y)
            + (cloud.points[i].z - obs.origin_.z) * (cloud.points[i].z - obs.origin_.z);

          //if the point is far enough away... we won't consider it
          if (sq_dist >= sq_obstacle_range)
            continue;

          //now we need to compute the map coordinates for the observation
          unsigned int mx, my, mz;
          if (cloud.points[i].z < origin_z_)
          {
              if (!worldToMap3D(cloud.points[i].x, cloud.points[i].y, origin_z_, mx, my, mz))
                continue;
          }
          else if (!worldToMap3D(cloud.points[i].x, cloud.points[i].y, cloud.points[i].z, mx, my, mz))
          {
              continue;
          }

          //mark the cell in the voxel grid and check if we should also mark it in the costmap
          if (voxel_grid_.markVoxelInMap(mx, my, mz, mark_threshold_))
          {
              unsigned int index = getIndex(mx, my);
              //these are the ones set as occupied 
              costmap_[index] = LETHAL_OBSTACLE;
              touch((double)cloud.points[i].x, (double)cloud.points[i].y, min_x, min_y, max_x, max_y);

              //keep track of which indices we updated 
              if(max_timeout){
                cm_list.indices.push_back(ObstaclePoint(index, (double)cloud.points[i].x, (double)cloud.points[i].y));
                count++;
              }
          }
      }
      if(max_timeout && count > 0){
        locations_utime.updateObstacleTime(cm_list);
        new_obs_list.push_back(cm_list);
      }
    }
  }

  if(clear_old_){
    //inflate the bounds - otherwise the inflation layer wont reset the inflated cost 
    *min_x -= inflation_radius_;
    *min_y -= inflation_radius_;
    *max_x += inflation_radius_;
    *max_y += inflation_radius_;
  }

  if (publish_voxel_)
  {
    costmap_2d::VoxelGrid grid_msg;
    unsigned int size = voxel_grid_.sizeX() * voxel_grid_.sizeY();
    grid_msg.size_x = voxel_grid_.sizeX();
    grid_msg.size_y = voxel_grid_.sizeY();
    grid_msg.size_z = voxel_grid_.sizeZ();
    grid_msg.data.resize(size);
    memcpy(&grid_msg.data[0], voxel_grid_.getData(), size * sizeof(unsigned int));

    grid_msg.origin.x = origin_x_;
    grid_msg.origin.y = origin_y_;
    grid_msg.origin.z = origin_z_;

    grid_msg.resolutions.x = resolution_;
    grid_msg.resolutions.y = resolution_;
    grid_msg.resolutions.z = z_resolution_;
    grid_msg.header.frame_id = global_frame_;
    grid_msg.header.stamp = ros::Time::now();
    voxel_pub_.publish(grid_msg);
  }

  footprint_layer_.updateBounds(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y);
}