ROS 学习记录（六）——TF 坐标变换实操
文章目录
最终要实现的效果：
思路如下：
- 创建 turtle2
- 控制 turtle1 随机运动
- 分别订阅 turtle1 和 turtle2 的位置，转化为 tf 坐标并广播。订阅两个 turtle 的位置的节点唯一的区别是命名空间不同，因此可以通过在两个命名空间中分别运行同一节点实现
- 计算 turtle2 和 turtle1 的相对位置，并控制 turtle2 移动
C++ 代码实现
创建 turtle2：
1#include "ros/ros.h"
2#include "turtlesim/Spawn.h"
3
4int main(int argc, char **argv) {
5 setlocale(LC_ALL, "");
6 ros::init(argc, argv, "spawn_turtle");
7 ros::NodeHandle nh;
8
9 ros::ServiceClient client = nh.serviceClient<turtlesim::Spawn>("/spawn"); // 创建客户端
10 ros::service::waitForService("/spawn"); // 等待服务启动
11
12 turtlesim::Spawn spawn;
13 spawn.request.name = "turtle2";
14 spawn.request.x = 1.0;
15 spawn.request.y = 1.0;
16 spawn.request.theta = 1.571;
17
18 if (client.call(spawn)) {
19 ROS_INFO("Turtle spawned successfully!");
20 } else {
21 ROS_ERROR("Failed to spawn turtle.");
22 }
23
24 return 0;
25}
控制 turtle1 随机运动：
1#include "geometry_msgs/Twist.h"
2#include "ros/ros.h"
3
4#include <random>
5
6double generate_random() {
7 std::random_device rand;
8 std::mt19937 gen(rand());
9 std::uniform_real_distribution<> dis(-3.142, 3.142);
10 return dis(gen);
11}
12
13int main(int argc, char **argv) {
14 setlocale(LC_ALL, "");
15 ros::init(argc, argv, "random_track");
16 ros::NodeHandle n;
17
18 ros::Publisher publisher = n.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 100);
19 geometry_msgs::Twist twist;
20 twist.linear.x = 3.3;
21
22 ros::Rate rate(2);
23 while (ros::ok()) {
24 twist.angular.z = generate_random();
25 publisher.publish(twist);
26 ros::spinOnce();
27 rate.sleep();
28 }
29 return 0;
30}
订阅 turtle1 和 turtle2 的位置，转化为 tf 坐标并广播：
1#include "geometry_msgs/TransformStamped.h"
2#include "ros/ros.h"
3#include "tf2/LinearMath/Quaternion.h"
4#include "tf2_ros/transform_broadcaster.h"
5#include "turtlesim/Pose.h"
6
7std::string ns;
8
9void callback(const turtlesim::Pose::ConstPtr &pose) {
10 // 创建 TF 广播对象
11 static tf2_ros::TransformBroadcaster broadcaster;
12
13 // 将 Pose 转化为 TransformStamped
14 geometry_msgs::TransformStamped ts;
15 ts.header.frame_id = "world";
16 ts.header.stamp = ros::Time::now();
17 ts.child_frame_id = ns;
18
19 ts.transform.translation.x = pose->x;
20 ts.transform.translation.y = pose->y;
21 ts.transform.translation.z = 0;
22
23 tf2::Quaternion qtn;
24 qtn.setRPY(0, 0, pose->theta);
25 ts.transform.rotation.x = qtn.getX();
26 ts.transform.rotation.y = qtn.getY();
27 ts.transform.rotation.z = qtn.getZ();
28 ts.transform.rotation.w = qtn.getW();
29
30 broadcaster.sendTransform(ts);
31}
32
33int main(int argc, char **argv) {
34 setlocale(LC_ALL, "");
35 ros::init(argc, argv, "broadcaster");
36 ros::NodeHandle nh;
37
38 ns = nh.getNamespace();
39
40 ros::Subscriber subscriber = nh.subscribe<turtlesim::Pose>(ns + "/pose", 100, callback);
41 ros::spin();
42 return 0;
43}
计算 turtle2 和 turtle1 的相对位置，并控制 turtle2 移动：
1#include "geometry_msgs/TransformStamped.h"
2#include "geometry_msgs/Twist.h"
3#include "ros/ros.h"
4#include "tf2_ros/transform_listener.h"
5
6int main(int argc, char **argv) {
7 setlocale(LC_ALL, "");
8 ros::init(argc, argv, "turtle2_controller");
9 ros::NodeHandle nh;
10
11 // 创建 TF 订阅对象
12 tf2_ros::Buffer buffer;
13 tf2_ros::TransformListener listener(buffer);
14
15 // 创建发布到 /turtle2/cmd_vel 的发布对象
16 ros::Publisher publisher = nh.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel", 100);
17
18 ros::Rate rate(10);
19 while (ros::ok()) {
20 try {
21 geometry_msgs::TransformStamped ts = buffer.lookupTransform("turtle2", "turtle1", ros::Time(0));
22
23 // 根据坐标信息生成速度信息
24 geometry_msgs::Twist twist;
25 twist.linear.x = 0.5 * sqrt(pow(ts.transform.translation.x, 2) + pow(ts.transform.translation.y, 2));
26 twist.angular.z = 4 * atan2(ts.transform.translation.y, ts.transform.translation.x);
27
28 publisher.publish(twist);
29 } catch (const tf2::TransformException &ex) {
30 ROS_WARN("Failed to process.");
31 }
32
33 rate.sleep();
34 ros::spinOnce();
35 }
36 return 0;
37}
launch 文件：
1<launch>
2 <node pkg="tf_turtle" type="broadcaster" name="broadcaster1" ns="turtle1" />
3 <node pkg="tf_turtle" type="broadcaster" name="broadcaster2" ns="turtle2" />
4 <node pkg="tf_turtle" type="controller" name="controller" output="screen" />
5 <node pkg="tf_turtle" type="rand_turtle1" name="rand_turtle1" />
6 <node pkg="tf_turtle" type="turtle_spawn" name="turtle_spawn" output="screen" />
7
8 <node pkg="turtlesim" type="turtlesim_node" name="turtlesim_node" required="true" />
9 <node pkg="rviz" type="rviz" name="rviz" args="-d $(find tf_turtle)/config/rviz/turtle.rviz" />
10</launch>
Python 代码实现
创建 turtle2：
1import rospy
2from turtlesim.srv import Spawn, SpawnRequest, SpawnResponse
3
4
5def main():
6 rospy.init_node("spawn_turtle")
7
8 client = rospy.ServiceProxy("/spawn", Spawn)
9 client.wait_for_service()
10
11 request = SpawnRequest()
12 request.name = "turtle2"
13 request.x = 1.0
14 request.y = 1.0
15 request.theta = 1.571
16
17 if client.call(request):
18 rospy.loginfo("Turtle spawned successfully!")
19 else:
20 rospy.logerr("Failed to spawn turtle.")
21
22
23if __name__ == "__main__":
24 main()
控制 turtle1 随机运动：
1from random import uniform
2
3import rospy
4from geometry_msgs.msg import Twist
5
6
7def main():
8 rospy.init_node("random_track")
9 publisher = rospy.Publisher("turtle1/cmd_vel", Twist, queue_size=100)
10
11 twist = Twist()
12 twist.linear.x = 3
13
14 rate = rospy.Rate(2)
15 while not rospy.is_shutdown():
16 twist.angular.z = uniform(-3.142, 3.142)
17 publisher.publish(twist)
18 rate.sleep()
19
20
21if __name__ == "__main__":
22 main()
订阅 turtle1 和 turtle2 的位置，转化为 tf 坐标并广播：
1import rospy
2import tf2_ros
3import tf_conversions
4from geometry_msgs.msg import TransformStamped
5from turtlesim.msg import Pose
6
7
8def callback(pose):
9 global ns
10
11 broadcaster = tf2_ros.TransformBroadcaster()
12
13 ts = TransformStamped()
14 ts.header.frame_id = "world"
15 ts.header.stamp = rospy.Time.now()
16
17 """
18 通过 launch 文件将 ns 参数传到 Python 节点中，默认会在前后都添加斜线
19 例如 turtle 被转换为 /turtle/
20 而在 C++ 节点中则会是 /turtle
21 因此要去掉最后一个斜线，否则无法识别这个 TF 对象
22 同理 main 函数中的 rospy.Subscriber(ns + "pose", Pose, callback) 用的是 "pose" 而非 "/pose"
23 """
24 ts.child_frame_id = ns[:-1]
25
26 ts.transform.translation.x = pose.x
27 ts.transform.translation.y = pose.y
28 ts.transform.translation.z = 0
29
30 qtn = tf_conversions.transformations.quaternion_from_euler(0, 0, pose.theta)
31 ts.transform.rotation.x = qtn[0]
32 ts.transform.rotation.y = qtn[1]
33 ts.transform.rotation.z = qtn[2]
34 ts.transform.rotation.w = qtn[3]
35
36 broadcaster.sendTransform(ts)
37
38
39def main():
40 global ns
41
42 rospy.init_node("broadcaster")
43
44 ns = rospy.get_namespace()
45 rospy.Subscriber(ns + "pose", Pose, callback)
46 rospy.spin()
47
48
49if __name__ == "__main__":
50 main()
计算 turtle2 和 turtle1 的相对位置，并控制 turtle2 移动：
1from math import atan2, pow, sqrt
2
3import rospy
4import tf2_ros
5from geometry_msgs.msg import Twist
6
7
8def main():
9 rospy.init_node("turtle2_controller")
10
11 buffer = tf2_ros.Buffer()
12 listener = tf2_ros.TransformListener(buffer)
13
14 publisher = rospy.Publisher("turtle2/cmd_vel", Twist, queue_size=100)
15
16 rate = rospy.Rate(10)
17 while not rospy.is_shutdown():
18 try:
19 ts = buffer.lookup_transform("turtle2", "turtle1", rospy.Time(0))
20
21 twist = Twist()
22 twist.linear.x = 0.5 * sqrt(
23 pow(ts.transform.translation.x, 2) + pow(ts.transform.translation.y, 2)
24 )
25 twist.angular.z = 4 * atan2(
26 ts.transform.translation.y, ts.transform.translation.x
27 )
28
29 publisher.publish(twist)
30 except tf2_ros.TransformException:
31 rospy.logwarn("Failed to process")
32
33 rate.sleep()
34
35
36if __name__ == "__main__":
37 main()
launch 文件：
1<launch>
2 <node pkg="tf_turtle" type="broadcaster.py" name="broadcaster1" ns="turtle1" />
3 <node pkg="tf_turtle" type="broadcaster.py" name="broadcaster2" ns="turtle2" />
4 <node pkg="tf_turtle" type="controller.py" name="controller" output="screen" />
5 <node pkg="tf_turtle" type="rand_turtle1.py" name="rand_turtle1" />
6 <node pkg="tf_turtle" type="turtle_spawn.py" name="turtle_spawn" output="screen" />
7
8 <node pkg="turtlesim" type="turtlesim_node" name="turtlesim_node" required="true" />
9 <node pkg="rviz" type="rviz" name="rviz" args="-d $(find tf_turtle)/config/rviz/turtle.rviz" />
10</launch>