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

bool LaserAggregationServiceCB(hubo_sensor_msgs::LidarAggregation::Request& req, hubo_sensor_msgs::LidarAggregation::Response& res)
{
    ROS_INFO("Attempting to aggregate %ld laser scans into a pointcloud", req.Scans.size());
    sensor_msgs::PointCloud2 full_cloud;
    if (req.Scans.size() > 1)
    {
        g_laser_projector.transformLaserScanToPointCloud(g_fixed_frame, req.Scans[0], full_cloud, *g_transformer);
        for (unsigned int index = 1; index < req.Scans.size(); index++)
        {
            sensor_msgs::PointCloud2 scan_cloud;
            g_laser_projector.transformLaserScanToPointCloud(g_fixed_frame, req.Scans[index], scan_cloud, *g_transformer);
            bool succeded = pcl::concatenatePointCloud(full_cloud, scan_cloud, full_cloud);
            if (!succeded)
            {
                ROS_ERROR("PCL could not concatenate pointclouds");
            }
        }
    }
    else if (req.Scans.size() == 1)
    {
        g_laser_projector.transformLaserScanToPointCloud(g_fixed_frame, req.Scans[0], full_cloud, *g_transformer);
    }
    else
    {
        ROS_WARN("No laser scans to aggregate");
    }
    full_cloud.header.frame_id = g_fixed_frame;
    full_cloud.header.stamp = ros::Time::now();
    res.Cloud = full_cloud;
    g_cloud_publisher.publish(full_cloud);
    return true;
}

void My_Filter::scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan) {
  sensor_msgs::PointCloud2 cloud;
//  scan->header.stamp += ros::Duration(0.5)
  try {
    projector.transformLaserScanToPointCloud("/base_link", *scan, cloud, tfListener);	// 1st arg was "base_link"
    header = cloud.header;
  } catch(tf::TransformException ex) {
    ROS_WARN("TransformException!");
    ROS_ERROR("%s", ex.what());
    return;
  }
  if (b_2d) {
    pcl::PointCloud<pcl::PointXYZ> pc_2d;
    pcl::fromROSMsg(cloud, pc_2d);
    int points = pc_2d.width;
    for (int i = 0; i < points; i++) {
      pc_2d.points[i].z = 0;
    }
    pc_comb_2d = pc_comb_2d + pc_2d;
  }
  if (b_3d) {
    pcl::PointCloud<pcl::PointXYZ> pc_3d;
    pcl::fromROSMsg(cloud, pc_3d);
    pc_comb_3d = pc_comb_3d + pc_3d;
  }
}

void Node::laser_1_cb(const sensor_msgs::LaserScan::ConstPtr& scan_in)
{

  if(!listener_.waitForTransform("/laser1", "/world", scan_in->header.stamp + ros::Duration().fromSec(scan_in->ranges.size()*scan_in->time_increment), ros::Duration(1.0))) {
    return;
  }

  sensor_msgs::PointCloud2 cloud;
  projector_.transformLaserScanToPointCloud("/world", *scan_in, cloud, listener_);

  pcl::PointCloud<pcl::PointXYZ> pcl_cloud;
  pcl::fromROSMsg(cloud, pcl_cloud);

  crop_cloud(pcl_cloud, 1, scan_in->header.stamp);

  if (!starting_config && fabs(angle) < 0.1)
  {
    starting_config = 1;
    ROS_INFO("Ready to Scan.");
  }

  if (starting_config)
    cloud1 += pcl_cloud;

  sensor_msgs::PointCloud2 cloud_out;
  pcl::toROSMsg(cloud1, cloud_out);
  cloud_out.header = cloud.header;

  pc_1_pub_.publish(cloud_out);
}

	void scanCallback (const sensor_msgs::LaserScan scan_in)  {
	    ROS_DEBUG("LaserScanReceiver: I have %d ranges", scan_in.ranges.size());
	    sensor_msgs::PointCloud cloud;
	    try {
		projector.transformLaserScanToPointCloud(
			      	  base_tf.c_str(),scan_in, cloud,listener);
	    }
	    catch (tf::TransformException& e)    {
	    	ROS_ERROR("%s", e.what());
	    	return;
	    }
	    
	    obstacle_x.clear();
	    obstacle_y.clear();
	    obstacle_x.reserve(cloud.points.size());
	    obstacle_y.reserve(cloud.points.size());
	    
	    for (std::vector<geometry_msgs::Point32>::iterator i=cloud.points.begin(); i!=cloud.points.end(); i++) {
		obstacle_x.push_back(i->x);
		obstacle_y.push_back(i->y);
	    }
	    
	    laser_count = obstacle_x.size();
	    pcl_pub.publish(cloud);
	 }

