ROS与Arduino-使用ros::Time和TF

使用ros::Time和TF

说明

• 这个教程展示使用ros::Time和TF在Arduino上创建tf publisher

代码

/* 
 * rosserial Time and TF Example
 * Publishes a transform at current time
 */

#include <ros.h>
#include <ros/time.h>
#include <tf/transform_broadcaster.h>

ros::NodeHandle  nh;

geometry_msgs::TransformStamped t;
tf::TransformBroadcaster broadcaster;

char base_link[] = "/base_link";
char odom[] = "/odom";

void setup()
{
  nh.initNode();
  broadcaster.init(nh);
}

void loop()
{  
  t.header.frame_id = odom;
  t.child_frame_id = base_link;
  t.transform.translation.x = 1.0; 
  t.transform.rotation.x = 0.0;
  t.transform.rotation.y = 0.0; 
  t.transform.rotation.z = 0.0; 
  t.transform.rotation.w = 1.0;  
  t.header.stamp = nh.now();
  broadcaster.sendTransform(t);
  nh.spinOnce();
  delay(10);
}

代码解释

1. 代码

#include <ros.h>
#include <ros/time.h>
#include <tf/transform_broadcaster.h>

解释：

• 必需包含的ros头文件
• 包含transform broadcaster头文件

2. 代码

geometry_msgs::TransformStamped t;
tf::TransformBroadcaster broadcaster;

char base_link[] = "/base_link";
char odom[] = "/odom";

解释：

• 实例化TransformStamped消息类，broadcaster类
• 指定需要进行转换的参考系名称/base_link，/odom

3. 代码

void setup()
{
  nh.initNode();
  broadcaster.init(nh);
}

解释：

• Arduino的setup函数，初始化节点处理类，初始化broadcaster类，使用节点处理类作为参数。
• 不这样做，broadcaster的发布就不起作用。

4. 代码

  t.header.frame_id = odom;
  t.child_frame_id = base_link;
  t.transform.translation.x = 1.0; 
  t.transform.rotation.x = 0.0;
  t.transform.rotation.y = 0.0; 
  t.transform.rotation.z = 0.0; 
  t.transform.rotation.w = 1.0; 

解释：

• Arduino的loop函数内，设置不同参考系的名称，及转换的值内容

5. 代码

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

解释：

• 调用nh.now()，返回当前的时间。作为ros::Time 的实例，就像使用 roscpp's ros::Time::now().

6. 代码

  broadcaster.sendTransform(t);
  nh.spinOnce();
  delay(10);

解释：

• 发布转换，调用spinOnce之后，需要等待一会，在循环。
• 转换都是按正常速率发布。
• 数据字段将从实际数据中填充

代码上传

• 利用Arduino IDE的upload功能上传代码到Arduino

运行代码

1. 运行roscore

$ roscore

2. 新终端运行，/dev/ttyUSB0为Arduino设备

rosrun rosserial_python serial_node.py /dev/ttyUSB0

或

rosrun rosserial_python serial_node.py _port:=/dev/ttyUSB0

3. 检查是否成功

rosrun tf tf_echo odom base_link

或

rosrun tf view_frames