ROS高效进阶第一章 -- ROS高级组件之 TF坐标变换 其二

1 资料

本文是TF坐标变换系列的第二篇，我们分别使用cpp和python实现turtle_tf。本文的参考资料如下：
（1）TF坐标变换系列的第一篇，为本文提供了所有的背景知识：ROS高效进阶第一章 – ROS高级组件之 TF坐标变换 其一
（2）《ROS机器人开发实践》胡春旭 第4章第二节

2 正文

2.1 cpp版turtle_tf

（1）创建 learning_tf 软件包，以及相应的文件

cd ~/catkin_ws/src
catkin_create_pkg learning_tf roscpp rospy tf turtlesim

cd learning_tf 
mkdir launch scripts
touch launch/start_cpp.launch launch/start_py.launch
touch src/turtle_tf_broadcaster.cpp src/turtle_tf_listener.cpp
touch scripts/turtle_tf_broadcaster.py scripts/turtle_tf_listener.p

（2）turtle_tf_broadcaster.cpp
对于代码注释里提到的欧拉角和四元数，以及turtlesim::Pose的具体解释，见：ROS高效进阶第一章 – ROS高级组件之 TF坐标变换 其一

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <turtlesim/Pose.h>
#include "cxxopts.hpp"
// poseCb()用于处理turtlesim::Pose消息，根据当前的pose消息内容，制作并广播指定turtle坐标系相对于world坐标系的tf数据
void poseCb(const turtlesim::PoseConstPtr& msg, tf::TransformBroadcaster& br, const std::string& turtle_name) {
    
    
    // 创建tf数据对象
    tf::Transform tf_data;
    // turtlesim::Pose的x,y值，z为0，就是turtle坐标系相对于world坐标系的平移向量
    tf_data.setOrigin(tf::Vector3(msg->x, msg->y, 0.0));
    // 创建四元数类，描述旋转
    tf::Quaternion q;
    // Quaternion提供设置欧拉角的接口，内部转换为四元数。
    // turtlesim::Pose的theta值，就是turtle坐标系相对于world坐标系的yaw（绕z轴）值，因为是平面，其余两个为0
    q.setRPY(0, 0, msg->theta);
    // 将四元数传给tf数据
    tf_data.setRotation(q);
	// 发送一个带时间戳的tf数据，其表示在world坐标系下，指定turtle坐标系的平移和旋转关系
	// 由于turtlesim::Pose的频率是50hz，因此这个tf广播器的频率也是50hz
    br.sendTransform(tf::StampedTransform(tf_data, ros::Time::now(), "world", turtle_name));
}

int main(int argc, char ** argv) {
    
    
    ros::init(argc, argv, "ycao_tf_broadcaster");

    // https://github.com/jarro2783/cxxopts
    cxxopts::Options options(argv[0], "specify turtle name");
    options.add_options()
        ("n,name", "Name of the turtle", cxxopts::value<std::string>())
        ("h,help", "show help");

    auto result = options.parse(argc, argv);
    if (result.count("help")) {
    
    
        ROS_INFO("%s", options.help().c_str());
        return 0;
    }
	// -n指定坐标系名，即turtle1或turtle2，该程序将广播指定坐标系相对于word坐标系的tf数据（平移和旋转）
    if (!result.count("name")) {
    
    
        ROS_ERROR("please specify turtle name using -n");
        return -1;
    }     

    std::string turtle_name = result["name"].as<std::string>();
    std::string pose_topic = turtle_name+"/pose";
    // 创建tf广播器，并传入turtlesim::Pose的回调函数，用来发送tf
    tf::TransformBroadcaster br;
    ros::NodeHandle nh;
    ros::Subscriber sub = nh.subscribe<turtlesim::Pose>(pose_topic, 10, std::bind(poseCb, std::placeholders::_1, std::ref(br), turtle_name));

	ros::spin();
    return 0;
}

（3）turtle_tf_listener.cpp
对于代码注释里提到的geometry_msgs::Twist的具体解释，见：ROS高效进阶第一章 – ROS高级组件之 TF坐标变换 其一

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/Twist.h>
#include <turtlesim/Spawn.h>

