《ROS精品入门》学习笔记四：ROS中的空间描述与变换

一.学习内容
本节课主要讲了一下内容：
1.空间描述与变换的基本概念
2.ROS中tf的使用方法
3. tf使用示例。示例一：使用tf使一个turtle跟踪另一个turtle；示例二：使用tf将base_laser坐标系下的数据转换到base_link坐标系下。
二.学习讲义



三.学习笔记
1. 在理论概念部分，感觉老师讲的有点问题（尤其PPT的P5页感觉都不太对）。这一块感觉还是找本机器人方面的书看一下比较好，我这儿参考了《机器人学导论》的第2章 “空间描述和变换”；《机器人学、机器视觉与控制》的第2章“位置与姿态描述”； 还有 WIKI上的“四元数”、“四元数与空间旋转”、“旋转矩阵”、“欧拉角”“Euler angles”；博客园的博文“三维旋转：旋转矩阵，欧拉角，四元数”。
2. 学习ROS其实wiki才是最好的学习资料，这里给几个网址，
A. tf的整体介绍：http://wiki.ros.org/tf
B. tf教程：http://wiki.ros.org/tf/Tutorials
C. tf介绍：http://wiki.ros.org/tf/Tutorials/Introduction%20to%20tf
其实这几个网址都是在一块的，建议把wiki上的教程看一看，写的很好，毕竟是最权威的。
3. tf中有一些常用的命令行工具，官网上都有详细介绍，其中我比较喜欢的：
rosrun rqt_tf_tree rqt_tf_tree (a runtime tool: creates a diagram of the frames being broadcast by ROS)
rosrun tf tf_echo [reference_frame] [target_frame] (the transform of the target_frame respect to the reference_frame)
4. 示例代码中用到了boost::function, boost::bind, boost::ref, 可看一下《Boost程序库完全开发指南》第11章 函数与回调
5. 运行程序时我们看一下代码中的节点图，以此分析一下整个程序的运行框架。
示例1：

示例2：

四.相关源码
1. 示例一package目录树：

learning_tf
--include
--src
   --turtle_tf_broadcaster.cpp
   --turtle_tf_listener.cpp
--launch
   --demo.launch
--CMakeLists.txt
--package.xml

源码：
turtle_tf_broadcaster.cpp:

#include
#include
#include

std::string turtle_name;
void poseCallback(const turtlesim::PoseConstPtr &msg){
    static tf::TransformBroadcaster br;
    tf::Transform transform;
    transform.setOrigin(tf::Vector3(msg->x, msg->y, 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, "turtle_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;
}

turtle_tf_listener.cpp:

#include
#include
#include
#include

int main(int argc, char** argv) {
    ros::init(argc, argv, "turtle_tf_listener");
    ros::NodeHandle node;
    ros::service::waitForService("spawn");  //等待，直到服务"spawn"出现。只有这样才能请求此服务再产生一个turtle
    ros::ServiceClient add_turtle=node.serviceClient<turtlesim::Spawn>("spawn");
    turtlesim::Spawn srv;
    add_turtle.call(srv);    //调用服务再产生一个turtle

    ros::Publisher turtle_vel=node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 10);
    tf::TransformListener listener;
    ros::Rate rate(10.0);
    while(node.ok()) {
        tf::StampedTransform transform;
        try {
            listener.lookupTransform("/turtle2", "/turtle1", ros::Time(0), transform); //get the Transform from turtle2 to turtle1
        }
        catch(tf::TransformException &ex) {
            ROS_ERROR("%s", ex.what());
            ros::Duration(1).sleep();
            continue;
        }
        geometry_msgs::Twist 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);  //发布消息使turtle2运动
        rate.sleep();
    }
    return 0;
}

demo.launch:

"turtlesim" type="turtlesim_node" name="sim" />
"turtlesim" type="turtle_teleop_key" name="teleop" output="screen" />
"learning_tf" type="turtle_tf_broadcaster" args="/turtle1" name="turtle1_tf" />
"learning_tf" type="turtle_tf_broadcaster" args="/turtle2" name="turtle2_tf" />
"learning_tf" type="turtle_tf_listener" name="listener" />

