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 不需要声明为静态的。你可以在函数内部使用局部变量来存储转换矩阵，只要每次调用函数时你都重新获取转换即可。
使用局部变量的优点
- 线程安全：局部变量在函数调用时创建，在函数返回时销毁。这样可以确保每个线程都有自己的变量副本，避免并发访问问题。
- 简洁性：局部变量不需要额外的管理，只要在函数内部正确使用即可。
示例代码
以下是使用局部变量的示例代码：
总结
- 使用局部变量来存储
tf::StampedTransform是推荐的做法，可以确保线程安全并简化代码管理。 - 除非有特殊需求（如需要跨多个函数共享同一个转换矩阵），否则不需要将
tf::StampedTransform声明为静态变量。
/san 激光雷达数据转成全局坐标/map下的点云数据
std::vector<geometry_msgs::Point> msgs_points;
std::vector< Point > points; //当前反光板数据
//todo sukai 查找多个反光柱的雷达强度激光点，points3 （并转到map坐标系下），msgs_points 储存一个充电桩数据
void findHigh (const sensor_msgs::LaserScan::ConstPtr& scanMsg,std::vector< Point > &points3,std::vector<geometry_msgs::Point> &msgs_points) {
static tf::TransformListener tf_listener;
// 获取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;
// }
Point end;//todo 存在一个问题，就是只有一组连续点时，无法判断，这时需要加入一个未知点 end 来区分
end.index=-100;
std::vector< Point > points2; //各个反光柱激光信息
std::vector< Point > points; //各个反观柱坐标信息
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);
//points.push_back(point);//将强度超过阀值的数据储存
// 将点转换到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);//将强度超过阀值的数据储存
}else{
if(i>0 && i<scanMsg->ranges.size()-1){
if (isfinite( scanMsg->ranges[i]) && scanMsg->intensities[i-1] > (scan_intensitie_-intensitie_threshole_) && scanMsg->intensities[i+1] > (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);
//points.push_back(point);//将强度超过阀值的数据储存
// 将点转换到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);//将强度超过阀值的数据储存
}//if (scan.intensities[i-1] > (scan_intensitie_-intensitie_threshole_) && scan.intensities[i+1] > (scan_intensitie_-intensitie_threshole_))
}//if(i>0&&i<scan.ranges.size()-1)
}//if (scan.intensities[i] > (scan_intensitie_-intensitie_threshole_)){
}//for (int i = 0; i < scanMsg-isfinite( scanMsg->ranges[i]) &>ranges.size(); i++)
ROS_INFO("1.所有强度数据: [points.size(): %i]", points.size());
// ROS_INFO("强度值: [intensity: %f, 雷达索引: %i, 距离: %f]", findMaxPoint.intensity, findMaxPoint.index, findMaxPoint.range);
if(points.size() >points_size_threshold_){//points.size() > 2
int sum = points[0].index - 1;
points.push_back(end);//todo 存在一个问题，就是只有一组连续点时，无法判断，这时需要加入一个未知点 end 来区分
for (int j = 0; j< points.size(); j++) { //分离各反光柱数据
// ROS_INFO("2.统一组数据: [points[points.size()-1].index: %i, index: %i, sum: %i]", points[points.size()-1].index,sum, points[j].index);
if((points[j].index - sum)== 1) { //判断是否连续,todo 存在一个问题，就是只有一个点时
points2.push_back(points[j]); //连续点为同一组
// ROS_INFO("2.1 统一组数据: [points2.size(): %i, index: %i, sum: %i]", points2.size(),sum, points[j].index);
ROS_INFO("2.1 统一组数据: [x: %f, y: %f]", points[j].x, points[j].y);
}
else{
if(points2.size()<15){
points2.clear();//不满足点云数量就去除
points2.push_back(points[j]);//把最后一个点云加入集合
ROS_INFO("3.不满足点云数量就去除: [points2.size(): %i]", points2.size());
}else{
//出现不连续时即可处理一组数据
//可以处理一组强度数据
double threshold = 0.1; // 调整阈值以适应不同的数据
bool isCircularflg = isCircular(points, isCircular_threshold_);
std::cout << "Is Circular: " << (isCircularflg ? "True" : "False") << "\n";
// bool isCircularflg =isCircular(points2);//todo sukai 判断是否是圆
bool isLineflg =isLinear(points, isLine_threshold_);//判断是否是直线
if(isCircularflg){
ROS_INFO("4.1 isCircularflg 把同一组数据取出来加入集合: [points2.size(): %i]", points2.size());
}
if(isLineflg){
ROS_INFO("4.2 isLineflg 把同一组数据取出来加入集合: [points2.size(): %i]", points2.size());
}
ROS_INFO("4.3 把同一组数据取出来加入集合: [points2.size(): %i]", points2.size());
ROS_INFO("4.4 把同一组数据取出来加入集合: [points_size_threshold_: %i]", points_size_threshold_);
//不是圆也不是直线，排除环境中的反光柱与反光板，使用充电桩上的反光片
// if((!isCircularflg&&!isLineflg)&& points2.size() >points_size_threshold_){
if((!isCircularflg&&!isLineflg)){
ROS_INFO("4.5 把同一组数据取出来加入集合: [points_size_threshold_: %i]", points_size_threshold_);
for (int i = 0; i < points2.size(); i++) {
Point point=points2[i];
points3.push_back(point);//将强度超过阀值的数据储存
geometry_msgs::Point pt;
pt.x = point.x;
pt.y = point.y;
msgs_points.push_back(pt);//将强度超过阀值的数据储存
ROS_INFO("4.把同一组数据取出来加入集合: [points3.size(): %i]", points3.size());
}
return;//只需要满足条件的一个充电桩的反光片数据
}// if(!isCircularflg&&!isLineflg&&points2.size()>points_size_threshold_){
points2.clear();
points2.push_back(points[j]);//下一组开始
}// if((points[j].index - sum)== 1)
}//if(points2.size()<points_size_threshold_)
sum =points[j].index;
}// for (int j = 0; j< points.size(); j++)
}//if(points.size() >2)
else{
// std::cerr<<"无法找到有效数据" <<std::endl;
}
} std::vector<geometry_msgs::Point> msgs_points;
std::vector< Point > points; //当前反光板数据
//todo sukai 查找多个反光柱的雷达强度激光点，points3 （并转到map坐标系下），msgs_points 储存一个充电桩数据
void findHigh (const sensor_msgs::LaserScan::ConstPtr& scanMsg,std::vector< Point > &points3,std::vector<geometry_msgs::Point> &msgs_points) {
static tf::TransformListener tf_listener;
// 获取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;
// }
Point end;//todo 存在一个问题，就是只有一组连续点时，无法判断，这时需要加入一个未知点 end 来区分
end.index=-100;
std::vector< Point > points2; //各个反光柱激光信息
std::vector< Point > points; //各个反观柱坐标信息
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);
//points.push_back(point);//将强度超过阀值的数据储存
// 将点转换到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);//将强度超过阀值的数据储存
}else{
if(i>0 && i<scanMsg->ranges.size()-1){
if (isfinite( scanMsg->ranges[i]) && scanMsg->intensities[i-1] > (scan_intensitie_-intensitie_threshole_) && scanMsg->intensities[i+1] > (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);
//points.push_back(point);//将强度超过阀值的数据储存
// 将点转换到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);//将强度超过阀值的数据储存
}//if (scan.intensities[i-1] > (scan_intensitie_-intensitie_threshole_) && scan.intensities[i+1] > (scan_intensitie_-intensitie_threshole_))
}//if(i>0&&i<scan.ranges.size()-1)
}//if (scan.intensities[i] > (scan_intensitie_-intensitie_threshole_)){
}//for (int i = 0; i < scanMsg-isfinite( scanMsg->ranges[i]) &>ranges.size(); i++)
ROS_INFO("1.所有强度数据: [points.size(): %i]", points.size());
// ROS_INFO("强度值: [intensity: %f, 雷达索引: %i, 距离: %f]", findMaxPoint.intensity, findMaxPoint.index, findMaxPoint.range);
if(points.size() >points_size_threshold_){//points.size() > 2
int sum = points[0].index - 1;
points.push_back(end);//todo 存在一个问题，就是只有一组连续点时，无法判断，这时需要加入一个未知点 end 来区分
for (int j = 0; j< points.size(); j++) { //分离各反光柱数据
// ROS_INFO("2.统一组数据: [points[points.size()-1].index: %i, index: %i, sum: %i]", points[points.size()-1].index,sum, points[j].index);
if((points[j].index - sum)== 1) { //判断是否连续,todo 存在一个问题，就是只有一个点时
points2.push_back(points[j]); //连续点为同一组
// ROS_INFO("2.1 统一组数据: [points2.size(): %i, index: %i, sum: %i]", points2.size(),sum, points[j].index);
ROS_INFO("2.1 统一组数据: [x: %f, y: %f]", points[j].x, points[j].y);
}
else{
if(points2.size()<15){
points2.clear();//不满足点云数量就去除
points2.push_back(points[j]);//把最后一个点云加入集合
ROS_INFO("3.不满足点云数量就去除: [points2.size(): %i]", points2.size());
}else{
//出现不连续时即可处理一组数据
//可以处理一组强度数据
double threshold = 0.1; // 调整阈值以适应不同的数据
bool isCircularflg = isCircular(points, isCircular_threshold_);
std::cout << "Is Circular: " << (isCircularflg ? "True" : "False") << "\n";
// bool isCircularflg =isCircular(points2);//todo sukai 判断是否是圆
bool isLineflg =isLinear(points, isLine_threshold_);//判断是否是直线
if(isCircularflg){
ROS_INFO("4.1 isCircularflg 把同一组数据取出来加入集合: [points2.size(): %i]", points2.size());
}
if(isLineflg){
ROS_INFO("4.2 isLineflg 把同一组数据取出来加入集合: [points2.size(): %i]", points2.size());
}
ROS_INFO("4.3 把同一组数据取出来加入集合: [points2.size(): %i]", points2.size());
ROS_INFO("4.4 把同一组数据取出来加入集合: [points_size_threshold_: %i]", points_size_threshold_);
//不是圆也不是直线，排除环境中的反光柱与反光板，使用充电桩上的反光片
// if((!isCircularflg&&!isLineflg)&& points2.size() >points_size_threshold_){
if((!isCircularflg&&!isLineflg)){
ROS_INFO("4.5 把同一组数据取出来加入集合: [points_size_threshold_: %i]", points_size_threshold_);
for (int i = 0; i < points2.size(); i++) {
Point point=points2[i];
points3.push_back(point);//将强度超过阀值的数据储存
geometry_msgs::Point pt;
pt.x = point.x;
pt.y = point.y;
msgs_points.push_back(pt);//将强度超过阀值的数据储存
ROS_INFO("4.把同一组数据取出来加入集合: [points3.size(): %i]", points3.size());
}
return;//只需要满足条件的一个充电桩的反光片数据
}// if(!isCircularflg&&!isLineflg&&points2.size()>points_size_threshold_){
points2.clear();
points2.push_back(points[j]);//下一组开始
}// if((points[j].index - sum)== 1)
}//if(points2.size()<points_size_threshold_)
sum =points[j].index;
}// for (int j = 0; j< points.size(); j++)
}//if(points.size() >2)
else{
// std::cerr<<"无法找到有效数据" <<std::endl;
}
}//将点的向量转换为PointCloud2消息 【收到的点已经是map坐标系】
void convertAndPublish(const std::vector<geometry_msgs::Point>& points,sensor_msgs::PointCloud2 &cloud_msg ) {
cloud_msg.header.stamp = ros::Time::now();
cloud_msg.header.frame_id =map_frame_id_ ; // 根据需要设置frame_id laser_frame_id_
// 设置点云消息的字段
cloud_msg.height = 1;
cloud_msg.width = points.size();
cloud_msg.is_dense = false;
sensor_msgs::PointCloud2Modifier modifier(cloud_msg);
modifier.setPointCloud2FieldsByString(1, "xyz");
modifier.resize(points.size());
// 使用迭代器填充点云消息的数据
sensor_msgs::PointCloud2Iterator<float> iter_x(cloud_msg, "x");
sensor_msgs::PointCloud2Iterator<float> iter_y(cloud_msg, "y");
sensor_msgs::PointCloud2Iterator<float> iter_z(cloud_msg, "z");
for (const auto& point : points) {
*iter_x = point.x;
*iter_y = point.y;
*iter_z = point.z;
++iter_x;
++iter_y;
++iter_z;
}
}void handleInitialPose(const trilateration::chargeworker_srvs::Request& req, trilateration::chargeworker_srvs::Response& re) {
isShowScan_=true;//获取坐标
std::this_thread::sleep_for(std::chrono::milliseconds(5000));
static tf::TransformListener tf_listener_chargeWorker;
int tf_listenerFig = false;
try {
tf::StampedTransform chargeWorker_to_map_now_transform;//mapTchargeWorker_now
tf::StampedTransform chargeWorker_to_base_link_now_transform;//base_linkTchargeWorker_now
ros::Duration tolerance(1.0); // 1 second tolerance
ros::Time common_time;
tf_listener_chargeWorker.waitForTransform(map_frame_id_, chargeWorker_frame_id_, ros::Time(), ros::Duration(2.0));
tf_listener_chargeWorker.waitForTransform(base_link_frame_id_, chargeWorker_frame_id_, ros::Time(), ros::Duration(2.0));
tf_listener_chargeWorker.getLatestCommonTime(map_frame_id_, chargeWorker_frame_id_, common_time, nullptr);
// Attempt to lookup the transform within the tolerance window
ros::Time start_time = common_time - tolerance;
ros::Time end_time = common_time + tolerance;
for (ros::Time t = start_time; t <= end_time; t += ros::Duration(0.1)) {
try {
//listener.lookupTransform(target_frame, source_frame, t, transform);
//mapTlaser
// tf_listener_chargeWorker.lookupTransform(map_frame_id_, chargeWorker_frame_id_, ros::Time(0), chargeWorker_to_map_transform);
//mapTlaser
tf_listener_chargeWorker.lookupTransform(map_frame_id_, chargeWorker_frame_id_, t, chargeWorker_to_map_now_transform);
std::this_thread::sleep_for(std::chrono::milliseconds(100));
tf_listener_chargeWorker.lookupTransform(base_link_frame_id_, chargeWorker_frame_id_, t, chargeWorker_to_base_link_now_transform);
tf_listenerFig = true;
// return true;
} catch (tf::TransformException& ex) {
// Continue trying within the window
ROS_WARN("%s", ex.what());
std::this_thread::sleep_for(std::chrono::milliseconds(100));
continue;
}
}
if(!tf_listenerFig){
re.result="error";
re.success=false;
isShowScan_=false;
return;
}
// mapTchargeWorker 通过充电桩反向计算位置
tf::StampedTransform chargeWorker_to_map_transform;
//mapTchargeWorker
bool loadTransformFromTxtfig = loadTransformFromTxt(chargeWorker_to_map_transform, chargeWorkerPose_path_);
if(loadTransformFromTxtfig){
//1.设（差异）变换矩阵 chargeWorker_nowTchargeWorker
tf::Transform chargeWorker_nowTchargeWorker = chargeWorker_to_map_now_transform.inverse() * chargeWorker_to_map_transform;
//计算 mapTbaselink
tf::Transform mapTbaselinkTransform = chargeWorker_to_map_transform * chargeWorker_nowTchargeWorker.inverse() * chargeWorker_to_base_link_now_transform.inverse();
re.resultTxt="chargeWorkerTbaselink: "+transformToString(chargeWorker_to_base_link_now_transform);
re.resul_english="mapTbaselink: "+transformToString(mapTbaselinkTransform);
re.result_odom_model_type="mapTchargeWorker: "+transformToString(chargeWorker_to_map_transform);
// 构建PoseWithCovarianceStamped消息
geometry_msgs::PoseWithCovarianceStamped initial_pose_msg;
initial_pose_msg.header.stamp = ros::Time::now(); // 当前时间戳
initial_pose_msg.header.frame_id = map_frame_id_; // 父坐标系为map
// 获取 transform 的平移部分
const tf::Vector3& origin = mapTbaselinkTransform.getOrigin();
initial_pose_msg.pose.pose.position.x = origin.x();
initial_pose_msg.pose.pose.position.y = origin.y();
initial_pose_msg.pose.pose.position.z = origin.z();
// 获取 transform 的旋转部分
const tf::Quaternion& quat = mapTbaselinkTransform.getRotation();
initial_pose_msg.pose.pose.orientation.x = quat.x();
initial_pose_msg.pose.pose.orientation.y = quat.y();
initial_pose_msg.pose.pose.orientation.z = quat.z();
initial_pose_msg.pose.pose.orientation.w = quat.w();
// 设置协方差矩阵（这里为示例值，实际应用中应根据传感器精度调整）
initial_pose_msg.pose.covariance = {1e-9, 0, 0, 0, 0, 0,
0, 1e-3, 1e-9, 0, 0, 0,
0, 0, 1e6, 0, 0, 0,
0, 0, 0, 1e6, 0, 0,
0, 0, 0, 0, 1e6, 0,
0, 0, 0, 0, 0, 1e-9};
// 发布消息
initial_pose_pub.publish(initial_pose_msg);
}else{
re.result="error";
re.success=false;
isShowScan_=false;
return;
}
}catch (tf::TransformException& ex)
{
ROS_ERROR_STREAM("TF Error: " << ex.what());
re.result="error";
re.success=false;
isShowScan_=false;
return;
}
re.result="ok";
re.success=true;
isShowScan_=false;
}
7224
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
