ROS--静态坐标变换（待续）
需求：
现有一机器人模型，核心构成包含主体与雷达，各对应一坐标系，坐标系的原点分别位于主体与雷达的物理中心，已知雷达原点相对于主体原点位移关系如下: x 0.2 y0.0 z0.5。当前雷达检测到一障碍物，在雷达坐标系中障碍物的坐标为 (2.0 3.0 5.0),请问，该障碍物相对于主体的坐标是多少？
方案A:C++实现
1.1 发布方实现
#include "ros/ros.h"
#include "tf2_ros/static_transform_broadcaster.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"
/*
需求：发布2个坐标系的相对关系
流程：
1.包含头文件
2.设置编码、节点初始化、NodeHandle
3.创建发布对象
4.组织被发布的消息
5.发布数据
6.spin()
*/
int main(int argc, char *argv[])
{
// 2.设置编码、节点初始化、NodeHandle
setlocale(LC_ALL,"");
ros::init(argc,argv,"static_pub");
ros::NodeHandle nh;
// 3.创建发布对象
tf2_ros::StaticTransformBroadcaster pub;
// 4.组织被发布的消息
geometry_msgs::TransformStamped tfs;
tfs.header.stamp = ros::Time::now();
tfs.header.frame_id = "base_link"; //相对坐标系关系中被参考的那一个(小车)
tfs.child_frame_id = "laser"; //雷达的
tfs.transform.translation.x = 0.2; //看需求
tfs.transform.translation.y = 0.0;
tfs.transform.translation.z = 0.5;
//需要根据欧拉角转换
tf2::Quaternion qtn; //创建四元数 对象
//向该对象设置欧拉角,这个对象可以将欧拉角转换成四元数
qtn.setRPY(0,0,0); //欧拉角的单位是弧度
tfs.transform.rotation.x = qtn.getX();
tfs.transform.rotation.y = qtn.getY();
tfs.transform.rotation.z = qtn.getZ();
tfs.transform.rotation.w = qtn.getW();
// 5.发布数据
pub.sendTransform(tfs);
// 6.spin()
ros::spin();
return 0;
}
输入：
rviz //调出 RViZ 界面
设置：
①Fixed Frame
②添加 TF 组件
1.2 订阅方实现
注：在运行时如果报错：
解决：
在转换算法前，添加休眠（需要订阅到消息后再转化）
ros::Duration(2).sleep();
代码：
#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"
/*
订阅方：订阅发布的坐标系相对关系，传入一个坐标点，调用 tf 实现转换
流程：
1.包含头文件
2.设置编码、节点初始化、NodeHandle(必须的)
3.创建一个订阅对象 --->订阅坐标系相对关系
4.组织一个坐标点数据
5.转换算法，调用 tf 内置实现
6.最后输出
*/
int main(int argc, char *argv[])
{
// 2.设置编码、节点初始化、NodeHandle(必须的)
setlocale(LC_ALL,"");
ros::init(argc,argv,"static_sub");
ros::NodeHandle nh;
// 3.创建一个订阅对象 --->订阅坐标系相对关系 (3-1 与 3-2 是结合使用的)
// 3-1. 创建一个 buffer 缓存
tf2_ros::Buffer buffer;
// 3-2. 再创建一个监听对象(监听对象可以将订阅的数据存入 buffer )
tf2_ros::TransformListener listen(buffer);
// 4.组织一个坐标点数据
geometry_msgs::PointStamped ps;
ps.header.frame_id = "laser";
ps.header.stamp = ros::Time::now();
ps.point.x = 2.0;
ps.point.y = 3.0;
ps.point.z = 5.0;
//添加休眠 （文章“注”的部分添加休眠）
ros::Duration(2).sleep();
// 5.转换算法，调用 tf 内置实现
ros::Rate rate(10);
while (ros::ok())
{
// 核心代码 ---- 将 ps 转换为 base_link 的坐标点
geometry_msgs::PointStamped ps_out;
/*
调用了 buffer 的转换函数 transform
参数1：被转换的坐标点数据；
参数2：目标坐标系；
返回值：转换后的坐标点信息（输出的坐标系）
注意：
1.调用时必须包含头文件 tf2_geometry_msgs/tf2_geometry_msgs.h
2.运行时存在的问题：抛出一个异常 base_link 不存在
原因：订阅数据是一个耗时操作，可能在调用 transform 转换函数时，坐标系的相对关系还没有被订阅到，故出现异常
解决：
方案1：在调用转换函数前执行休眠（简单）
方案2：进行异常处理
*/
ps_out = buffer.transform(ps,"base_link");
// 6.最后输出
ROS_INFO("转换后的坐标值:(%.2f,%.2f,%.2f),参考的坐标系:%s",
ps_out.point.x,
ps_out.point.y,
ps_out.point.z,
ps_out.header.frame_id.c_str());
rate.sleep();
ros::spinOnce();
}
return 0;
}
方案1：在调用转换函数前执行休眠
先运行发布方，再运行订阅方：
方案二：try处理
修改最后 ps_out 和 ROS_INFO 部分
try
{
ps_out = buffer.transform(ps,"base_link");
// 6.最后输出
ROS_INFO("转换后的坐标值:(%.2f,%.2f,%.2f),参考的坐标系:%s",
ps_out.point.x,
ps_out.point.y,
ps_out.point.z,
ps_out.header.frame_id.c_str());
}
catch(const std::exception& e)
{
// std::cerr << e.what() << '\n';
ROS_INFO("异常消息:%s",e.what());
}
方案B:Python实现
2.1 发布方实现
#! /usr/bin/env python
import rospy
import tf2_ros
import tf
from geometry_msgs.msg import TransformStamped
"""
发布方:发布2个坐标系的相对关系(小车的底盘 ---> base_link 和 雷达 --> laser)
流程:
1.导包
2.初始化节点
3.创建发布对象
4.组织被发布的数据
5.发布数据
6.spin()
"""
if __name__ == "__main__":
# 2.初始化节点
rospy.init_node("static_pub_p")
# 3.创建发布对象
pub = tf2_ros.StaticTransformBroadcaster()
# 4.组织被发布的数据
ts = TransformStamped()
# header
ts.header.stamp = rospy.Time.now()
ts.header.frame_id = "base_link"
# child frame
ts.child_frame_id = "laser"
# 相对关系（偏移 与 四元数）
ts.transform.translation.x = 0.2
ts.transform.translation.y = 0.0
ts.transform.translation.z = 0.5
# 4-1 先从欧拉角转换成四元数
qtn = tf.transformations.quaternion_from_euler(0,0,0)
# 4-2 设置四元数
ts.transform.rotation.x = qtn[0]
ts.transform.rotation.y = qtn[1]
ts.transform.rotation.z = qtn[2]
ts.transform.rotation.w = qtn[3]
# 5.发布数据
pub.sendTransform(ts)
# 6.spin()
rospy.spin()
输出：
2.2 订阅方实现
#! /usr/bin/env python
import rospy
import tf2_ros
from tf2_geometry_msgs import tf2_geometry_msgs
"""
订阅方:订阅坐标变换消息,传入被转换的坐标点,调用转换算法
流程:
1.导包
2.初始化节点
3.创建订阅对象
4.组织被转换的坐标点
5.转换逻辑实现,调用tf封装的算法
6.输出结果
"""
if __name__ == "__main__":
# 2.初始化节点
rospy.init_node("static_sub_p")
# 3.创建订阅对象
# 3-1.创建缓存对象
buffer = tf2_ros.Buffer()
# 3-2.创建订阅对象的坐标点
sub = tf2_ros.TransformListener(buffer)
# 4.组织被转换的坐标点
ps = tf2_geometry_msgs.PointStamped()
ps.header.stamp =rospy.Time.now()
ps.header.frame_id = "laser"
ps.point.x = 2.0
ps.point.y = 3.0
ps.point.z = 5.0
# 5.转换逻辑实现,调用tf封装的算法
rate = rospy.Rate(10)
while not rospy.is_shutdown():
try:
# 转换实现
""""
参数1:被转换的坐标点
参数2:目标坐标系
返回值:转换后的坐标点
注:
问题:抛出异常 base_link 不存在
原因:转换函数调用时,可能还没有订阅到坐标系的相对信息
解决: try 捕获异常,并处理
"""
ps_out = buffer.transform(ps,"base_link")
# 6.输出结果
rospy.loginfo("转换后的坐标:(%.2f,%.2f,%.2f),参考的坐标系:%s",
ps_out.point.x,
ps_out.point.y,
ps_out.point.z,
ps_out.header.frame_id
)
except Exception as e:
rospy.logwarn("错误提示:%s",e)
rate.sleep()
备注：Pykdl模块出错（版本不匹配，未能跑出结果）
方案C：命令行
当坐标系之间的相对位置固定时，那么所需参数也是固定的: 父系坐标名称、子级坐标系名称、x偏移量、y偏移量、z偏移量、x 翻滚角度、y俯仰角度、z偏航角度，实现逻辑相同，参数不同，那么 ROS 系统就已经封装好了专门的节点，使用方式如下:
rosrun tf2_ros static_transform_publisher x偏移量 y偏移量 z偏移量 z偏航角度 y俯仰角度 x翻滚角度 父级坐标系 子级坐标系
例：
示例:rosrun tf2_ros static_transform_publisher 0.2 0 0.5 0 0 0 /baselink /laser
即：