CMakeLists.txt:

cmake_minimum_required(VERSION 2.8.3)
project(learning_tf)

## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  turtlesim
  tf
)

## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)


## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()

################################################
## Declare ROS messages, services and actions ##
################################################

## To declare and build messages, services or actions from within this
## package, follow these steps:
## * Let MSG_DEP_SET be the set of packages whose message types you use in
##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
## * In the file package.xml:
##   * add a build_depend tag for "message_generation"
##   * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
##     but can be declared for certainty nonetheless:
##     * add a run_depend tag for "message_runtime"
## * In this file (CMakeLists.txt):
##   * add "message_generation" and every package in MSG_DEP_SET to
##     find_package(catkin REQUIRED COMPONENTS ...)
##   * add "message_runtime" and every package in MSG_DEP_SET to
##     catkin_package(CATKIN_DEPENDS ...)
##   * uncomment the add_*_files sections below as needed
##     and list every .msg/.srv/.action file to be processed
##   * uncomment the generate_messages entry below
##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)

## Generate messages in the 'msg' folder
# add_message_files(
#   FILES
#   Message1.msg
#   Message2.msg
# )

## Generate services in the 'srv' folder
# add_service_files(
#   FILES
#   Service1.srv
#   Service2.srv
# )

## Generate actions in the 'action' folder
# add_action_files(
#   FILES
#   Action1.action
#   Action2.action
# )

## Generate added messages and services with any dependencies listed here
# generate_messages(
#   DEPENDENCIES
#   std_msgs  # Or other packages containing msgs
# )

################################################
## Declare ROS dynamic reconfigure parameters ##
################################################

## To declare and build dynamic reconfigure parameters within this
## package, follow these steps:
## * In the file package.xml:
##   * add a build_depend and a run_depend tag for "dynamic_reconfigure"
## * In this file (CMakeLists.txt):
##   * add "dynamic_reconfigure" to
##     find_package(catkin REQUIRED COMPONENTS ...)
##   * uncomment the "generate_dynamic_reconfigure_options" section below
##     and list every .cfg file to be processed

## Generate dynamic reconfigure parameters in the 'cfg' folder
# generate_dynamic_reconfigure_options(
#   cfg/DynReconf1.cfg
#   cfg/DynReconf2.cfg
# )

###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if you package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES learning_tf
#  CATKIN_DEPENDS roscpp rospy
#  DEPENDS system_lib
)

###########
## Build ##
###########

## Specify additional locations of header files
## Your package locations should be listed before other locations
# include_directories(include)
include_directories(
  ${catkin_INCLUDE_DIRS}
)

## Declare a C++ library
# add_library(learning_tf
#   src/${PROJECT_NAME}/learning_tf.cpp
# )

## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure
# add_dependencies(learning_tf ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

## Declare a C++ executable
 add_executable(turtle_tf_broadcaster src/turtle_tf_broadcaster.cpp)
  add_executable(turtle_tf_listener src/turtle_tf_listener.cpp)

## Add cmake target dependencies of the executable
## same as for the library above
# add_dependencies(learning_tf_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

## Specify libraries to link a library or executable target against
 target_link_libraries(turtle_tf_broadcaster
   ${catkin_LIBRARIES}
 )
  target_link_libraries(turtle_tf_listener
   ${catkin_LIBRARIES}
 )

#############
## Install ##
#############

# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html

## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# install(PROGRAMS
#   scripts/my_python_script
#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )

## Mark executables and/or libraries for installation
# install(TARGETS learning_tf learning_tf_node
#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )

## Mark cpp header files for installation
# install(DIRECTORY include/${PROJECT_NAME}/
#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
#   FILES_MATCHING PATTERN "*.h"
#   PATTERN ".svn" EXCLUDE
# )

## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
#   # myfile1
#   # myfile2
#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )

#############
## Testing ##
#############

## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_learning_tf.cpp)
# if(TARGET ${PROJECT_NAME}-test)
#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()

## Add folders to be run by python nosetests
# catkin_add_nosetests(test)

package.xml:

