9.24 动态坐标关系/多坐标关系/实操=还没自己写
动态坐标变换
所谓动态坐标变换，是指两个坐标系之间的相对位置是变化的。
动态坐标的一个问题，是每次都要取一个坐标值吗
需求描述:
启动 turtlesim_node,该节点中窗体有一个世界坐标系(左下角为坐标系原点)，乌龟是另一个坐标系，键盘控制乌龟运动，将两个坐标系的相对位置动态发布。
结果演示:
实现分析:
- 乌龟本身不但可以看作坐标系，也是世界坐标系中的一个坐标点
- 订阅 turtle1/pose,可以获取乌龟在世界坐标系的 x坐标、y坐标、偏移量以及线速度和角速度
- 将 pose 信息转换成 坐标系相对信息并发布
实现流程:C++ 与 Python 实现流程一致
- 新建功能包，添加依赖
- 创建坐标相对关系发布方(同时需要订阅乌龟位姿信息)
- 创建坐标相对关系订阅方
- 执行
方案A:C++实现
1.创建功能包
创建项目功能包依赖于 tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs、turtlesim
2.发布方
问题：发布方是通过什么话题来发布的？ 通过坐标吧
3.订阅方
一个问题在订阅方中，下面的时间搓是不允许的，会报异常。
原因：动态情况：订阅放中的buffer缓存存储发布方发送的坐标已经大量的时间，但数据传输是有延迟的，
订阅方在转换坐标中，会将转换的坐标系的时间搓进行比对，如果时间差太多，会报错。而使用下面的操作
意味这不设置值，则没事
ps.header.stamp = ros::Time(0.0);
4.执行
可以使用命令行或launch文件的方式分别启动发布节点与订阅节点，如果程序无异常，与演示结果类似。
可以使用 rviz 查看坐标系相对关系。
多坐标变换 看到不是很懂
需求描述:
现有坐标系统，父级坐标系统 world,下有两子级系统 son1，son2，son1 相对于 world，以及 son2 相对于 world 的关系是已知的，求 son1原点在 son2中的坐标，又已知在 son1中一点的坐标，要求求出该点在 son2 中的坐标
实现分析:
- 首先，需要发布 son1 相对于 world，以及 son2 相对于 world 的坐标消息
- 然后，需要订阅坐标发布消息，并取出订阅的消息，借助于 tf2 实现 son1 和 son2 的转换
- 最后，还要实现坐标点的转换
实现流程:C++ 与 Python 实现流程一致
- 新建功能包，添加依赖
- 创建坐标相对关系发布方(需要发布两个坐标相对关系)
- 创建坐标相对关系订阅方
- 执行
方案A:C++实现
1.创建功能包
创建项目功能包依赖于 tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs、turtlesim
tf2 tf2_ros tf2_geometry_msgs roscpp rospy std_msgs geometry_msgs
2.发布方
为了方便，使用静态坐标变换发布
<launch> <node pkg="tf2_ros" type="static_transform_publisher" name="son1" args="0.2 0.8 0.3 0 0 0 /world /son1" output="screen" /> <node pkg="tf2_ros" type="static_transform_publisher" name="son2" args="0.5 0 0 0 0 0 /world /son2" output="screen" /> </launch> Copy3.订阅方
/* 需求: 现有坐标系统，父级坐标系统 world,下有两子级系统 son1，son2， son1 相对于 world，以及 son2 相对于 world 的关系是已知的， 求 son1 与 son2中的坐标关系，又已知在 son1中一点的坐标，要求求出该点在 son2 中的坐标 实现流程: 1.包含头文件 2.初始化 ros 节点 3.创建 ros 句柄 4.创建 TF 订阅对象 5.解析订阅信息中获取 son1 坐标系原点在 son2 中的坐标 解析 son1 中的点相对于 son2 的坐标 6.spin */ //1.包含头文件 #include "ros/ros.h" #include "tf2_ros/transform_listener.h" #include "tf2/LinearMath/Quaternion.h" #include "tf2_geometry_msgs/tf2_geometry_msgs.h" #include "geometry_msgs/TransformStamped.h" #include "geometry_msgs/PointStamped.h" int main(int argc, char *argv[]) { setlocale(LC_ALL,""); // 2.初始化 ros 节点 ros::init(argc,argv,"sub_frames"); // 3.创建 ros 句柄 ros::NodeHandle nh; // 4.创建 TF 订阅对象 tf2_ros::Buffer buffer; tf2_ros::TransformListener listener(buffer); // 5.解析订阅信息中获取 son1 坐标系原点在 son2 中的坐标 ros::Rate r(1); while (ros::ok()) { try { // 解析 son1 中的点相对于 son2 的坐标 geometry_msgs::TransformStamped tfs = buffer.lookupTransform("son2","son1",ros::Time(0)); ROS_INFO("Son1 相对于 Son2 的坐标关系:父坐标系ID=%s",tfs.header.frame_id.c_str()); ROS_INFO("Son1 相对于 Son2 的坐标关系:子坐标系ID=%s",tfs.child_frame_id.c_str()); ROS_INFO("Son1 相对于 Son2 的坐标关系:x=%.2f,y=%.2f,z=%.2f", tfs.transform.translation.x, tfs.transform.translation.y, tfs.transform.translation.z ); // 坐标点解析 geometry_msgs::PointStamped ps; ps.header.frame_id = "son1"; ps.header.stamp = ros::Time::now(); ps.point.x = 1.0; ps.point.y = 2.0; ps.point.z = 3.0; geometry_msgs::PointStamped psAtSon2; psAtSon2 = buffer.transform(ps,"son2"); ROS_INFO("在 Son2 中的坐标:x=%.2f,y=%.2f,z=%.2f", psAtSon2.point.x, psAtSon2.point.y, psAtSon2.point.z ); } catch(const std::exception& e) { // std::cerr << e.what() << '\n'; ROS_INFO("异常信息:%s",e.what()); } r.sleep(); // 6.spin ros::spinOnce(); } return 0; } Copy配置文件此处略。
4.执行
可以使用命令行或launch文件的方式分别启动发布节点与订阅节点，如果程序无异常，将输出换算后的结果。
方案B:Python实现
1.创建功能包
创建项目功能包依赖于 tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs、turtlesim
2.发布方
为了方便，使用静态坐标变换发布
<launch> <node pkg="tf2_ros" type="static_transform_publisher" name="son1" args="0.2 0.8 0.3 0 0 0 /world /son1" output="screen" /> <node pkg="tf2_ros" type="static_transform_publisher" name="son2" args="0.5 0 0 0 0 0 /world /son2" output="screen" /> </launch> Copy3.订阅方
#!/usr/bin/env python """ 需求: 现有坐标系统，父级坐标系统 world,下有两子级系统 son1，son2， son1 相对于 world，以及 son2 相对于 world 的关系是已知的， 求 son1 与 son2中的坐标关系，又已知在 son1中一点的坐标，要求求出该点在 son2 中的坐标 实现流程: 1.导包 2.初始化 ROS 节点 3.创建 TF 订阅对象 4.调用 API 求出 son1 相对于 son2 的坐标关系 5.创建一依赖于 son1 的坐标点，调用 API 求出该点在 son2 中的坐标 6.spin """ # 1.导包 import rospy import tf2_ros from geometry_msgs.msg import TransformStamped from tf2_geometry_msgs import PointStamped if __name__ == "__main__": # 2.初始化 ROS 节点 rospy.init_node("frames_sub_p") # 3.创建 TF 订阅对象 buffer = tf2_ros.Buffer() listener = tf2_ros.TransformListener(buffer) rate = rospy.Rate(1) while not rospy.is_shutdown(): try: # 4.调用 API 求出 son1 相对于 son2 的坐标关系 #lookup_transform(self, target_frame, source_frame, time, timeout=rospy.Duration(0.0)): tfs = buffer.lookup_transform("son2","son1",rospy.Time(0)) rospy.loginfo("son1 与 son2 相对关系:") rospy.loginfo("父级坐标系:%s",tfs.header.frame_id) rospy.loginfo("子级坐标系:%s",tfs.child_frame_id) rospy.loginfo("相对坐标:x=%.2f, y=%.2f, z=%.2f", tfs.transform.translation.x, tfs.transform.translation.y, tfs.transform.translation.z, ) # 5.创建一依赖于 son1 的坐标点，调用 API 求出该点在 son2 中的坐标 point_source = PointStamped() point_source.header.frame_id = "son1" point_source.header.stamp = rospy.Time.now() point_source.point.x = 1 point_source.point.y = 1 point_source.point.z = 1 point_target = buffer.transform(point_source,"son2",rospy.Duration(0.5)) rospy.loginfo("point_target 所属的坐标系:%s",point_target.header.frame_id) rospy.loginfo("坐标点相对于 son2 的坐标:(%.2f,%.2f,%.2f)", point_target.point.x, point_target.point.y, point_target.point.z ) except Exception as e: rospy.logerr("错误提示:%s",e) rate.sleep() # 6.spin # rospy.spin() Copy权限设置以及配置文件此处略。
4.执行
可以使用命令行或launch文件的方式分别启动发布节点与订阅节点，如果程序无异常，将输出换算后的结果。
坐标关系查看
在机器人系统中，涉及的坐标系有多个，为了方便查看，ros 提供了专门的工具，可以用于生成显示坐标系关系的 pdf 文件，该文件包含树形结构的坐标系图谱。
6.1准备
首先调用rospack find tf2_tools查看是否包含该功能包，如果没有，请使用如下命令安装:
sudo apt install ros-noetic-tf2-tools
Copy
6.2使用
6.2.1生成 pdf 文件
启动坐标系广播程序之后，运行如下命令:
rosrun tf2_tools view_frames.py
会产生类似于下面的日志信息:
[INFO] [1592920556.827549]: Listening to tf data during 5 seconds...
[INFO] [1592920561.841536]: Generating graph in frames.pdf file...
查看当前目录会生成一个 frames.pdf 文件
6.2.2查看文件
可以直接进入目录打开文件，或者调用命令查看文件:evince frames.pdf
内如如图所示:
另请参考:
TF坐标变换实操
笔记： 抽象的问题都映射有一个显示的问题，一个导弹如何击打目标
设计一张世界地图
车和导弹都有自己的坐标
根据各自的坐标实现坐标变换，计算出车相对导弹的位置
通过计算车在导弹坐标系的位置是（5，14.5）
需求描述:
程序启动之初: 产生两只乌龟，中间的乌龟(A) 和 左下乌龟(B), B 会自动运行至A的位置，并且键盘控制时，只是控制 A 的运动，但是 B 可以跟随 A 运行
结果演示:
实现分析:
乌龟跟随实现的核心，是乌龟A和B都要发布相对世界坐标系的坐标信息，然后，订阅到该信息需要转换获取A相对于B坐标系的信息，最后，再生成速度信息，并控制B运动。
- 启动乌龟显示节点
- 在乌龟显示窗体中生成一只新的乌龟(需要使用服务)
- 编写两只乌龟发布坐标信息的节点
- 编写订阅节点订阅坐标信息并生成新的相对关系生成速度信息
实现流程:C++ 与 Python 实现流程一致
- 新建功能包，添加依赖
- 编写服务客户端，用于生成一只新的乌龟
- 编写发布方，发布两只乌龟的坐标信息
- 编写订阅方，订阅两只乌龟信息，生成速度信息并发布
- 运行
准备工作:
1.了解如何创建第二只乌龟，且不受键盘控制
创建第二只乌龟需要使用rosservice,话题使用的是 spawn
rosservice call /spawn "x: 1.0
y: 1.0
theta: 1.0
name: 'turtle_flow'"
name: "turtle_flow"
键盘是无法控制第二只乌龟运动的，因为使用的话题: /第二只乌龟名称/cmd_vel,对应的要控制乌龟运动必须发布对应的话题消息
2.了解如何获取两只乌龟的坐标
是通过话题 /乌龟名称/pose 来获取的
x: 1.0 //x坐标
y: 1.0 //y坐标
theta: -1.21437060833 //角度
linear_velocity: 0.0 //线速度
angular_velocity: 1.0 //角速度
Copy
方案A:C++实现
1.创建功能包
tf2 tf2_ros tf2_geometry_msgs roscpp rospy std_msgs geometry_msgs turtlesim
创建项目功能包依赖于 tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs、turtlesim
2.服务客户端(生成乌龟)
/my_turtle/cmd_vel 话题名称一样命名空间不一样
/my_turtle/color_sensor
/my_turtle/pose
/rosout
/rosout_agg
/turtle1/cmd_vel
/turtle1/color_sensor
/turtle1/pose
/*
创建第二只小乌龟
*/
#include "ros/ros.h"
#include "turtlesim/Spawn.h"
int main(int argc, char *argv[])
{
setlocale(LC_ALL,"");
//执行初始化
ros::init(argc,argv,"create_turtle");
//创建节点
ros::NodeHandle nh;
//创建服务客户端
ros::ServiceClient client = nh.serviceClient<turtlesim::Spawn>("/spawn");
ros::service::waitForService("/spawn");
turtlesim::Spawn spawn;
spawn.request.name = "turtle2";
spawn.request.x = 1.0;
spawn.request.y = 2.0;
spawn.request.theta = 3.12415926;
bool flag = client.call(spawn);
if (flag)
{
ROS_INFO("乌龟%s创建成功!",spawn.response.name.c_str());
}
else
{
ROS_INFO("乌龟2创建失败!");
}
ros::spin();
return 0;
}
Copy
配置文件此处略。
3.发布方(发布两只乌龟的坐标信息)
可以订阅乌龟的位姿信息，然后再转换成坐标信息，两只乌龟的实现逻辑相同，只是订阅的话题名称，生成的坐标信息等稍有差异，可以将差异部分通过参数传入:
- 该节点需要启动两次
- 每次启动时都需要传入乌龟节点名称(第一次是 turtle1 第二次是 turtle2)
/*
该文件实现:需要订阅 turtle1 和 turtle2 的 pose，然后广播相对 world 的坐标系信息
注意: 订阅的两只 turtle,除了命名空间(turtle1 和 turtle2)不同外,
其他的话题名称和实现逻辑都是一样的，
所以我们可以将所需的命名空间通过 args 动态传入
实现流程:
1.包含头文件
2.初始化 ros 节点
3.解析传入的命名空间
4.创建 ros 句柄
5.创建订阅对象
6.回调函数处理订阅的 pose 信息
6-1.创建 TF 广播器
6-2.将 pose 信息转换成 TransFormStamped
6-3.发布
7.spin
*/
//1.包含头文件
#include "ros/ros.h"
#include "turtlesim/Pose.h"
#include "tf2_ros/transform_broadcaster.h"
#include "tf2/LinearMath/Quaternion.h"
#include "geometry_msgs/TransformStamped.h"
//保存乌龟名称
std::string turtle_name;
void doPose(const turtlesim::Pose::ConstPtr& pose){
// 6-1.创建 TF 广播器 ---------------------------------------- 注意 static
static tf2_ros::TransformBroadcaster broadcaster;
// 6-2.将 pose 信息转换成 TransFormStamped
geometry_msgs::TransformStamped tfs;
tfs.header.frame_id = "world";
tfs.header.stamp = ros::Time::now();
tfs.child_frame_id = turtle_name;
tfs.transform.translation.x = pose->x;
tfs.transform.translation.y = pose->y;
tfs.transform.translation.z = 0.0;
tf2::Quaternion qtn;
qtn.setRPY(0,0,pose->theta);
tfs.transform.rotation.x = qtn.getX();
tfs.transform.rotation.y = qtn.getY();
tfs.transform.rotation.z = qtn.getZ();
tfs.transform.rotation.w = qtn.getW();
// 6-3.发布
broadcaster.sendTransform(tfs);
}
int main(int argc, char *argv[])
{
setlocale(LC_ALL,"");
// 2.初始化 ros 节点
ros::init(argc,argv,"pub_tf");
// 3.解析传入的命名空间
if (argc != 2)
{
ROS_ERROR("请传入正确的参数");
} else {
turtle_name = argv[1];
ROS_INFO("乌龟 %s 坐标发送启动",turtle_name.c_str());
}
// 4.创建 ros 句柄
ros::NodeHandle nh;
// 5.创建订阅对象
ros::Subscriber sub = nh.subscribe<turtlesim::Pose>(turtle_name + "/pose",1000,doPose);
// 6.回调函数处理订阅的 pose 信息
// 6-1.创建 TF 广播器
// 6-2.将 pose 信息转换成 TransFormStamped
// 6-3.发布
// 7.spin
ros::spin();
return 0;
}
Copy
配置文件此处略。
//线速度和角速度
4.订阅方(解析坐标信息并生成速度信息)
/*
订阅 turtle1 和 turtle2 的 TF 广播信息，查找并转换时间最近的 TF 信息
将 turtle1 转换成相对 turtle2 的坐标，在计算线速度和角速度并发布
实现流程:
1.包含头文件
2.初始化 ros 节点
3.创建 ros 句柄
4.创建 TF 订阅对象
5.处理订阅到的 TF
6.spin
*/
//1.包含头文件
#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"
#include "geometry_msgs/TransformStamped.h"
#include "geometry_msgs/Twist.h"
int main(int argc, char *argv[])
{
setlocale(LC_ALL,"");
// 2.初始化 ros 节点
ros::init(argc,argv,"sub_TF");
// 3.创建 ros 句柄
ros::NodeHandle nh;
// 4.创建 TF 订阅对象
tf2_ros::Buffer buffer;
tf2_ros::TransformListener listener(buffer);
// 5.处理订阅到的 TF
// 需要创建发布 /turtle2/cmd_vel 的 publisher 对象
ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel",1000);
ros::Rate rate(10);
while (ros::ok())
{
try
{
//5-1.先获取 turtle1 相对 turtle2 的坐标信息
geometry_msgs::TransformStamped tfs = buffer.lookupTransform("turtle2","turtle1",ros::Time(0));
//5-2.根据坐标信息生成速度信息 -- geometry_msgs/Twist.h
geometry_msgs::Twist twist;
twist.linear.x = 0.5 * sqrt(pow(tfs.transform.translation.x,2) + pow(tfs.transform.translation.y,2));
twist.angular.z = 4 * atan2(tfs.transform.translation.y,tfs.transform.translation.x);
//5-3.发布速度信息 -- 需要提前创建 publish 对象
pub.publish(twist);
}
catch(const std::exception& e)
{
// std::cerr << e.what() << '\n';
ROS_INFO("错误提示:%s",e.what());
}
rate.sleep();
// 6.spin
ros::spinOnce();
}
return 0;
}
Copy
配置文件此处略。
5.运行
使用 launch 文件组织需要运行的节点，内容示例如下:
浙公网安备 33010602011771号