1.坐标msg消息:
订阅发布模型中数据载体 msg 是一个重要实现,首先需要了解一下,在坐标转换实现中常用的 msg:geometry_msgs/TransformStamped
和geometry_msgs/PointStamped,
前者用于传输坐标系相关位置信息,后者用于传输某个坐标系内坐标点的信息。在坐标变换中,频繁的需要使用到坐标系的相对关系以及坐标点信息。
输入rosmsg info geometry_msgs/TransformStamped,可以看到显示
std_msgs/Header header #头信息
uint32 seq #|-- 序列号
time stamp #|-- 时间戳
string frame_id #|-- 坐标 ID
string child_frame_id #子坐标系的 id
geometry_msgs/Transform transform #坐标信息
geometry_msgs/Vector3 translation #偏移量
float64 x #|-- X 方向的偏移量
float64 y #|-- Y 方向的偏移量
float64 z #|-- Z 方向上的偏移量
geometry_msgs/Quaternion rotation #四元数
float64 x
float64 y
float64 z
float64 w
四元数用于表示坐标的相对姿态。
输入rosmsg info geometry_msgs/PointStamped,可以看到显示
std_msgs/Header header #头
uint32 seq #|-- 序号
time stamp #|-- 时间戳
string frame_id #|-- 所属坐标系的 id
geometry_msgs/Point point #点坐标
float64 x #|-- x y z 坐标
float64 y
float64 z
2.TF静态坐标变换:
实现:
现有一机器人模型,核心构成包含主体与雷达,各对应一坐标系,坐标系的原点分别位于主体与雷达的物理中心,已知雷达原点相对于主体原点位移关系如下: x 0.2 y0.0 z0.5。当前雷达检测到一障碍物,在雷达坐标系中障碍物的坐标为 (2.0 3.0 5.0),请问,求障碍物相对于主体的坐标是多少?
2.1
实现分析:
实现流程(C++ 实现):
2.2创建功能包,依赖 tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs(创包过程与配置过程不赘述,参照过往文章):
2.3发布者实现:
/*
静态坐标变换发布方:
发布关于 laser 坐标系的位置信息
实现流程:
1.包含头文件
2.初始化 ROS 节点
3.创建静态坐标转换广播器
4.创建坐标系信息
5.广播器发布坐标系信息
6.spin()
*/
// 1.包含头文件
#include "ros/ros.h"
#include "tf2_ros/static_transform_broadcaster.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"
int main(int argc, char *argv[])
{
setlocale(LC_ALL,"");
// 2.初始化 ROS 节点
ros::init(argc,argv,"static_brocast");
// 3.创建静态坐标转换广播器
tf2_ros::StaticTransformBroadcaster broadcaster;
// 4.创建坐标系信息
geometry_msgs::TransformStamped ts;
//----设置头信息
ts.header.seq = 100;
ts.header.stamp = ros::Time::now();
ts.header.frame_id = "base_link";
//----设置子级坐标系
ts.child_frame_id = "laser";
//----设置子级相对于父级的偏移量
ts.transform.translation.x = 0.2;
ts.transform.translation.y = 0.0;
ts.transform.translation.z = 0.5;
//----设置四元数:将 欧拉角数据转换成四元数
tf2::Quaternion qtn;
qtn.setRPY(0,0,0);
ts.transform.rotation.x = qtn.getX();
ts.transform.rotation.y = qtn.getY();
ts.transform.rotation.z = qtn.getZ();
ts.transform.rotation.w = qtn.getW();
// 5.广播器发布坐标系信息
broadcaster.sendTransform(ts);
ros::spin();
return 0;
}
2.4订阅者实现:
/*
订阅坐标系信息,生成一个相对于 子级坐标系的坐标点数据,转换成父级坐标系中的坐标点
实现流程:
1.包含头文件
2.初始化 ROS 节点
3.创建 TF 订阅节点
4.生成一个坐标点(相对于子级坐标系)
5.转换坐标点(相对于父级坐标系)
6.spin()
*/
//1.包含头文件
#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "geometry_msgs/PointStamped.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h" //注意: 调用 transform 必须包含该头文件
int main(int argc, char *argv[])
{
setlocale(LC_ALL,"");
// 2.初始化 ROS 节点
ros::init(argc,argv,"tf_sub");
ros::NodeHandle nh;
// 3.创建 TF 订阅节点
tf2_ros::Buffer buffer;
tf2_ros::TransformListener listener(buffer);
ros::Rate r(1);
while (ros::ok())
{
// 4.生成一个坐标点(相对于子级坐标系)
geometry_msgs::PointStamped point_laser;
point_laser.header.frame_id = "laser";
point_laser.header.stamp = ros::Time::now();
point_laser.point.x = 1;
point_laser.point.y = 2;
point_laser.point.z = 7.3;
// 5.转换坐标点(相对于父级坐标系)
//新建一个坐标点,用于接收转换结果
//--------------使用 try 语句或休眠,否则可能由于缓存接收延迟而导致坐标转换失败------------------------
try
{
geometry_msgs::PointStamped point_base;
point_base = buffer.transform(point_laser,"base_link");
ROS_INFO("转换后的数据:(%.2f,%.2f,%.2f),参考的坐标系是:%s",point_base.point.x,point_base.point.y,point_base.point.z,point_base.header.frame_id.c_str());
}
catch(const std::exception& e)
{
// std::cerr << e.what() << '\n';
ROS_INFO("程序异常.....");
}
r.sleep();
ros::spinOnce();
}
return 0;
}
2.5rviz可视化显示:
红色x轴,绿色y轴,蓝色z轴。