<package>
  <name>learning_tfname>
  <version>0.0.0version>
  <description>The learning_tf packagedescription>

   
  
  
  <maintainer email="[email protected]">aicrobomaintainer>


  
  
  
  <license>TODOlicense>


  
  
  
  


  
  
  
  


  
  
  
  
  
  
  
  
  
  
  
  <buildtool_depend>catkinbuildtool_depend>
  <build_depend>roscppbuild_depend>
  <build_depend>rospybuild_depend>
  <run_depend>roscpprun_depend>
  <run_depend>rospyrun_depend>


  
  <export>
    

  export>
package>

1. 示例二package目录树：

robot_serup_tf
--include
--src
   --tf_listener.cpp
   --tf_publisher.cpp
--launch
   --demo.launch
--CMakeLists.txt
--package.xml

源码：
tf_listener.cpp:

#include<ros/ros.h>
#include<geometry_msgs/PointStamped.h>
#include<tf/transform_listener.h>

void transformPoint(const tf::TransformListener &listener) {
    geometry_msgs::PointStamped laser_point;
    laser_point.header.frame_id="base_laser";
    laser_point.header.stamp=ros::Time();
    laser_point.point.x=rand()%5;
    laser_point.point.y=rand()%5;
    laser_point.point.z=rand()%5;
    try{
        geometry_msgs::PointStamped base_point;
        listener.transformPoint("base_link", laser_point, base_point); //Transform a Stamped Point Message into the target frame

        ROS_INFO("base_laser:(%.2f, %.2f, %.2f) -----> base_link:(%.2f, %.2f, %.2f) at time %.2f",laser_point.point.x, laser_point.point.y, laser_point.point.z, base_point.point.x, base_point.point.y, base_point.point.z, base_point.header.stamp.toSec());
    }
    catch(tf::TransformException &ex) {
        ROS_ERROR("Received an exception trying to transform a point form \"base_laser\" to \"base_link\": %s", ex.what());
    }
}

int main(int argc, char** argv){
    ros::init(argc, argv, "tf_listener");
    ros::NodeHandle n;
    tf::TransformListener listener(ros::Duration(10));
    //we'll transform a point once every second
    //typedef boost::function TimerCallback;
    //Timer createTimer(Duration period, void(T::*callback)........
    //对自由方法来说，直接boost::bind(函数名, 参数1，参数2，...)
    //对类方法来说，直接boost::bind(&类名::方法名，类实例指针，参数1，参数2）
    ros::Timer timer=n.createTimer(ros::Duration(1.0),boost::bind(&transformPoint, boost::ref(listener)));
    ros::spin();
    return 0;
}

tf_publisher.cpp:

#include
#include

int main(int argc, char** argv) {
    ros::init(argc, argv, "tf_publisher");
    ros::NodeHandle n;
    ros::Rate r(100);
    tf::TransformBroadcaster broadcaster;
    tf::Transform transform;
    transform.setOrigin(tf::Vector3(0.1, 0, 0.2));
    tf::Quaternion q;
    q.setRPY(0, 0, 0);
    transform.setRotation(q);
    while(n.ok()) {
        broadcaster.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "base_link", "base_laser"));  //发布了base_link和base_laser之间的坐标关系
        r.sleep();
    }

    return 0;
}

demo.launch:

<launch>
  
  <node pkg="tf" type="static_transform_publisher" name="broadcaster" args="0.1 0.0 0.2 0 0 0 1 base_link base_camera 100" />
  <node pkg="robot_serup_tf" type="tf_publisher" name="publisher" />
  <node pkg="robot_serup_tf" type="tf_listener" name="listener" output="screen"/>
launch>

CMakeLists.txt:

cmake_minimum_required(VERSION 2.8.3)
project(robot_serup_tf)

## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
  geometry_msgs
  roscpp
  tf
)

## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)


## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()

################################################
## Declare ROS messages, services and actions ##
################################################

