ros点的坐标转换PointStamped和自己计算

#include <tf/transform_listener.h>
#include <tf/transform_datatypes.h>
#include <ros/ros.h>
#include <iostream>

using namespace std;

int main(int argc, char **argv) {
	ros::init(argc,argv,"transform_point");
	ros::NodeHandle n;
	tf::TransformListener listener;
	geometry_msgs::PointStamped point_source, point_target;
	point_source.point.x = 1.3;
	point_source.point.y = 0.25;
	point_source.point.z = 0;
	point_source.header.frame_id = "base_footprint";
	point_source.header.stamp = ros::Time();
	while(ros::ok()) {
		try {
    		listener.transformPoint("target_frame", point_source, point_target);
    		cout << point_target.point.x << endl;
    		cout << point_target.point.y << endl;
    		cout << point_target.point.z << endl << endl;
    		return 0;
    	} catch(tf::TransformException& ex) {
    		ROS_ERROR("%s", ex.what());
    		//return 0;
    	}
    }
	return 0;
}

• 和自己计算的比较

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <tf/transform_datatypes.h>
#include <sensor_msgs/LaserScan.h>

class Callback
{
public:
  tf::TransformListener listener1;
  tf::TransformListener listener2;
  tf::StampedTransform transform;
  ros::Subscriber sub1;
  ros::Subscriber sub2;

  Callback(ros::NodeHandle n)
  {
    sub1 = n.subscribe<sensor_msgs::LaserScan>("/scan", 1, &Callback::CB1, this);
    sub2 = n.subscribe<sensor_msgs::LaserScan>("/scan", 1, &Callback::CB2, this);
  }
  void CB1(const sensor_msgs::LaserScanConstPtr &scan);
  void CB2(const sensor_msgs::LaserScanConstPtr &scan);
};

void Callback::CB1(const sensor_msgs::LaserScanConstPtr &scan)
{
	double x = 2.8, y =-1.3, z = 1.1;
	ROS_INFO("1");
	ROS_INFO(std::to_string(scan->header.seq).c_str());
	geometry_msgs::PointStamped point_source, point_target;
	point_source.point.x = x;
	point_source.point.y = y;
	point_source.point.z = z;
	point_source.header.frame_id = "base_link";
	point_source.header.stamp = ros::Time();
	try{
		listener1.transformPoint("map", point_source, point_target);
		ROS_INFO(std::to_string(point_target.point.x).c_str());
		ROS_INFO(std::to_string(point_target.point.y).c_str());
		ROS_INFO(std::to_string(point_target.point.z).c_str());
	} catch(tf::TransformException& ex) {
		return;
	}
}

void Callback::CB2(const sensor_msgs::LaserScanConstPtr &scan)
{
	double x = 2.8, y = -1.3, z = 1.1;
	ROS_INFO("2");
    ROS_INFO(std::to_string(scan->header.seq).c_str());
	try {
		listener2.waitForTransform("/map",
                              "/base_link",
                              ros::Time(0), ros::Duration(0.2));
    	listener2.lookupTransform("map",
                             "base_link",
                             ros::Time(0), transform);
	} catch(tf::TransformException& ex) {
		return;
	}
	double R[4][4];
	double qx = transform.getRotation().x(), qy = transform.getRotation().y(),
          qz = transform.getRotation().z(), qw = transform.getRotation().w();
	double scale = 1 / sqrt(qx * qx + qy * qy + qz * qz + qw * qw);
    qx *= scale, qy *= scale, qz *= scale, qw *= scale;
    R[0][0] = 1 - 2 * qy * qy - 2 * qz * qz;
    R[0][1] = 2 * qx * qy - 2 * qz * qw;
    R[0][2] = 2 * qx * qz + 2 * qy * qw;
    R[0][3] = 0;
    R[1][0] = 2 * qx * qy + 2 * qz * qw;
    R[1][1] = 1 - 2 * qx * qx - 2 * qz * qz;
    R[1][2] = 2.0f * qy * qz - 2.0f * qx * qw;
    R[1][3] = 0;
    R[2][0] = 2.0f * qx * qz - 2.0f * qy * qw;
    R[2][1] = 2.0f * qy * qz + 2.0f * qx * qw;
    R[2][2] = 1.0f - 2.0f * qx * qx - 2.0f * qy * qy;
    R[2][3] = R[3][0] = R[3][1] = R[3][2] = 0;
    R[3][3] = 1;
    double xx = x * R[0][0] + y * R[0][1] + z * R[0][2] + R[0][3] +
               transform.getOrigin().x();
    double yy = x * R[1][0] + y * R[1][1] + z * R[1][2] + R[1][3] +
               transform.getOrigin().y();
    double zz = x * R[2][0] + y * R[2][1] + z * R[2][2] + R[2][3] +
               transform.getOrigin().z();
	ROS_INFO(std::to_string(xx).c_str());
	ROS_INFO(std::to_string(yy).c_str());
	ROS_INFO(std::to_string(zz).c_str());
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "lwd_tf");
  ros::NodeHandle n;
  Callback cb(n);
  ros::spin();
  return 0;
}