ROS入门21讲(6)

十、ROS中的坐标系管理系统

1、机器人中的坐标变换

某位姿在A、B两个坐标系下的坐标变换

参考:《机器人学导论》

机器人系统中繁杂的坐标系

2、TF功能包

TF功能包能干什么？

①五秒钟(10s之内)之前，机器人头部坐标系相对于全局坐标系的关系是什么样的？

②机器人夹取的物体相对于机器人中心坐标系的位置在哪里？

③机器人中心坐标系相对于全局坐标系的位置在哪里？

TF坐标变换如何实现？

①广播TF变换

②监听TF变换

移动机器人的本体坐标系与雷达坐标系

坐标系之间的数据变换

命令：

$ sudo apt-get install ros-melodic-turtle-tf

$ roslaunch turtle_tf turtle_tf_demo.launch

图示：

$ rosrun turtlesim turtle_teleop_key

$ rosrun tf view_frames

命令行工具：$ rosrun tf tf_echo turtle1 turtle2

可视化工具：

命令：

　　$ rosrun rviz rviz -d 'rospack find turtle_tf' /rviz/turtle_rviz.rviz

十一、TF坐标系广播与监听的编程实现

1、创建功能包

命令：

$ cd ~/catkin_ws/src

$ catkin_create_pkg learning_tf roscpp rosoy tf turtlesim

2、创建tf广播器代码

如何实现一个tf广播器？

①定义TF广播器(TransformBroadcaster)

②创建坐标变换值

③发布坐标变换(sendTransform)

C++代码：

 1 /**
 2  * 该例程产生tf数据，并计算、发布turtle2的速度指令
 3  */
 4
 5 #include <ros/ros.h>
 6 #include <tf/transform_broadcaster.h>
 7 #include <turtlesim/Pose.h>
 8
 9 std::string turtle_name;
10
11 void poseCallback(const turtlesim::PoseConstPtr& msg)
12 {
13     // 创建tf的广播器
14     static tf::TransformBroadcaster br;
15
16     // 初始化tf数据
17     tf::Transform transform;
18     transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );
19     tf::Quaternion q;
20     q.setRPY(0, 0, msg->theta);
21     transform.setRotation(q);
22
23     // 广播world与海龟坐标系之间的tf数据
24     br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
25 }
26
27 int main(int argc, char** argv)
28 {
29     // 初始化ROS节点
30     ros::init(argc, argv, "my_tf_broadcaster");
31
32     // 输入参数作为海龟的名字
33     if (argc != 2)
34     {
35         ROS_ERROR("need turtle name as argument");
36         return -1;
37     }
38
39     turtle_name = argv[1];
40
41     // 订阅海龟的位姿话题
42     ros::NodeHandle node;
43     ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);
44
45     // 循环等待回调函数
46     ros::spin();
47
48     return 0;
49 };

python代码：

 1 ########################################################################
 2 ####          Copyright 2020 GuYueHome (www.guyuehome.com).          ###
 3 ########################################################################
 4
 5 # 该例程将请求/show_person服务，服务数据类型learning_service::Person
 6
 7 import roslib
 8 roslib.load_manifest('learning_tf')
 9 import rospy
10
11 import tf
12 import turtlesim.msg
13
14 def handle_turtle_pose(msg, turtlename):
15     br = tf.TransformBroadcaster()
16     br.sendTransform((msg.x, msg.y, 0),
17                      tf.transformations.quaternion_from_euler(0, 0, msg.theta),
18                      rospy.Time.now(),
19                      turtlename,
20                      "world")
21
22 if __name__ == '__main__':
23     rospy.init_node('turtle_tf_broadcaster')
24     turtlename = rospy.get_param('~turtle')
25     rospy.Subscriber('/%s/pose' % turtlename,
26                      turtlesim.msg.Pose,
27                      handle_turtle_pose,
28                      turtlename)
29     rospy.spin()

3、创建tf监听器代码

如何实现一个TF监听器？

①定义TF监听器(TransformListener)

②查找坐标变换(waitForTransform、lookupTransform)

C++代码：

 1 /**
 2  * 该例程监听tf数据，并计算、发布turtle2的速度指令
 3  */
 4
 5 #include <ros/ros.h>
 6 #include <tf/transform_listener.h>
 7 #include <geometry_msgs/Twist.h>
 8 #include <turtlesim/Spawn.h>
 9