int main(int argc, char ** argv) {
    
    
    ros::init(argc, argv, "ycao_tf_listener");
    ros::NodeHandle nh;
    // 调用/spawn服务，生成turtle2
    ros::ServiceClient spawnClient = nh.serviceClient<turtlesim::Spawn>("/spawn");
    ros::service::waitForService("/spawn");
    turtlesim::Spawn::Request req;
    turtlesim::Spawn::Response resp;
    bool ok = spawnClient.call(req, resp);
    if (ok) {
    
    
        ROS_INFO("spawned a turtle named %s", resp.name.c_str());
    } else {
    
    
        ROS_ERROR("Failed to spawn");
    }
	// 创建turtle2的行动控制topic
    ros::Publisher cmd_pub = nh.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel", 10);
	// 创建tf 监听器，以10hz的频率，监听相对于turtle2坐标系，turtle1坐标系的tf值（平移和旋转）
    tf::TransformListener listener;
    ros::Rate loop_rate(10);
    while (nh.ok()) {
    
    
        tf::StampedTransform tf_data;
        try {
    
    
			listener.waitForTransform("/turtle2", "/turtle1", ros::Time(0), ros::Duration(3.0));
			// 监听相对于turtle2坐标系，turtle1坐标系的tf值（平移和旋转）
			listener.lookupTransform("/turtle2", "/turtle1", ros::Time(0), tf_data);            
        } catch (tf::TransformException &ex) {
    
    
            ROS_ERROR("%s", ex.what());
            ros::Duration(1.0).sleep();
            continue;
        }
        
		// 根据turtle1相对于turtle2坐标系的tf值，设置turtle2的运动指令，让turtle2跟着turtle1跑
        geometry_msgs::Twist vel;
        
        // getOrigin()返回相对于turtle2坐标系，turtle1坐标原点的位置，也就是turtle1的位置
        // getRotation()返回相对于turtle2坐标系，turtle1坐标系的旋转角度，用四元数表示，也就是turtle1的朝向，这里用不到这个信息
        
        // atan2(y,x)一种反正切函数，返回turtle1与turtle2坐标原点连线与turtle2坐标系x轴正向之间的夹角，如果turtle1位于turtle2坐标系x轴上方，则为正值，反之为负值
        // 4.0是个经验值，当两者夹角比较大时，turtle2会更快的转向turtle1，随着夹角变小，转的越来越慢，直到夹角为0        
        vel.linear.x = 0.5 * sqrt(pow(tf_data.getOrigin().x(), 2) + pow(tf_data.getOrigin().y(), 2));
        
        // sqrt()是求turtle1与turtle两者的距离，也就是x平方加y平方，然后开根号。
        // 0.5是经验值，距离越大，turtle2的速度越大，随着距离减小，速度越来越慢，直到距离为0
        // 0.5可以控制turtle2的行进速度过快，避免出现“横冲直撞”，导致跟不住turtle1        
        vel.angular.z = 4.0 * atan2(tf_data.getOrigin().y(), tf_data.getOrigin().x());

        // 下面的改动，可以让turtle1控制turtle2的转向，即turtle2跟随turtle1转，但turtle2追不上turtle1，也很好玩
        /*
        double roll, pitch, yaw;
        tf::Quaternion qua = tf_data.getRotation();
        tf::Matrix3x3(qua).getRPY(roll, pitch, yaw);
        vel.angular.z = 4.0 * yaw;
		*/
		
        cmd_pub.publish(vel);
        loop_rate.sleep();
        ros::spinOnce();
    }
    return 0;
}

（4）start_cpp.launch

 <launch>
    <!-- Turtlesim Node-->
    <node pkg="turtlesim" type="turtlesim_node" name="sim"/>
    <node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>

	<!-- 广播两个turtle坐标系相对于word坐标系的tf -->
    <node pkg="learning_tf" type="learning_tf_broadcaster" args="-n /turtle1" name="turtle1_tf_broadcaster" />
    <node pkg="learning_tf" type="learning_tf_broadcaster" args="-n /turtle2" name="turtle2_tf_broadcaster" />
	
	<!-- 创建turtle2，监听turtle1相对于turtle2坐标系的tf值，根据tf值设置turtle2的运动指令，让turtle2跟着turtle1跑 -->
    <node pkg="learning_tf" type="learning_tf_listener" name="listener" />
  </launch>

2.2 python版turtle_tf

（1）turtle_tf_broadcaster.py

#! /usr/bin/env python3
import roslib
import rospy
from functools import partial
import tf
from turtlesim.msg import Pose

def pose_cb(msg, tf_br, turtlename):
  # 设置指定turtle坐标系相对于work坐标系的平移数据
  translate_date = (msg.x, msg.y, 0)
  # quaternion_from_euler直译过来就是来自欧拉角的四元数
  # 设置指定turtle坐标系相对于world坐标系的旋转数据
  rotation_data = tf.transformations.quaternion_from_euler(0, 0, msg.theta)
  # 发送相对于world坐标系，指定turtle坐标系的tf（注意：这里的world写在后面，cpp里是写在前面）
  tf_br.sendTransform(translate_date, rotation_data, rospy.Time.now(), turtlename, "world")