## To declare and build messages, services or actions from within this
## package, follow these steps:
## * Let MSG_DEP_SET be the set of packages whose message types you use in
##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
## * In the file package.xml:
##   * add a build_depend tag for "message_generation"
##   * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
##     but can be declared for certainty nonetheless:
##     * add a run_depend tag for "message_runtime"
## * In this file (CMakeLists.txt):
##   * add "message_generation" and every package in MSG_DEP_SET to
##     find_package(catkin REQUIRED COMPONENTS ...)
##   * add "message_runtime" and every package in MSG_DEP_SET to
##     catkin_package(CATKIN_DEPENDS ...)
##   * uncomment the add_*_files sections below as needed
##     and list every .msg/.srv/.action file to be processed
##   * uncomment the generate_messages entry below
##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)

## Generate messages in the 'msg' folder
# add_message_files(
#   FILES
#   Message1.msg
#   Message2.msg
# )

## Generate services in the 'srv' folder
# add_service_files(
#   FILES
#   Service1.srv
#   Service2.srv
# )

## Generate actions in the 'action' folder
# add_action_files(
#   FILES
#   Action1.action
#   Action2.action
# )

## Generate added messages and services with any dependencies listed here
# generate_messages(
#   DEPENDENCIES
#   geometry_msgs
# )

################################################
## Declare ROS dynamic reconfigure parameters ##
################################################

## To declare and build dynamic reconfigure parameters within this
## package, follow these steps:
## * In the file package.xml:
##   * add a build_depend and a run_depend tag for "dynamic_reconfigure"
## * In this file (CMakeLists.txt):
##   * add "dynamic_reconfigure" to
##     find_package(catkin REQUIRED COMPONENTS ...)
##   * uncomment the "generate_dynamic_reconfigure_options" section below
##     and list every .cfg file to be processed

## Generate dynamic reconfigure parameters in the 'cfg' folder
# generate_dynamic_reconfigure_options(
#   cfg/DynReconf1.cfg
#   cfg/DynReconf2.cfg
# )

###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if you package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES robot_serup_tf
#  CATKIN_DEPENDS geometry_msgs roscpp tf
#  DEPENDS system_lib
)

###########
## Build ##
###########

## Specify additional locations of header files
## Your package locations should be listed before other locations
# include_directories(include)
include_directories(
  ${catkin_INCLUDE_DIRS}
)

## Declare a C++ library
# add_library(robot_serup_tf
#   src/${PROJECT_NAME}/robot_serup_tf.cpp
# )

## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure
# add_dependencies(robot_serup_tf ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

## Declare a C++ executable
 add_executable(tf_listener src/tf_listener.cpp)
 add_executable(tf_publisher src/tf_publisher.cpp)

## Add cmake target dependencies of the executable
## same as for the library above
# add_dependencies(robot_serup_tf_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

## Specify libraries to link a library or executable target against
 target_link_libraries(tf_listener 
   ${catkin_LIBRARIES}
 )
 target_link_libraries(tf_publisher
   ${catkin_LIBRARIES}
 )

#############
## Install ##
#############

# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html

## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# install(PROGRAMS
#   scripts/my_python_script
#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )

## Mark executables and/or libraries for installation
# install(TARGETS robot_serup_tf robot_serup_tf_node
#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )

## Mark cpp header files for installation
# install(DIRECTORY include/${PROJECT_NAME}/
#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
#   FILES_MATCHING PATTERN "*.h"
#   PATTERN ".svn" EXCLUDE
# )

## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
#   # myfile1
#   # myfile2
#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )

#############
## Testing ##
#############

## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_robot_serup_tf.cpp)
# if(TARGET ${PROJECT_NAME}-test)
#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()

## Add folders to be run by python nosetests
# catkin_add_nosetests(test)

package.xml:

<package>
  <name>robot_serup_tfname>
  <version>0.0.0version>
  <description>The robot_serup_tf packagedescription>

   
  
  
  <maintainer email="[email protected]">aicrobomaintainer>


  
  
  
  <license>TODOlicense>


  
  
  
  


  
  
  
  


  
  
  
  
  
  
  
  
  
  
  
  <buildtool_depend>catkinbuildtool_depend>
  <build_depend>geometry_msgsbuild_depend>
  <build_depend>roscppbuild_depend>
  <build_depend>tfbuild_depend>
  <run_depend>geometry_msgsrun_depend>
  <run_depend>roscpprun_depend>
  <run_depend>tfrun_depend>


  
  <export>
    

  export>
package>