10 int main(int argc, char** argv)
11 {
12     // 初始化ROS节点
13     ros::init(argc, argv, "my_tf_listener");
14
15     // 创建节点句柄
16     ros::NodeHandle node;
17
18     // 请求产生turtle2
19     ros::service::waitForService("/spawn");
20     ros::ServiceClient add_turtle = node.serviceClient<turtlesim::Spawn>("/spawn");
21     turtlesim::Spawn srv;
22     add_turtle.call(srv);
23
24     // 创建发布turtle2速度控制指令的发布者
25     ros::Publisher turtle_vel = node.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel", 10);
26
27     // 创建tf的监听器
28     tf::TransformListener listener;
29
30     ros::Rate rate(10.0);
31     while (node.ok())
32     {
33         // 获取turtle1与turtle2坐标系之间的tf数据
34         tf::StampedTransform transform;
35         try
36         {
37             listener.waitForTransform("/turtle2", "/turtle1", ros::Time(0), ros::Duration(3.0));
38             listener.lookupTransform("/turtle2", "/turtle1", ros::Time(0), transform);
39         }
40         catch (tf::TransformException &ex)
41         {
42             ROS_ERROR("%s",ex.what());
43             ros::Duration(1.0).sleep();
44             continue;
45         }
46
47         // 根据turtle1与turtle2坐标系之间的位置关系，发布turtle2的速度控制指令
48         geometry_msgs::Twist vel_msg;
49         vel_msg.angular.z = 4.0 * atan2(transform.getOrigin().y(),
50                                         transform.getOrigin().x());
51         vel_msg.linear.x = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) +
52                                       pow(transform.getOrigin().y(), 2));
53         turtle_vel.publish(vel_msg);
54
55         rate.sleep();
56     }
57     return 0;
58 };

python代码：

 1 ########################################################################
 2 ####          Copyright 2020 GuYueHome (www.guyuehome.com).          ###
 3 ########################################################################
 4
 5 # 该例程将请求/show_person服务，服务数据类型learning_service::Person
 6
 7 import roslib
 8 roslib.load_manifest('learning_tf')
 9 import rospy
10 import math
11 import tf
12 import geometry_msgs.msg
13 import turtlesim.srv
14
15 if __name__ == '__main__':
16     rospy.init_node('turtle_tf_listener')
17
18     listener = tf.TransformListener()
19
20     rospy.wait_for_service('spawn')
21     spawner = rospy.ServiceProxy('spawn', turtlesim.srv.Spawn)
22     spawner(4, 2, 0, 'turtle2')
23
24     turtle_vel = rospy.Publisher('turtle2/cmd_vel', geometry_msgs.msg.Twist,queue_size=1)
25
26     rate = rospy.Rate(10.0)
27     while not rospy.is_shutdown():
28         try:
29             (trans,rot) = listener.lookupTransform('/turtle2', '/turtle1', rospy.Time(0))
30         except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
31             continue
32
33         angular = 4 * math.atan2(trans[1], trans[0])
34         linear = 0.5 * math.sqrt(trans[0] ** 2 + trans[1] ** 2)
35         cmd = geometry_msgs.msg.Twist()
36         cmd.linear.x = linear
37         cmd.angular.z = angular
38         turtle_vel.publish(cmd)
39
40         rate.sleep()

4、配置tf广播器与监听器代码编译规则

如何配置CMakeLists.txt中的编译规则？

①设置需要配置的代码和生成的可执行文件

②设置链接库

CMakeLists.txt中需要添加：

add_executable(turtle_tf_broadcaster src/turtle_tf_broadcaster.cpp)
target_link_libraries(turtle_tf_broadcaster ${catkin_LIBRARIES})

add_executable(turtle_tf_listener src/turtle_tf_listener.cpp)
target_link_libraries(turtle_tf_listener ${catkin_LIBRARIES})

5、编译并运行

命令：

$ cd ~/catkin_ws

$ catkin_make

$ source devel/setup.bash

$ roscore

$ rosrun turtlesim turtlesim_node

$ rosrun learning_tf turtle_tf_broadcaster __name:=turtle1_tf_broadcaster /turtle1

$ rosrun learning_tf turtle_tf_broadcaster __name:=turtle2_tf_broadcaster /turtle2

$ rosrun learning_tf turtle_tf_listener

$ rosrun turtlesim turtle_teleop_key

图示：