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

原创

已于 2022-12-01 10:27:03 修改 · 823 阅读

3 ·

CC 4.0 BY-SA版权

文章标签：

#ros

于 2020-12-25 10:17:50 首次发布

#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::S