ROS tf基础教程（1）

目标功能

• 在tf教程中，我们需要创建四种节点，分别是键盘控制节点teleop、乌龟仿真节点turtlesim、tf广播节点tf broadcaster、tf监听节点tf listener。
• turtlesim启动后会默认生成一只小乌龟“turtle1”，tf listener启动时负责在仿真器中生成另一只小乌龟“turtle2”。
• teleop发布turtle1/command_velocity话题，turtlesim订阅后“turtle1”开始运动。
• turtlesim发布/turtle1/pose和/turtle2/pose话题，tf broadcaster订阅后转换成tf::StampedTransform格式，得到“turtle1”和“turtle2”各自的坐标系，并进行广播。
• tf listener监听到坐标系广播后，根据“turtle1”和“turtle2”之间的变换关系，计算出“turtle2”跟随“turtle1”所需要的线速度和角速度，并发布turtle2/command_velocity话题，turtlesim订阅后，“turtle2”开始跟随运动。

工程文件框架

假设名为ros_ws的文件夹用来存放ROS程序

• ros_ws
  • build
  • devel
  • src
    • learning_tf
      • include
      • launch
        • start_demo.launch
      • src
        • turtle_tf_broadcaster.cpp
        • turtle_tf_listener.cpp
      • CMakeLists.txt
      • package.xml
    • CMakeLists.txt

乌龟仿真节点和键盘控制节点

在launch文件中设置需要启动的节点以及所需参数

  <launch>
    <!-- Turtlesim节点-->
    <node pkg="turtlesim" type="turtlesim_node" name="sim"/>
    <node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>
    
    <!-- Axes -->
    <param name="scale_linear" value="2" type="double"/>
    <param name="scale_angular" value="2" type="double"/>
  </launch>

sim节点启动后，仿真器中会出现一个随机外形的小乌龟，默认命名为“turtle1”。turtlesim这个包提供了spawn服务用来生成小乌龟，命名规则是按生成的序号，如“turtle2”、“turtle3”。默认键盘控制第一个小乌龟“turtle1”。

tf广播节点

turtle_tf_broadcaster.cpp源代码为：

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <turtlesim/Pose.h>

std::string turtle_name;
/*
* @ brief 接收到小乌龟的位姿数据后，转换成Ｔransoform格式并广播
* @ param msg 从turtlesim订阅的乌龟位姿
*/
void poseCallback(const turtlesim::PoseConstPtr& msg){
  static tf::TransformBroadcaster br;
  tf::Transform transform;
  transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );
  tf::Quaternion q;
  q.setRPY(0, 0, msg->theta);
  transform.setRotation(q);
  br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
}

int main(int argc, char** argv){
  ros::init(argc, argv, "my_tf_broadcaster");
  if (argc != 2)
  {
  	ROS_ERROR("need turtle name as argument"); 
  	return -1;
  };
  turtle_name = argv[1];
  ros::NodeHandle node;
  ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);
  ros::spin();
  return 0;
};

每次从话题/turtlex/pose（这里x等于1或2）得到小乌龟二维位姿的数据，就转换为一个关于三维位姿的变换矩阵（从世界坐标系“world”变换到乌龟坐标系“turtlex”），然后发布出去。

注意到sendTransform的第二个参数，是指发布该变换矩阵的时间，这里用的是系统的当前时间。

为了生成可执行文件并顺利启动，在CMakeLists.txt中添加：

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

在launch文件中添加：

 <node pkg="learning_tf" type="turtle_tf_broadcaster" args="/turtle1" name="turtle1_tf_broadcaster" />
 <node pkg="learning_tf" type="turtle_tf_broadcaster" args="/turtle2" name="turtle2_tf_broadcaster" />

tf监听节点

turtle_tf_listener.cpp源代码为：

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <turtlesim/Velocity.h>
#include <turtlesim/Spawn.h>
int main(int argc, char** argv){
  ros::init(argc, argv, "my_tf_listener");
  ros::NodeHandle node;
  ros::service::waitForService("spawn");
  ros::ServiceClient add_turtle = node.serviceClient<turtlesim::Spawn>("spawn");
  turtlesim::Spawn srv;
  add_turtle.call(srv);

  ros::Publisher turtle_vel = node.advertise<turtlesim::Velocity>("turtle2/command_velocity", 10);
  tf::TransformListener listener;
  ros::Rate rate(10.0);		//发布频率为10Hz，即每100ms发布一次
  
  while (node.ok()){
    /* 监听turtle2到turtle1的变换 */
    tf::StampedTransform transform;
    try{
      listener.lookupTransform("/turtle2", "/turtle1", ros::Time(0), transform);
    }catch (tf::TransformException ex){
      ROS_ERROR("%s",ex.what());
      ros::Duration(1.0).sleep();
    }
    
    /* 将tf::StampedTransform transform格式转换为turtlesim::Velocity格式并发布 */
    turtlesim::Velocity vel_msg;
    vel_msg.angular.z = 4.0 * atan2(transform.getOrigin().y(), transform.getOrigin().x());
    vel_msg.linear.x = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) + pow(transform.getOrigin().y(), 2));
    turtle_vel.publish(vel_msg);
    rate.sleep();
  }
  return 0;
};

前半部分是调用了turtlesim包提供的spawn服务来创建新的乌龟，后半部分的主循环有两个任务：

• 获取两只乌龟最新的以变换矩阵形式存在的位姿信息，计算从“turtle2”到“turtle1”的变换矩阵。
• 计算“turtle2”为了跟上“turtle1”所需要的线速度和角速度，并以turtlesim::Velocity的数据格式在发布"turtle2/command_velocity"话题。

为了生成可执行文件并顺利启动，在CMakeLists.txt中添加：

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

在launch文件中添加：

<node pkg="learning_tf" type="turtle_tf_listener" name="listener" />