【FirePrime】通过arduino学习ROS系列3发布坐标变换信息

liuyu-419812

      在机器人中，包含很多的坐标系，这些坐标系之间的关系对机器人以后的运动学、轨迹规划等是很重要的。因此，我们将在Arduino上使用ros::Time和TF创建一个tf发布者。通过tf发布者向ROS发布坐标变换信息。当然，这里只是模拟发送信息的过程，并没有一个机器人模型。

      参考：http://wiki.ros.org/rosserial_arduino/Tutorials/Time%20and%20TF

      可以通过在Arduino examples菜单选择ros_lib->TimeTf，打开下面的代码：

1. /*  
   
2. * rosserial Time and TF Example
   
3. * Publishes a transform at current time
   
4. */
   
5. #include <ros.h>
   
6. #include <ros/time.h>
   
7. #include <tf/transform_broadcaster.h>
   
8. ros::NodeHandle  nh;
   
9. geometry_msgs::TransformStamped t;
   
10. tf::TransformBroadcaster broadcaster;
    
11. char base_link[] = "/base_link";
    
12. char odom[] = "/odom";
    
13. void setup()
    
14. {
    
15.   nh.initNode();
    
16.   broadcaster.init(nh);
    
17. }
    
18. void loop(){
    
19. t.header.frame_id = odom;
    
20. t.child_frame_id = base_link;         
    
21. t.transform.translation.x = 1.0;           
    
22. t.transform.rotation.x = 0.0;         
    
23. t.transform.rotation.y = 0.0;           
    
24. t.transform.rotation.z = 0.0;           
    
25. t.transform.rotation.w = 1.0;   
    
26. t.header.stamp = nh.now();         
    
27. broadcaster.sendTransform(t);  
    
28. nh.spinOnce();  
    
29. delay(10);
    
30. }

复制代码

      下面，我们来分析一下代码。

1. #include <ros.h>
   
2. #include <ros/time.h>
   
3. #include <tf/transform_broadcaster.h>

复制代码

      我们需要包括通常的ROS头文件，也包含一个关于广播坐标变换的头文件。

1. ros::NodeHandle  nh;
   
2. geometry_msgs::TransformStamped t;
   
3. tf::TransformBroadcaster broadcaster;
   
4. char base_link[] = "/base_link";
   
5. char odom[] = "/odom";

复制代码

      接着，我们实例化一个将要使用的TransformStamped消息和一个广播者。我们也需要指定发布的变换的坐标系的名字。

1. broadcaster.init(nh);

复制代码

      在setup()函数内部，我们以节点句柄作为参数调用TransformBroadcaster的init()函数。没有这个调用，broadcaster不会正确发布。

1. t.header.frame_id = odom;
   
2. t.child_frame_id = base_link;  
   
3. t.transform.translation.x = 1.0;   
   
4. t.transform.rotation.x = 0.0;  
   
5. t.transform.rotation.y = 0.0;   
   
6. t.transform.rotation.z = 0.0;   
   
7. t.transform.rotation.w = 1.0;

复制代码

      在loop()函数内部，我们填充我们的变换域。坐标系ID设置正确的字符串名字，设置平移和旋转变换的值。

1. t.header.stamp = nh.now();

复制代码

      调用nh.now()函数，返回正确的时间，作为一个ros::Time实例。

1. broadcaster.sendTransform(t);  
   
2. nh.spinOnce();  
   
3. delay(10);

复制代码

      最后我们发布变换，然后调用spinOnce，延时。变换以一定的速度发布，尽管通常我们填充的数据域来自真实数据。

      接着需要编译、上传代码。之后打开一个终端，运行代码：

1. roscore

复制代码

      打开一个新的终端，运行代码：

1. source ~/<ws>/install/setup.bash
   
2. rosrun rosserial_python serial_node.py /dev/ttyACM0

复制代码

      <ws>为放置rosserial包的工作空间

      最后，打开一个新的终端，使用下面的命令检查变换

1. rosrun tf tf_echo odom base_link

复制代码

      或

1. rrosrun tf view_frames

复制代码

这个命令会生成一个pdf文件。
      下面的两个图为执行的命令截图，也包含收到的来自Arduino的坐标变换消息。

      这幅图为运行时的图片，Arduino的TX端口一直亮着，说明在以很高的频率发送信息。

      这幅图为生成的pdf内容，包含坐标变换的信息，如坐标系，发布者，频率等。