def main():
  rospy.init_node("turtle_tf_broadcaster")
  
  # python版的tf广播器，利用全局参数传参
  # rospy.get_param()是python的参数获取接口，cpp里是ros::param::get()
  turtlename = rospy.get_param("~turtle")

  pose_topic = "/%s/pose" %turtlename
  tf_br = tf.TransformBroadcaster()
  bind_pose_cb = partial(pose_cb, tf_br=tf_br, turtlename=turtlename)
  rospy.Subscriber(pose_topic, Pose, bind_pose_cb)
  rospy.spin()

if __name__ == "__main__":
  main()

（2）turtle_tf_listener.py
实现的逻辑与cpp版，完全一样

#！/usr/bin/env python3
import roslib
import rospy
import math
import tf
from geometry_msgs.msg import Twist
from turtlesim.srv import Spawn

def main():
  rospy.init_node("turtle_tf_listener")
  
  rospy.wait_for_service("spawn")
  client = rospy.ServiceProxy("spawn", Spawn)
  try:
    req = SpawnRequest()
    resp = client(req)
    rospy.loginfo("spawned a turtle name %s" %resp.name)
  except rospy.ServiceException as e:
      rospy.loginfo("service call fail: %s" %e)

  cmd_pub= rospy.Publisher("/turtle2/cmd_vel", Twist, queue_size=10)
  
  tf_listener = tf.TransformListener()
  rate = rospy.Rate(10)
  while not rospy.is_shutdown():
    try:
      (translation, rotation) = tf_listener.lookupTransform("/turtle2", "turtle1", rospy.Time(0))
    except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
      continue

    cmd_vel = Twist()
    cmd_vel.linear.x= 0.5 * math.sqrt(trans[0] ** 2 + trans[1] ** 2)
    cmd_vel.angular.z = 4 * math.atan2(translation[1], translation[0])

    # 下面的改动，可以让turtle1控制turtle2的转向，即turtle2跟随turtle1转，但turtle2追不上turtle1
    # euler = (roll, pitch, yaw)    
    '''
    quaternion = rotation
    euler = tf.transformations.euler_from_quaternion(quaternion)
    cmd_vel.angular.z = 4 * euler[2]
    '''
    
    cmd_pub.publish(cmd_vel)
    rate.sleep()

if __name__ == "__main__":
  main()

（3）start_py.launch
参考上面的start_cpp.launch就能看懂，唯一的区别就是使用全局参数给 tf 广播器传参

 <launch>
    <!-- Turtlesim Node-->
    <node pkg="turtlesim" type="turtlesim_node" name="sim"/>
    <node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>

    <node pkg="learning_tf" type="turtle_tf_broadcaster.py" name="turtle1_tf_broadcaster" respawn="false" output="screen">
      <param name="turtle" type="string" value="turtle1"/>
    </node>
    <node pkg="learning_tf" type="turtle_tf_broadcaster.py" name="turtle2_tf_broadcaster" respawn="false" output="screen">
      <param name="turtle" type="string" value="turtle2"/>
    </node>

    <node pkg="learning_tf" type="learning_tf_listener" name="listener" />
  </launch>

2.3 编译和运行

（1）CMakeLists.txt

cmake_minimum_required(VERSION 3.0.2)
project(learning_tf)
find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  tf
  turtlesim
)
catkin_package()
include_directories(
  ${
    
    catkin_INCLUDE_DIRS}
  include/learning_tf
)
add_executable(${
    
    PROJECT_NAME}_broadcaster src/turtle_tf_broadcaster.cpp)
add_executable(${
    
    PROJECT_NAME}_listener src/turtle_tf_listener.cpp)
target_link_libraries(${
    
    PROJECT_NAME}_broadcaster
  ${
    
    catkin_LIBRARIES}
)
target_link_libraries(${
    
    PROJECT_NAME}_listener
  ${
    
    catkin_LIBRARIES}
)
catkin_install_python(PROGRAMS
  scripts/turtle_tf_broadcaster.py scripts/turtle_tf_listener.py
  DESTINATION ${
    
    CATKIN_PACKAGE_BIN_DESTINATION}
)

（2）编译运行

cd ~/catkin_ws/
catkin_make --source src/learning_tf
source devel/setup.bash
roslaunch learning_tf start_cpp.launch
roslaunch learning_tf start_py.launch

3 总结

本节代码托管在本人的github上：learning_tf