千言万语离不开一句话tfBroadcaster.sendTransform(odomTrans);
1.其中tfBroadcaster为专门用来发布广播的对象.
需要进行这样的声明tf::TransformBroadcaster tfBroadcaster;
2.odomTrans则包含了坐标之间的关系信息.
他是需要这样声明的nav_msgs::Odometry odomData;
3.坐标系描述谁的关系,关系咋样?这个要说清楚
odomTrans.frame_id_ = "map"; //全局坐标
odomTrans.child_frame_id_ = "sensor"; //本地坐标的id,也就是自己
odomTrans.stamp_ = odomTime; //时间戳
odomTrans.setRotation(tf::Quaternion(geoQuat.x, geoQuat.y, geoQuat.z, geoQuat.w));
//发布四元数(旋转)
odomTrans.setOrigin(tf::Vector3(vehicleX, vehicleY, vehicleZ));
//发布xyz偏移量(平移)4.发布
PS:旋转平移的相关参数是没有办法直接获得的,它们是通过imu,激光雷达估计或者其他方式进行推断的.
值得一提的是其实对于ros来说没有所谓的世界坐标系.
例如:
1.你可以以"世界坐标系"为世界坐标系,也可以以机器人的base_link为世界坐标
2.但是可以以fram_id来划分不同的世界,这个和上面在同一个rviz可以调出来.根据母frame(parent_frame)划分不同的世界.一个母frame一个世界.
机器人通过第一个标题将base_link与世界坐标系绑定后,后续再有像是激光雷达和camera这种定死在base_link上面的可以通过tf包来绑定,这样就不用重复地发布tf坐标了.
<node pkg="tf" type="static_transform_publisher" name="vehicleTransPublisher"
args="-$(arg sensorOffsetX) -$(arg sensorOffsetY) 0 0 0 0 /sensor /vehicle 1000"/>
name表示tf这个可执行文件以什么名字作为节点的名称向tf工具来发布话题
args前6个分别是相对于base_link的平移和旋转量
/sensor是base_link
/vehicle是传感器的名称,这里可以理解为camera
甚至可以莫须有一个坐标系出来,它不依托物质基础,你想在哪里设置坐标系就设置一个
像下面这张图黄色这根线长出天际,是因为我在其很高的上方设置了一个坐标系.
以古月居的广播教学为例子https://www.guyuehome.com/34664
turtle_tf_listener.cpp中
listener.waitForTransform("/turtle2", "/turtle1", ros::Time(0), ros::Duration(3.0));
//等待响应
listener.lookupTransform("/turtle2", "/turtle1", ros::Time(0), transform);
//坐标信息交给transform
transform.getOrigin().y() //获得目标y坐标
transform.getOrigin().x() //获得目标x坐标
transform.getOrigin().z() //获得目标z坐标
w = transform.getRotation().getW(); //获得四元数
x = transform.getRotation().getX();
y = transform.getRotation().getY();
z = transform.getRotation().getZ();
/*四元数转rpy*/
tf::Quaternion q(x,y,z,w);
double roll, pitch, yaw;//定义存储r\p\y的容器
tf::Matrix3x3(q).getRPY(roll, pitch, yaw);//进行转换参考链接:1.tf(Transform Frame)变换_wanghua609的博客-CSDN博客_lookuptransform
2.PCL：旋转、平移点云_通哈膨胀哈哈哈的博客-CSDN博客_pcl点云旋转
下面这个是将2D雷达进行旋转平移的例子,代码在下方
其中有意思的是:
1.由于消息是从gazebo发出来的,似乎直接接受到的话题消息不需要进行旋转(而现实当中是需要旋转的)
图(一)
图(二)
左边白色为map坐标系下的点云信息；右边为雷达坐标系下的点云信息(未经过旋转平移,所以点云分布在原点附近).
两图的小车方向显然发生了变化(可以看Axes红绿蓝三轴的朝向不同了),但是点云并不会以小车进行旋转.=>因此这里点云只需要进行平移,不需要进行旋转.
2.代码分析
listener.lookupTransform("/map", "/sensor", now, transform);
这句话是为了获取 transform的,我们发现点云都分布在map原点附近,而不是分布在小车附近,这是我们需要改变的点.因此我们需要把所有点云坐标加上从原点指到小车的这个向量,那么就可以完成我们的需求.因此这句代码targe_frame是"/map",source_frame为"/sensor".这样能完成点云从sensor坐标系向map坐标系的变换.
x=transform.getOrigin().x();
y=transform.getOrigin().y();
z=transform.getOrigin().z();
transform_me.translation() << x, y, 0;
pcl::transformPointCloud (cloud, cloud, transform_me);3.重要的东西!!!时间戳
如果说将接受到的雷达信息传进来,再进行坐标变换,那么从雷达信息生成的时间点到开始进行坐标变换的这个时刻存在时间差!!!!会造成定位不准,白瞎了激光雷达的性能.
因此,要保证雷达点云生成的时间戳和tf关系的时间戳尽量接近.
ros::Time now = input.header.stamp;
listener.lookupTransform("/map", "/sensor", now, transform);将等待tf时间戳的输入设为雷达信息的时间戳stamp.
//这段代码订阅laserscan转成pointcloud2
#include <ros/ros.h>
#include <iostream>
#include <pcl/visualization/cloud_viewer.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/LaserScan.h>
#include <pcl/filters/voxel_grid.h>
#include <tf/transform_listener.h>
#include <pcl/common/transforms.h>
#include <pcl/filters/passthrough.h>
// #include <geometry_msgs/TwistStamped.h>
#define PI 3.14159265359
std::vector<int> scanInd;
class cloudHandler
{
public:
pcl::PointCloud<pcl::PointXYZI> cloud;
pcl::PointCloud<pcl::PointXYZI> copy_cloud;
pcl::PointCloud<pcl::PointXYZI> TFED_cloud;
tf::TransformListener listener;
tf::StampedTransform transform;
double x,y,z;
double roll, pitch, yaw;
cloudHandler()
{
pcl_sub = nh.subscribe("/2D_Lascan", 100, &cloudHandler::cloudCB, this);
pcl_pub = nh.advertise<sensor_msgs::PointCloud2>("/registered_scan_tem", 2);
}
void cloudCB(const sensor_msgs::LaserScan &input) //将ros消息转为pcl点云格式后将其在viwer窗口上表现出来
{
cloud.clear();
copy_cloud.clear();
std::vector<float> ranges = input.ranges;
cloud.points.resize(ranges.size());
//?? 这里给pcl2声明容量
cloud.width = ranges.size();
//?? 这里给pcl2 width,heighy赋值
cloud.height = 1;
cloud.header.stamp = input.header.stamp.toSec();
//?? 这里给pcl2时间戳赋值
cloud.header.frame_id = "map";
//?? 这里给pcl2设置发送ID
/*--------------这段进行"角度range"到(x,y)坐标的转换---------------*/
//转换到二维XY平面坐标系下;
// std::cout<<"cloud其他初始化"<<std::endl;
for(int i=0; i< ranges.size(); i++)
{
double angle = input.angle_min + i * input.angle_increment;
double lX = ranges[i] * cos(angle);
double lY = ranges[i] * sin(angle);
float intensity = input.intensities[i];
// std::cout << ranges[i] << " , " << std::endl;
cloud.points[i].x=lX;
cloud.points[i].y=lY;
cloud.points[i].z=0;
cloud.points[i].intensity=intensity;
}
/*--------------这段进行"角度range"到(x,y)坐标的转换---------------*/
/*----------------------------------------------------旋转平移----------------------------------------------------------------*/
try
{
//查询是否有这两个坐标系，查询当前时间，如果超过3s则报错
ros::Time now = input.header.stamp;
std::cout<<now<<std::endl;
std::cout<<ros::Time(0)<<std::endl;
listener.waitForTransform("/map", "/sensor", now, ros::Duration(3.0));
listener.lookupTransform("/map", "/sensor", now, transform); //将点云加上map到激光雷达的向量,进行平移
x=transform.getOrigin().x();
y=transform.getOrigin().y();
z=transform.getOrigin().z();
tf::Quaternion q(transform.getRotation().getX(),transform.getRotation().getY(),transform.getRotation().getZ(),transform.getRotation().getW());
tf::Matrix3x3(q).getRPY(roll, pitch, yaw);
Eigen::Affine3f transform_me = Eigen::Affine3f::Identity();
transform_me.translation() << x, y, 0; //将点云加上map到激光雷达的向量,进行平移
// transform_me.rotate (Eigen::AngleAxisf (roll, Eigen::Vector3f::UnitX()));//这边不需要旋转,因为2D雷达的消息直接和map的方向对上了,这我也很迷为什么对上了
pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
pcl::transformPointCloud (cloud, cloud, transform_me);
/*------------------------------增加立体感------------------------------------------*/
pcl::copyPointCloud(cloud,copy_cloud);
for(int h=0;h<=20;h+=2)
{
for(int i=0;i<copy_cloud.points.size();i++)
{
copy_cloud.points[i].z=(float)h/10;
}
cloud+=copy_cloud;
}
/*------------------------------增加立体感------------------------------------------*/
sensor_msgs::PointCloud2 cloud_pu;
pcl::toROSMsg(cloud, cloud_pu);
pcl_pub.publish(cloud_pu);
}
catch (tf::TransformException &ex)
{
ROS_ERROR("%s",ex.what());
// ros::Duration(1.0).sleep();
// continue;
}
/*----------------------------------------------------旋转平移----------------------------------------------------------------*/
}
protected:
ros::NodeHandle nh;
ros::Subscriber pcl_sub;
ros::Subscriber subSpeed;
ros::Subscriber subTerrainCloud;
pcl::visualization::CloudViewer viewer; //标题
ros::Timer viewer_timer;
ros::Publisher pcl_pub;
};
main (int argc, char **argv)
{
ros::init (argc, argv, "pcl_visualize");
cloudHandler handler;
ros::spin();
return 0;
}五.怎么获取base_link相对于世界坐标系的偏移量?