C++ LaserProjection::projectLaser方法代码示例

void My_Filter::scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan){
    sensor_msgs::PointCloud2 raw_cloud_msg;
    sensor_msgs::PointCloud2 local_cloud_msg;
    sensor_msgs::PointCloud2 global_cloud_msg;
    PointCloudPtr_t scale_cloud(new PointCloud_t());
    PointCloudPtr_t local_cloud(new PointCloud_t());
    PointCloudPtr_t global_cloud(new PointCloud_t());
    projector_.projectLaser(*scan, raw_cloud_msg);
    pcl::fromROSMsg(raw_cloud_msg, *scale_cloud);
    
    for(int i=0; i<scale_cloud->size(); i++)
    {
      pcl::PointXYZ& point = scale_cloud->at(i);
      point.z = std::floor(point.z);
    }
    
    this->filterGlobal(scale_cloud, global_cloud);
    this->filterLocal(scale_cloud,  local_cloud);
    
    pcl::toROSMsg(*global_cloud, global_cloud_msg);
    pcl::toROSMsg(*local_cloud,  local_cloud_msg);
    
    this->local_cloud_publisher_.publish(local_cloud_msg);
    this->global_cloud_publisher_.publish(global_cloud_msg);
    
}

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 laser_receive(const sensor_msgs::LaserScan::ConstPtr& reading) {
    sensor_msgs::PointCloud cloud;
    projector_.projectLaser(*reading, cloud);
	adjustByIMUData(cloud);

	std::vector<point2d_t> target;
    
	for(int i = 0; i < cloud.points.size(); ++i) {
		point2d_t pt;
		pt.x = cloud.points[i].x;
		pt.y = cloud.points[i].y;
		target.push_back(pt);
	} 

	// build tree for search
	if(reference.size() != 0) {
		tree2d_t *tree = tree2d_build(reference.begin(), reference.end(), X, depth_of_tree);
		if (!tree) {
			std::cout << "failed!" << std::endl;
			return;
		}

		reference = target;

		// find pose
		pose_t pose;
		pose.p.x = 0;
		pose.p.y = 0;
		pose.yaw = 0;
		pose = icp_align(tree, target, pose, iterations);
		std::cout << pose << std::endl;
		
		pose_estimator::pose_estimator_msg outMsg;
		outMsg.x = pose.p.x;
		outMsg.y = pose.p.y;
		outMsg.yaw = pose.yaw;
		
		pub.publish(outMsg);
		
		// free tree
		tree2d_free(&tree);
		if (tree) {
			std::cout << "failed!" << std::endl;
			return;
		}
	} else {
		reference = target;
	}
}

void callback (const sensor_msgs::LaserScan::ConstPtr& scan_in)
{
	sensor_msgs::LaserScan scan_filtered=*scan_in;
	int total_points=(scan_filtered.angle_max-scan_filtered.angle_min)/scan_filtered.angle_increment;
	for (int i=0;i<total_points;i++)
	{
		if(scan_filtered.ranges[i]==0)
		{		
			scan_filtered.ranges[i]=5;
		}
	}
	projector_.projectLaser(scan_filtered,cloud);
	pcl_pub.publish(cloud);

}

  void scanCallback (const sensor_msgs::LaserScan::ConstPtr& scan_in)
  {
    sensor_msgs::PointCloud2 cloud;

    try
    {
    	//projector_.transformLaserScanToPointCloud(
    	//          "laser",*scan_in, cloud,listener_);
	projector_.projectLaser(*scan_in,cloud);

    }
    catch (tf::TransformException& e)
    {
        std::cout << e.what();
        return;
    }
    
    // Do something with cloud.

    for(unsigned int i=0; i<cloud.fields.size(); i++)
		{cloud.fields[i].count = 1;}
 

     if (::frame_count==0)
    {
		wall=cloud;	
		scan_pub_.publish(wall); //save 1st pointcloud as wall sensor_msg
		ROS_INFO("published wall");
    }
    else{

    	scan_pub_.publish(cloud);
		//ROS_INFO("published frame %f ",::frame_count);
    }
     ::frame_count++;
  }