C++ TransformListener::transformPointCloud方法代码示例

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 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 PointCloud2ToPointCloud::Callback(const sensor_msgs::PointCloud2ConstPtr& input)
{
  sensor_msgs::PointCloud output , output_tf;
  sensor_msgs::convertPointCloud2ToPointCloud(*input,output);

  if(!listener.waitForTransform( output.header.frame_id, "/odom", 
    input->header.stamp + ros::Duration().fromSec(0.1), ros::Duration(1.0))){
    return;
  }  

  try{
      listener.transformPointCloud("/odom",output, output_tf);
      pointcloud_pub.publish(output_tf);
  }
  catch (tf::TransformException ex){
      ROS_ERROR("%s",ex.what());
  }

}

	//  Callback to register with tf::MessageFilter to be called when transforms are available
	void cloud_cb(const boost::shared_ptr<const sensor_msgs::PointCloud2>& input) 
	{
	    sensor_msgs::PointCloud2 output;
	    try 
	    {
		tf_.transformPointCloud(target_frame_, *input, output);
		pub_.publish(output);
/*
		printf("point of turtle 3 in frame of turtle 1 Position(x:%f y:%f z:%f)\n", 
			point_out.point.x,
			point_out.point.y,
			point_out.point.z);
*/
			}
	    catch (tf::TransformException &ex) 
	    {
		printf ("Failure %s\n", ex.what()); //Print exception which was caught
	    }
	};

void GridConstructionNode::scanCallback (const sm::LaserScan& scan)
{
    try {

        // We'll need the transform between sensor and fixed frames at the time when the scan was taken
        if (!tf_.waitForTransform(fixed_frame_, sensor_frame_, scan.header.stamp, ros::Duration(1.0)))
        {
            ROS_WARN_STREAM ("Timed out waiting for transform from " << sensor_frame_ << " to "
                             << fixed_frame_ << " at " << scan.header.stamp.toSec());
            return;
        }

        // Figure out current sensor position
        tf::Pose identity(tf::createIdentityQuaternion(), tf::Vector3(0, 0, 0));
        tf::Stamped<tf::Pose> odom_pose;
        tf_.transformPose(fixed_frame_, tf::Stamped<tf::Pose> (identity, ros::Time(), sensor_frame_), odom_pose);

        // Project scan from sensor frame (which varies over time) to odom (which doesn't)
        sm::PointCloud fixed_frame_cloud;
        laser_geometry::LaserProjection projector_;
        projector_.transformLaserScanToPointCloud (fixed_frame_, scan, fixed_frame_cloud, tf_);

        // Now transform back into sensor frame at a single time point
        sm::PointCloud sensor_frame_cloud;
        tf_.transformPointCloud (sensor_frame_, scan.header.stamp, fixed_frame_cloud, fixed_frame_,
                                 sensor_frame_cloud);

        // Construct and save LocalizedCloud
        CloudPtr loc_cloud(new gu::LocalizedCloud());
        loc_cloud->cloud.points = sensor_frame_cloud.points;
        tf::poseTFToMsg(odom_pose, loc_cloud->sensor_pose);
        loc_cloud->header.frame_id = fixed_frame_;
        Lock lock(mutex_);
        last_cloud_=loc_cloud;
    }
    catch (tf::TransformException& e) {
        ROS_INFO ("Not saving scan due to tf lookup exception: %s",
                  e.what());
    }
}

/*
 * Filter robot out of the current scan
 */