//receive a sensor_msgs::laserscan from the robot laser
//translate it to the right frame and project to a sensor_msgs::PointCloud2.
void subLaserScanCallback(const LaserScan::ConstPtr& scan_in) 
{
	//transform to the right frame
	if(!mTransformListener->waitForTransform(scan_in->header.frame_id, "/map", scan_in->header.stamp + ros::Duration().fromSec(scan_in->ranges.size()*scan_in->time_increment), ros::Duration(1.0)))	
	{
		return;
	}	
	
	//project to a sensor_msgs::PointCloud2 message 
	//for usage of the pcl library and forward compatibility
	PointCloud2 pointCloud;
	try
	{
		mLaserProjector.transformLaserScanToPointCloud("/map", *scan_in, pointCloud, *mTransformListener);
	}
	catch (tf::TransformException& e)
	{
		ROS_ERROR("obstacle_detector - %s", e.what());
	}

	if (pointCloud.width * pointCloud.height >0) 
	{		
		pointCloud.header.stamp = ros::Time::now();
        pointCloud.header.frame_id = "/map";
        
        pubCloud.publish(pointCloud);	
    }	
}

void Match_Filter::scanCB(const sensor_msgs::LaserScan::ConstPtr& scan){		
	sensor_msgs::PointCloud2 cloud_in,cloud_out;
	projector_.transformLaserScanToPointCloud("/lidar_link",*scan,cloud_in,tfListener_);
	rot_tf.header.frame_id = "/ChestLidar";
//	lidar mx28 axis aligned mode
//	rot_tf.transform.rotation.x = enc_tf_.getRotation().x();
//	rot_tf.transform.rotation.y = enc_tf_.getRotation().y();
//	rot_tf.transform.rotation.z = enc_tf_.getRotation().z();
//	rot_tf.transform.rotation.w = enc_tf_.getRotation().w();

//	lidar mx28 axis perpendicular modeg
	tf::Quaternion q1;
	q1.setRPY(-M_PI/2,0,0);
	tf::Transform m1(q1);
	tf::Quaternion q2(enc_tf_.getRotation().x(),enc_tf_.getRotation().y(),enc_tf_.getRotation().z(),enc_tf_.getRotation().w());
	tf::Transform m2(q2);

	tf::Transform m4;
	m4 = m2*m1; // rotate lidar axis and revolute mx28 axis
	rot_tf.transform.rotation.x = m4.getRotation().x();
	rot_tf.transform.rotation.y = m4.getRotation().y();
	rot_tf.transform.rotation.z = m4.getRotation().z();
	rot_tf.transform.rotation.w = m4.getRotation().w();
	
	tf2::doTransform(cloud_in,cloud_out,rot_tf);
	point_cloud_pub_.publish(cloud_out);		
	moving_now.data = dxl_ismove_;
	dxl_ismove_pub_.publish(moving_now);
}

