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
图示：
