Ejemplos de cloud_for_pub en C++ (Cpp)

Ejemplo n.º 1

Archivo: main.cpp Proyecto: rmkeezer/igvc-software

void scanCallback(const sensor_msgs::LaserScan::ConstPtr& msg)
{
  sensor_msgs::LaserScan scanData = *msg;

  auto rangeIsValid = [&scanData](float range) {
    return !std::isnan(range) && range > scanData.range_min && range < scanData.range_max;
  };

  for (unsigned int i = 0; i < scanData.ranges.size(); i++)
  {
    float& range = scanData.ranges[i];
    if (range > scanData.range_max || range < scanData.range_min)
      continue;
    // Too close
    if (range < min_dist)
      range = scanData.range_max + 1;
    // No valid neighbors
    else if ((i == 0 || !rangeIsValid(scanData.ranges[i - 1])) &&
             (i == (scanData.ranges.size() - 1) || !rangeIsValid(scanData.ranges[i + 1])))
      range = scanData.range_max + 1;
    // No close neighbors
    else if ((i == 0 || abs(scanData.ranges[i - 1] - range) > neighbor_dist) &&
             (i == (scanData.ranges.size() - 1) || abs(scanData.ranges[i + 1] - range) > neighbor_dist))
      range = scanData.range_max + 1;
  }

  sensor_msgs::PointCloud2 cloud;
  projection.projectLaser(scanData, cloud);
  cloud.header.frame_id = "/lidar";

  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_for_pub(new pcl::PointCloud<pcl::PointXYZ>());
  fromROSMsg(cloud, *cloud_for_pub);
  _pointcloud_pub.publish(*cloud_for_pub);
}

Ejemplo n.º 2

void scanCallback(const sensor_msgs::LaserScan::ConstPtr& msg)
{
    sensor_msgs::LaserScan scanData = *msg;

    auto rangeIsValid = [&scanData](float range) {
        return !std::isnan(range) && range > scanData.range_min && range < scanData.range_max;
    };

    for(auto i = 0; i < scanData.ranges.size(); i++)
    {
        float& range = scanData.ranges[i];
        if(range > scanData.range_max || range < scanData.range_min)
            continue;
        // Too close
        if(range < 0.4)
            range = scanData.range_max + 1;
        // Too far
        else if(range > 6.0)
            range = scanData.range_max + 1;
        // No valid neighbors
        else if((i == 0 || !rangeIsValid(scanData.ranges[i-1])) && (i == (scanData.ranges.size()-1) || !rangeIsValid(scanData.ranges[i+1])))
            range = scanData.range_max + 1;
        // No close neighbors
        else if((i == 0 || abs(scanData.ranges[i-1] - range) > 0.2) && (i == (scanData.ranges.size()-1) || abs(scanData.ranges[i+1] - range) > 0.2))
            range = scanData.range_max + 1;
    }

    sensor_msgs::PointCloud2 cloud;
    projection.projectLaser(scanData, cloud);
    cloud.header.frame_id = "/lidar";

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_for_pub(new pcl::PointCloud<pcl::PointXYZ>());
    fromROSMsg(cloud, *cloud_for_pub);
    cloud_for_pub->push_back(pcl::PointXYZ(-3,0,0));    //really solid non-hacky fix for some unknown error
    _pointcloud_pub.publish(*cloud_for_pub);
}