void LaserscanMerger::scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan, std::string topic)
{
	sensor_msgs::PointCloud tmpCloud1,tmpCloud2;
	sensor_msgs::PointCloud2 tmpCloud3;

    // Verify that TF knows how to transform from the received scan to the destination scan frame
	tfListener_.waitForTransform(scan->header.frame_id.c_str(), destination_frame.c_str(), scan->header.stamp, ros::Duration(1));

	projector_.transformLaserScanToPointCloud(scan->header.frame_id, *scan, tmpCloud1, tfListener_);
	try
	{
		tfListener_.transformPointCloud(destination_frame.c_str(), tmpCloud1, tmpCloud2);
	}catch (tf::TransformException ex){ROS_ERROR("%s",ex.what());return;}

	for(int i=0; i<input_topics.size(); ++i)
	{
		if(topic.compare(input_topics[i]) == 0)
		{
			sensor_msgs::convertPointCloudToPointCloud2(tmpCloud2,tmpCloud3);
			pcl_conversions::toPCL(tmpCloud3, clouds[i]);
			clouds_modified[i] = true;
		}
	}	

    // Count how many scans we have
	int totalClouds = 0;
	for(int i=0; i<clouds_modified.size(); ++i)
		if(clouds_modified[i])
			++totalClouds;

    // Go ahead only if all subscribed scans have arrived
	if(totalClouds == clouds_modified.size())
	{
		pcl::PCLPointCloud2 merged_cloud = clouds[0];
		clouds_modified[0] = false;

		for(int i=1; i<clouds_modified.size(); ++i)
		{
			pcl::concatenatePointCloud(merged_cloud, clouds[i], merged_cloud);
			clouds_modified[i] = false;
		}
	
		point_cloud_publisher_.publish(merged_cloud);

		Eigen::MatrixXf points;
		getPointCloudAsEigen(merged_cloud,points);

		pointcloud_to_laserscan(points, &merged_cloud);
	}
}

        void scanCallback (const sensor_msgs::LaserScan::ConstPtr& scan_in)
        {
            if(!listener_.waitForTransform(
                        scan_in->header.frame_id,
                        base_frame,
                        scan_in->header.stamp + ros::Duration().fromSec(scan_in->ranges.size()*scan_in->time_increment),
                        ros::Duration(1.0))){
                return;
            }

            sensor_msgs::PointCloud2 cloud2;
            projector_.transformLaserScanToPointCloud(base_frame,*scan_in,
                    cloud2,listener_);
            scanPub.publish(cloud2);
        }

void LaserMerger::scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan, std::string topic)
{
	sensor_msgs::PointCloud tmpCloud1,tmpCloud2;

	tfListener_.waitForTransform(scan->header.frame_id.c_str(), destination_frame.c_str(), scan->header.stamp, ros::Duration(1));

	projector_.transformLaserScanToPointCloud(scan->header.frame_id, *scan, tmpCloud1, tfListener_);
	try
	{
		tfListener_.transformPointCloud(destination_frame.c_str(), tmpCloud1, tmpCloud2);
	}catch (tf::TransformException ex){ROS_ERROR("%s",ex.what());return;}

	for(int i=0; i<input_topics.size(); ++i)
	{
		if(topic.compare(input_topics[i]) == 0)
		{
			sensor_msgs::convertPointCloudToPointCloud2(tmpCloud2,clouds[i]);
			clouds_modified[i] = true;
		}
	}	

	int totalClouds = 0;
	for(int i=0; i<clouds_modified.size(); ++i)
		if(clouds_modified[i])
			++totalClouds;

	if(totalClouds == clouds_modified.size())
	{
		sensor_msgs::PointCloud2 merged_cloud = clouds[0];
		clouds_modified[0] = false;

		for(int i=1; i<clouds_modified.size(); ++i)
		{
			pcl::concatenatePointCloud(merged_cloud, clouds[i], merged_cloud);
			clouds_modified[i] = false;
		}
	
		point_cloud_publisher_.publish(merged_cloud);

		Eigen::MatrixXf points;
		getPointCloudAsEigen(merged_cloud,points);

		pointcloud_to_laserscan(points, &merged_cloud);
	}
}

void ScanCallback (const sensor_msgs::LaserScan::ConstPtr& msg){
	for(int i=0; i<1080; i=i+4){
		histlaser[(int)floor(i/4)]=msg->ranges[i];
		if(histlaser[i/4]>msg->range_max || histlaser[i/4]<msg->range_min) histlaser[i/4]=0.0;
		}
	static tf::TransformListener listener;
	static laser_geometry::LaserProjection projector;
	
 	try{
		sensor_msgs::PointCloud cloud;
		projector.transformLaserScanToPointCloud("map",*msg, cloud, listener);
	  	// Do something with cloud.
		drawData.laserScan.clear();
		for(unsigned int i=0; i<cloud.points.size(); i=i+4) drawData.laserScan.push_back(toimage(cloud.points[i].x,cloud.points[i].y));
    	}
    catch (tf::TransformException ex){
      	ROS_ERROR("%s",ex.what());
    	}
	}

  void scanCallback (const sensor_msgs::LaserScan::ConstPtr& scan_in)
  {
    sensor_msgs::PointCloud cloud;
    try
    {
        projector_.transformLaserScanToPointCloud(
          "base_link",*scan_in, cloud,listener_);
    }
    catch (tf::TransformException& e)
    {
        std::cout << e.what();
        return;
    }
    
    // Do something with cloud.

    scan_pub_.publish(cloud);

  }

void laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan, tf::TransformListener* listener) {
  ROS_DEBUG("get scan data");

  string fid = scan->header.frame_id;
  ROS_DEBUG_STREAM("scan->frame_id:" << fid << " reference frame_id:" << ref_frame_id);
  try {
    // LRFの最後の光線の時刻で変換できるようになるまで待つ
    bool is_timeout = !listener->waitForTransform(
        ref_frame_id, fid,
        scan->header.stamp + ros::Duration().fromSec(scan->ranges.size() * scan->time_increment),
        ros::Duration(3.0));
    if(is_timeout){
      ROS_WARN("waitForTransform timeout");
      return;
    }
  } catch (tf::TransformException& ex) {
    ROS_WARN("%s", ex.what());
    return;
  }

  sensor_msgs::PointCloud cloud;
  static laser_geometry::LaserProjection projector_;
  try{
    projector_.transformLaserScanToPointCloud(ref_frame_id, *scan, cloud, *listener);
  } catch (tf::TransformException& ex) {
    ROS_WARN("%s", ex.what());
    return;
  }

  // pclに変換できた点のLaserScanでのインデックス配列が格納されている,channelsインデックス
  int ch_i = -1;
  for (int i = 0; i < cloud.channels.size(); i++) {
    if ("index" == cloud.channels[i].name) {
      ch_i = i;
      break;
    }
  }
  if (-1 == ch_i) {
    ROS_ERROR("There is not 'index' channel in PointCloud converted from LaserScan");
  }

  // 四角形内のインデックスを記録
  vector<int> rip_idx;
  for (int i = 0; i < cloud.points.size(); i++) {
    float x = cloud.points.at(i).x;
    float y = cloud.points.at(i).y;
    if (xmin < x && x < xmax && ymin < y && y < ymax) {
      int tmp = (int)cloud.channels[ch_i].values[i];
      rip_idx.push_back(tmp);
    }
  }

  // インデックスに登録された点を削除，無効な距離を設定
  sensor_msgs::LaserScan scan_out = *scan;
  for (vector<int>::iterator it = rip_idx.begin(); it < rip_idx.end(); ++it) {
    int i = (int)*it;
    scan_out.ranges.at(i) = scan_out.range_max + 1.0;
  }

  pub.publish(scan_out);
}

	void ConvertLaser(const sensor_msgs::LaserScan::ConstPtr& scan_in)
	{
		sensor_msgs::LaserScan temp = *scan_in;
		sensor_msgs::LaserScan* scan = &temp;

		// Remove points that are to close and or the boat itself
		//	The LaserProjection class only removes points that are to far away
		//	It does not remove points that are to close
		//	To workaround this we iterate through the raw laser scan and change all values
		//	that are minimum value to above maximum value
		float angle = scan->angle_min;
		for(std::vector<float>::iterator it = scan->ranges.begin(); it != scan->ranges.end(); ++it)
		{
			// 1 cm epsilon
			if (*it <= scan->range_min + 0.01 || isSelf(*it, angle))
			{
				*it = scan->range_max + 0.01;
			}

			// Increment
			angle += scan->angle_increment;
		}

		//Convert to point cloud
		sensor_msgs::PointCloud2 cloud;					//Define a new pc msg
		bool transform_ready = false;
		try
		{
			//Wait until available
			if(transform_.waitForTransform(
						scan->header.frame_id,							//Transform from lidar
						frame_.c_str(),	 								//to frame_
																		//Most recent time (start_time + number_of_points * time_increment)
						scan->header.stamp + ros::Duration().fromSec(scan->ranges.size() * scan->time_increment),
						ros::Duration(2)))								//Timeout after 2 seconds
			{
				transform_ready = true;
			}
			else
			{
				ROS_WARN("Transform from %s to %s timed out", scan->header.frame_id.c_str(), frame_.c_str());
			}

		}
		catch (tf::TransformException& e)
		{
			ROS_ERROR("Transform error from %s to %s: %s", scan->header.frame_id.c_str(), frame_.c_str(), e.what());
		}

		//If we can transform then do it!
		if(transform_ready)
		{
			//Project laser scan into a point cloud in frame_
			try		// It is possible that some of the transforms are to far into the past
			{
				projector_.transformLaserScanToPointCloud(frame_.c_str(), *scan, cloud, transform_);
			}
			catch(tf::TransformException& e)
			{
				ROS_ERROR("Transform error from %s to %s: %s", scan->header.frame_id.c_str(), frame_.c_str(), e.what());
			}

			//Publish the cloud
			pc_pub_.publish(cloud);
		}
	}

	/***
	 *\brief Callback function that collects all scans and converts to pointcloud2
	 *\param  LaserScan
	 *
	 */
	void scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan_ptr) {

		sensor_msgs::LaserScan _scan_in = *scan_ptr;

		if (_scan_in.header.frame_id.compare("/laser") != 0) {
			ROS_WARN("An unknown source: %s", _scan_in.header.frame_id);
			return;
		}

		if (maxToProducePcdCount > 0 && producedPcdCount >= maxToProducePcdCount) {
			//ROS_WARN("Already %d point cloud produced", maxToProducePcdCount);
			bRunning = false;
			return;
		}

		ros::Time originalTime = _scan_in.header.stamp;
		_scan_in.header.stamp = originalTime - ros::Duration(0.05); // hack: Verzoegerung

		if (!_tfListener.waitForTransform( /* wait for possible TF */
				target_frame,
				_scan_in.header.frame_id,
				_scan_in.header.stamp, ros::Duration(0.01)))
		{
			resetCloudCollectionOnError();
			ROS_WARN("Timeout with waitForTransform");
			return;
		}

		sensor_msgs::PointCloud2 sub_cloud;
		tf::StampedTransform transform;

		try { /* transform laser scan to a sub_cloud */

			_laserProjector.transformLaserScanToPointCloud(
					target_frame,
					_scan_in,
					sub_cloud,
					_tfListener,
					-1.0, laser_geometry::channel_option::Default);

		} catch (tf::TransformException& exc) {
			resetCloudCollectionOnError();
			ROS_ERROR("Failed with transformLaserScanToPointCloud: %s", exc.what());
			return;
		}

		try {
			//Get the searched TF.  Throws exception on failure.
			_tfListener.lookupTransform(_scan_in.header.frame_id,
					target_frame, _scan_in.header.stamp, transform);
		} catch (tf::TransformException &ex) {
			resetCloudCollectionOnError();
			ROS_ERROR("Failed with lookupTransform %s\n", ex.what()); //Print exception which was caught
			return;
		}

		if (!isSkipThisCloud) {
			sensor_msgs::PointCloud2 tmp_cloud = _resulting_cloud;
			pcl::concatenatePointCloud(tmp_cloud, sub_cloud, _resulting_cloud);
		}

		float roll,pitch,yaw;
		getRPY(transform.getRotation(),roll,pitch,yaw);
		const double current_yaw = yaw;

		bool isCrossZeroIndicator = false;
		if ((0 < _the_last_2nd_yaw
				&& 0 < _the_last_1st_yaw
				&& 0 > current_yaw)  ||
			(0 > _the_last_2nd_yaw
				&& 0 > _the_last_1st_yaw
				&& 0 < current_yaw)) {

			isCrossZeroIndicator = true;

		}

		if (isCrossZeroIndicator) {
			// we have now a possible complete scan !
			if(!isSkipThisCloud)
			{
				producedPcdCount++;
				ROS_INFO("Collected %d cloud with size [%d,%d]", producedPcdCount, _resulting_cloud.width, _resulting_cloud.height);

				_resulting_cloud.header.stamp = _scan_in.header.stamp;
				_scan_pub.publish(_resulting_cloud);

				pcl::PointCloud<pcl::PointXYZI> cloud;
				pcl::fromROSMsg(_resulting_cloud, cloud);

				time_t rawtime;
				time(&rawtime);
				std::stringstream newtimestamp;
				newtimestamp << pcd_prefix << "_" << rawtime;
				//pcd/" + newtimestamp.str() + "
//.........这里部分代码省略.........