void NiftiLaserFiltering::robot_filter(sensor_msgs::LaserScan& scan)
{
	const double invalid = scan.range_max+1;
	std::list<double> blacklisted;
	sensor_msgs::PointCloud close_ptcld;
	sensor_msgs::PointCloud transformed_ptcld;
	close_ptcld.header = scan.header;
	sensor_msgs::ChannelFloat32 index;
	index.name = "index";
	geometry_msgs::Point32 point;
	point.z = 0;
	double angle;
	const ros::Time time = scan.header.stamp;

	double max_dist = 0.7;
	if (has_arm) { // when extended the end of the arm is further away
		max_dist = 1.4;
	}
	double eps = 0.015;

	// create a small point cloud with only the close points
	for (unsigned int i=0; i<scan.ranges.size(); i++) {
		if (scan.ranges[i]<max_dist) {
			angle = scan.angle_min + i*scan.angle_increment;
			point.x = scan.ranges[i]*cos(angle);
			point.y = scan.ranges[i]*sin(angle);
			close_ptcld.points.push_back(point);
			index.values.push_back(i);
		}
	}
	close_ptcld.channels.push_back(index);
	//std::cout << close_ptcld.points.size() << std::endl;

	double x, y, z;
	// remove body
	transformed_ptcld = close_ptcld;
	for (unsigned int i=0; i<transformed_ptcld.points.size(); i++) {
		x = transformed_ptcld.points[i].x;
		y = transformed_ptcld.points[i].y;
		z = transformed_ptcld.points[i].z;
		if ((fabs(y)<eps+0.15)&&(fabs(z)<eps+0.10)&&(x<eps+0.05))
			blacklisted.push_back(transformed_ptcld.channels[0].values[i]);
	}
	// remove left track
	if (tf_listener.waitForTransform(laser_frame, "/left_track", time,
			ros::Duration(1.))) {
		tf_listener.transformPointCloud("/left_track", close_ptcld, transformed_ptcld);
		for (unsigned int i=0; i<transformed_ptcld.points.size(); i++) {
			x = transformed_ptcld.points[i].x;
			y = transformed_ptcld.points[i].y;
			z = transformed_ptcld.points[i].z;
			if
			((fabs(y)<eps+(0.097/2))&&(fabs(x)<eps+0.09+(0.4997/2))&&(z>-eps-0.0705)&&(z<eps+0.1095))
				blacklisted.push_back(transformed_ptcld.channels[0].values[i]);
		}
	} else {
		ROS_WARN_STREAM("Timeout (1s) while waiting between "<<laser_frame<<
				" and /left_track for robot_filter (ignoring this part).");
	}
	// remove left track
	if (tf_listener.waitForTransform(laser_frame, "/right_track", time,
			ros::Duration(1.))) {
		tf_listener.transformPointCloud("/right_track", close_ptcld, transformed_ptcld);
		for (unsigned int i=0; i<transformed_ptcld.points.size(); i++) {
			x = transformed_ptcld.points[i].x;
			y = transformed_ptcld.points[i].y;
			z = transformed_ptcld.points[i].z;
			if
			((fabs(y)<eps+(0.097/2))&&(fabs(x)<eps+0.09+(0.4997/2))&&(z>-eps-0.0705)&&(z<eps+0.1095))
				blacklisted.push_back(transformed_ptcld.channels[0].values[i]);
		}
	} else {
		ROS_WARN_STREAM("Timeout (1s) while waiting between "<<laser_frame<<
				" and /right_track for robot_filter (ignoring this part).");
	}

	// remove front left flipper
	if (tf_listener.waitForTransform(laser_frame, "/front_left_flipper", time,
			ros::Duration(1.))) {
		tf_listener.transformPointCloud("/front_left_flipper", close_ptcld, transformed_ptcld);
		for (unsigned int i=0; i<transformed_ptcld.points.size(); i++) {
			x = transformed_ptcld.points[i].x;
			y = transformed_ptcld.points[i].y;
			z = transformed_ptcld.points[i].z;
			if ((fabs(y)<eps+(0.050/2))&&(x>-eps-0.090)&&(x<eps+0.3476)&&
					((x*sin(11.1*M_PI/180)+fabs(z)*cos(11.1*M_PI/180))<3*eps+0.090))
				blacklisted.push_back(transformed_ptcld.channels[0].values[i]);
		}
	} else {
		ROS_WARN_STREAM("Timeout (1s) while waiting between "<<laser_frame<<
				" and /front_left_flipper for robot_filter (ignoring this part).");
	}
	// remove front right flipper
	if (tf_listener.waitForTransform(laser_frame, "/front_right_flipper", time,
			ros::Duration(1.))) {
		tf_listener.transformPointCloud("/front_right_flipper", close_ptcld, transformed_ptcld);
		for (unsigned int i=0; i<transformed_ptcld.points.size(); i++) {
//.........这里部分代码省略.........