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); }
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); }