laser坐标系下的点，转到map坐标系下

法一：

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/PointStamped.h>
#include <sensor_msgs/LaserScan.h>
#include <vector>
#include <cmath>struct Point {int index;float range;float intensity;float angle;float x;float y;
};void scanCallback(const sensor_msgs::LaserScan::ConstPtr& scanMsg, tf::TransformListener& tf_listener, const std::string& map_frame_id, const std::string& laser_frame_id) {std::vector<Point> points;// 获取laser到map的转换矩阵tf::StampedTransform laser_to_map_transform;try {tf_listener.lookupTransform(map_frame_id, laser_frame_id, ros::Time(0), laser_to_map_transform);} catch (tf::TransformException &ex) {ROS_ERROR("%s", ex.what());ros::Duration(1.0).sleep();return;}for (int i = 0; i < scanMsg->ranges.size(); i++) {if (isfinite(scanMsg->ranges[i]) && scanMsg->intensities[i] > (scan_intensitie_ - intensitie_threshole_) &&scanMsg->ranges[i] < scanMsg->angle_max && scanMsg->ranges[i] > scanMsg->angle_min) {  // 强度阀值Point point;point.index = i;  // 记录高强点数据point.range = scanMsg->ranges[i];  // + RADIUS;  // 查找最大值确定圆心方法需要加半径，三角函数法不需要point.intensity = scanMsg->intensities[i];point.angle = scanMsg->angle_min + scanMsg->angle_increment * point.index;point.x = point.range * cos(point.angle);point.y = point.range * sin(point.angle);// 将点转换到map坐标系下geometry_msgs::PointStamped laser_point;laser_point.header.frame_id = laser_frame_id;laser_point.header.stamp = ros::Time();laser_point.point.x = point.x;laser_point.point.y = point.y;laser_point.point.z = 0.0;geometry_msgs::PointStamped map_point;try {tf_listener.transformPoint(map_frame_id, laser_point, map_point);point.x = map_point.point.x;point.y = map_point.point.y;} catch (tf::TransformException &ex) {ROS_ERROR("%s", ex.what());continue;}points.push_back(point);  // 将强度超过阀值的数据储存}}// 在这里你可以使用转换后的 points
}int main(int argc, char** argv) {ros::init(argc, argv, "scan_transformer");ros::NodeHandle nh;std::string map_frame_id = "map";std::string laser_frame_id = "laser";tf::TransformListener tf_listener;ros::Subscriber scan_sub = nh.subscribe<sensor_msgs::LaserScan>("scan", 1000, boost::bind(scanCallback, _1, boost::ref(tf_listener), map_frame_id, laser_frame_id));ros::spin();return 0;
}

法二:

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/PointStamped.h>
#include <sensor_msgs/LaserScan.h>
#include <vector>
#include <cmath>struct Point {int index;float range;float intensity;float angle;float x;float y;
};void scanCallback(const sensor_msgs::LaserScan::ConstPtr& scanMsg, tf::TransformListener& tf_listener, const std::string& map_frame_id, const std::string& laser_frame_id) {std::vector<Point> points;// 获取laser到map的转换矩阵tf::StampedTransform laser_to_map_transform;try {tf_listener.lookupTransform(map_frame_id, laser_frame_id, ros::Time(0), laser_to_map_transform);} catch (tf::TransformException &ex) {ROS_ERROR("%s", ex.what());ros::Duration(1.0).sleep();return;}for (int i = 0; i < scanMsg->ranges.size(); i++) {if (isfinite(scanMsg->ranges[i]) && scanMsg->intensities[i] > (scan_intensitie_ - intensitie_threshole_) &&scanMsg->ranges[i] < scanMsg->angle_max && scanMsg->ranges[i] > scanMsg->angle_min) {  // 强度阀值Point point;point.index = i;  // 记录高强点数据point.range = scanMsg->ranges[i];  // + RADIUS;  // 查找最大值确定圆心方法需要加半径，三角函数法不需要point.intensity = scanMsg->intensities[i];point.angle = scanMsg->angle_min + scanMsg->angle_increment * point.index;point.x = point.range * cos(point.angle);point.y = point.range * sin(point.angle);// 将点转换到map坐标系下geometry_msgs::PointStamped laser_point;laser_point.header.frame_id = laser_frame_id;laser_point.header.stamp = ros::Time();laser_point.point.x = point.x;laser_point.point.y = point.y;laser_point.point.z = 0.0;geometry_msgs::PointStamped map_point;try {tf_listener.transformPoint(map_frame_id, laser_point, map_point);point.x = map_point.point.x;point.y = map_point.point.y;} catch (tf::TransformException &ex) {ROS_ERROR("%s", ex.what());continue;}points.push_back(point);  // 将强度超过阀值的数据储存}}// 在这里你可以使用转换后的 points
}int main(int argc, char** argv) {ros::init(argc, argv, "scan_transformer");ros::NodeHandle nh;std::string map_frame_id = "map";std::string laser_frame_id = "laser";tf::TransformListener tf_listener;ros::Subscriber scan_sub = nh.subscribe<sensor_msgs::LaserScan>("scan", 1000, boost::bind(scanCallback, _1, boost::ref(tf_listener), map_frame_id, laser_frame_id));ros::spin();return 0;
}

在大多数情况下，tf::StampedTransform 不需要声明为静态的。你可以在函数内部使用局部变量来存储转换矩阵，只要每次调用函数时你都重新获取转换即可。