坐标转换tf应用

tf::MessageFilter

• message_filters是一个用于roscpp和rospy的实用程序库
• 它集合了许多的常用的消息“过滤”算法
• 消息过滤器message_filters类似一个消息缓存，当消息到达消息过滤器的时候，可能并不会立即输出，而是在稍后的时间点里满足一定条件下输出
• tf::MessageFilter可以接收所有指定类型的消息，然后过滤出能够在指定坐标系中转换的消息
• 为了提供统一的接口与输出，message_filters中所有的消息过滤器都遵循着相同的模式连接输入和输出
  • 输入:通过过滤器构造函数或者是connectInput()方法链接
  • 输出:通过registerCallback()方法连接,可以使用registerCallback()方法注册多个回调，他们将会按照注册的顺序依次进行调用

思考：如果知道一个物体在全局坐标系中的位置，如何将该物体的位置转化到指定坐标系的位置?

操作小记

在/src目录下新建turtle_tf_message_filter.cpp文件，输入以下代码

#include "ros/ros.h"
#include "tf/transform_listener.h"
#include "tf/message_filter.h"
#include "message_filters/subscriber.h"

class PoseDrawer
{
public:
  //初始化消息过滤器，指定目标作为为turtle1
  //当订阅到能够转化到turtle1坐标系中的点时，触发回调函数
  PoseDrawer() : tf_(),  target_frame_("turtle1")
  {
    point_sub_.subscribe(n_, "turtle_point_stamped", 10);
    tf_filter_ = new tf::MessageFilter<geometry_msgs::PointStamped>(point_sub_, tf_, target_frame_, 10);
    tf_filter_->registerCallback( boost::bind(&PoseDrawer::msgCallback, this, _1) );
  } ;

private:
  //定义消息订阅器
  message_filters::Subscriber<geometry_msgs::PointStamped> point_sub_;
  tf::TransformListener tf_;
  //订阅消息过滤器
  tf::MessageFilter<geometry_msgs::PointStamped> * tf_filter_;
  ros::NodeHandle n_;
  std::string target_frame_;

  //回调函数中将过滤出来的点转化到turtle1坐标系中，并打印
  void msgCallback(const boost::shared_ptr<const geometry_msgs::PointStamped>& point_ptr) 
  {
    geometry_msgs::PointStamped point_out;
    try 
    {
      tf_.transformPoint(target_frame_, *point_ptr, point_out);
      
      printf("point of turtle 3 in frame of turtle 1 Position(x:%f y:%f z:%f)\n", 
             point_out.point.x,
             point_out.point.y,
             point_out.point.z);
    }
    catch (tf::TransformException &ex) 
    {
      printf ("Failure %s\n", ex.what()); //Print exception which was caught
    }
  };

};

修改CMakeList.txt文件

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

编译：catkin_make

在launch文件夹下新建：turtle_tf_sensor.launch 输入以下代码：

<launch>
  <include file="$(find turtle_tf)/launch/turtle_tf_demo.launch" />
  <node name="turtle_pose_stamped_publisher" pkg="turtle_tf" type="turtle_tf_message_broadcaster.py" respawn="false" output="screen" />
  <node name="turtle_tf_message_filter" pkg="learning_tf" type="turtle_tf_message_filter" respawn="false" output="screen" />
</launch>

运行：roslaunch learning_tf turtle_tf_sensor.launch

rqt_graph：观察节点之间的信息流

使用tf设置机器人

本节学习robot state publisher 发布机器人tf的原理：在自己的机器人上使用robot state publisher
如下图所示，是一个轮式机器人，机器上安装了一个二位激光雷达。定义两个坐标系：

• base_link以车体中心为坐标系原点
• laser_link以二位激光雷达中心为坐标系
  
  假设二维激光雷达测量到障碍物的坐标为：(0.3, 0, 0)。这个坐标是以laser_link为基坐标系得到的坐标，如果要机器人避开该障碍，那么需要知道该障碍在机器人坐标系(base_link)中的位置。这就需要我们进行转换，如下图所示：
  
  我们可以选择自己管理这些关系，但是将会非常繁琐。所幸我们可以用tf来定义这两个坐标系之间的关系

操作小记

接下来我们将创建一个tf树来实现这一简单示例的变换。
创建两个节点，一个用于base_link坐标系，一个用于base_laser坐标系。以base_link为父坐标系，base_lase作为子坐标系，然后创建一个节点负责在base_laser中获取点并将之转化到base_link中

创建一个功能包：catkin_create_pkg robot_setup_tf roscpp tf geometry_msgs
安装一些依赖：sudo apt-get install ros-kinetic-navigation-tutorials
在src/文件夹下，创建文件tf_broadcaster. cpp，输入以下代码：

#include <ros/ros.h>
// 头文件包含
#include <tf/transform_broadcaster.h>

int main(int argc, char** argv){
  ros::init(argc, argv, "robot_tf_publisher");
  ros::NodeHandle n;

  ros::Rate r(100);

  // 创建一个用以发布base_link到base_laser的坐标变换
  tf::TransformBroadcaster broadcaster;

  while(n.ok()){
    broadcaster.sendTransform(
      tf::StampedTransform(
        tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.1, 0.0, 0.2)),
        ros::Time::now(),"base_link", "base_laser"));
    r.sleep();
  }
}

说明：TransformBroadcaster需要五个参数：

• 第一个是一个四元数表示的俯仰、偏航、翻滚角，这里我们三个都取零
• 第二个参数用一个向量表示坐标的变换
• 第三个表示时间戳
• 第四个表示父节点名称
• 第五个表示子节点名称

前面我们创建了一个节点来发布 base_laser --> base_link 的坐标变换。现在，我们将编写一个节点，使用该坐标变换在“base_laser”中获取一个点，并将其转换为“base_link”中的一个点
在src/文件夹下创建tf_listener.cpp文件，并添加以下内容：

 #include <ros/ros.h>
 #include <geometry_msgs/PointStamped.h>
 //添加头文件
 #include <tf/transform_listener.h>
 
 //创建一个函数给定一个TransformListener对象
 //获取base_laser中的一个点，并转换到base_link坐标系中
 void transformPoint(const tf::TransformListener& listener){
   //这里创建一个geometry_msgs::PointStamped的对象存放一个点的信息
   geometry_msgs::PointStamped laser_point;
   //里面头午的frame_id字段设置为base_laser
   laser_point.header.frame_id = "base_laser";
 
   //stamp字段填入时间戳
   laser_point.header.stamp = ros::Time();
 
   //point.x y z的值分别设置为1.0 0.2 0.0
   laser_point.point.x = 1.0;
   laser_point.point.y = 0.2;
   laser_point.point.z = 0.0;
 
   try{
     //现在我们在base_laser下有一个点了，我们希望将其转换到base_link中
     //所以我们使用TransformListener对象，并调用transformPoint()
     geometry_msgs::PointStamped base_point;
     listener.transformPoint("base_link", laser_point, base_point);
 
     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){
     //如果处于某种原因，base_laser到base_link的转换不能被TF发布
     //当transformPoint()被调用时，TransformListener会抛出一个异常
     ROS_ERROR("Received an exception trying to transform a point from \"base_laser\" to \"base_link\": %s", ex.what());
   }
 }
 
 int main(int argc, char** argv){
   ros::init(argc, argv, "robot_tf_listener");
   ros::NodeHandle n;
 
   tf::TransformListener listener(ros::Duration(10));
 
   //we'll transform a point once every second
   ros::Timer timer = n.createTimer(ros::Duration(1.0), boost::bind(&transformPoint, boost::ref(listener)));
 
   ros::spin();
 
 }

在CMakeLists.txt文件的末尾添加以下内容：

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

编译工作空间：catkin_make

接下来我们运行一下我们的程序，在第一个终端运行 roscore
在第二个终端运行tf_broadcaster：rosrun robot_setup_tf tf_broadcaster
然后我们在第三个终端运行tf_listenner显示我们的模拟点从base_laser到base_link的转换rosrun robot_setup_tf tf_listener
如果没出错，则会是如图所示：

在机器人上使用机器人状态发布器

本节我们讲解在机器人中使用robot state publisher（机器人状态发布器）
当你的机器人有很多关节一起工作时，发布他们所有的TF就变得非常困难了，这时候机器人状态发布器就可以很好的帮你解决这个问题了
机器人状态发布器能将你的机器人状态发布到TF库中；因为机器人状态发布器中有机器人运动学模型，所以当我们知道机器人每个关节的给定位置后，机器人状态发布器就能够计算并发布机器人中各个关节的三维姿态了。

方法1

一般我们要运行robot_state_publisher时我们将其作为一个节点来运行，我们只需要将机器人的urdf文件加载到参数服务器上，将机器人的关节位置信息发布出来就可以了
例如：

方法2

我们也可以在C++代码中应用库函数来使用机器人状态发布器
首先我们要包含头文件：
#include <robot_state_publisher/robot_state_publisher.h>
然后我们需要一个接收KDL树的一个构造函数：
RobotStatePublisher(const KDL::Tree& tree);
最后我们每当想要发布机器人状态时就调用PublishTransform函数：

// publish moving joints
void publishTransforms(const std::map<std::string, double>& joint_positions,
                        const ros::Time& time);

// publish fixed joints
void publishFixedTransforms();

robot_state_publisher应用实例

本节我们将结合一个具体的urdf机器人描述文件，应用robot_state_publisher编写一个节点发布机器人的关节变化。
首先切换到工作空间的src文件夹下创建一个包：
catkin_create_pkg r2d2 roscpp rospy tf sensor_msgs std_msgs
然后在src下创建state_publisher.cpp，并输入以下代码：

#include <string>
#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#include <tf/transform_broadcaster.h>

int main(int argc, char** argv) 
{
	ros::init(argc, argv, "state_publisher");
	ros::NodeHandle n;
	ros::Publisher joint_pub = n.advertise<sensor_msgs::JointState>("joint_states", 1);
	tf::TransformBroadcaster broadcaster;
	ros::Rate loop_rate(30);

	const double degree = M_PI/180;

	// robot state
	double tilt = 0, tinc = degree, swivel=0, angle=0, height=0, hinc=0.005;

	// message declarations
	geometry_msgs::TransformStamped odom_trans;
	sensor_msgs::JointState joint_state;
	odom_trans.header.frame_id = "odom";
	odom_trans.child_frame_id = "axis";

	while (ros::ok()) {
		//update joint_state
		joint_state.header.stamp = ros::Time::now();
		joint_state.name.resize(3);
		joint_state.position.resize(3);
		joint_state.name[0] ="swivel";
		joint_state.position[0] = swivel;
		joint_state.name[1] ="tilt";
		joint_state.position[1] = tilt;
		joint_state.name[2] ="periscope";
		joint_state.position[2] = height;


		// update transform
		// (moving in a circle with radius=2)
		odom_trans.header.stamp = ros::Time::now();
		odom_trans.transform.translation.x = cos(angle)*2;
		odom_trans.transform.translation.y = sin(angle)*2;
		odom_trans.transform.translation.z = .7;
		odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(angle+M_PI/2);

		//send the joint state and transform
		joint_pub.publish(joint_state);
		broadcaster.sendTransform(odom_trans);

		// Create new robot state
		tilt += tinc;
		if (tilt<-.5 || tilt>0) tinc *= -1;
		height += hinc;
		if (height>.2 || height<0) hinc *= -1;
		swivel += degree;
		angle += degree/4;

		// This will adjust as needed per iteration
		loop_rate.sleep();
	}


	return 0;
}

在刚才创建的包中创建一个display.launch文件，代码如下：

<launch>
  <param name="robot_description" command="cat $(find r2d2)/urdf/model.xml" />
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
  <node name="state_publisher" pkg="r2d2" type="state_publisher" />
</launch>

在r2d2功能包下新建目录urdf，并新建model.xml文件，代码如下：

<robot name="r2d2">

<link name="axis">
  <inertial>
    <mass value="1"/>
    <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100" />
    <origin/>
  </inertial>
  <visual>
    <origin xyz="0 0 0" rpy="1.57 0 0" />
    <geometry>
      <cylinder radius="0.01" length=".5" />
    </geometry>
    <material name="gray">
      <color rgba=".2 .2 .2 1" />
    </material>
  </visual>

  <collision>
    <origin xyz="0 0 0" rpy="1.57 0 0" />
    <geometry>
      <cylinder radius="0.01" length=".5" />
    </geometry>
    <contact_coefficients mu="0" kp="1000.0" kd="1.0"/>
  </collision>
</link>

<link name="leg1">
  <inertial>
    <mass value="1"/>
    <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100" />
    <origin/>
  </inertial>

  <visual>
    <origin xyz="0 0 -.3" />
    <geometry>
      <box size=".20 .10 .8" />
    </geometry>
    <material name="white">
      <color rgba="1 1 1 1"/>
    </material>
  </visual>

  <collision>
    <origin xyz="0 0 -.3" />
    <geometry>
      <box size=".20 .10 .8" />
    </geometry>
    <contact_coefficients mu="0" kp="1000.0" kd="1.0"/>
  </collision>
</link>

<joint name="leg1connect" type="fixed">
  <origin xyz="0 .30 0" />
  <parent link="axis"/>
  <child link="leg1"/>
</joint>

<link name="leg2">
  <inertial>
    <mass value="1"/>
    <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100" />
    <origin/>
  </inertial>

  <visual>
    <origin xyz="0 0 -.3" />
    <geometry>
      <box size=".20 .10 .8" />
    </geometry>
    <material name="white">
      <color rgba="1 1 1 1"/>
    </material>
  </visual>

  <collision>
    <origin xyz="0 0 -.3" />
    <geometry>
      <box size=".20 .10 .8" />
    </geometry>
    <contact_coefficients mu="0" kp="1000.0" kd="1.0"/>
  </collision>
</link>

<joint name="leg2connect" type="fixed">
  <origin xyz="0 -.30 0" />
  <parent link="axis"/>
  <child link="leg2"/>
</joint>

<link name="body">
  <inertial>
    <mass value="1"/>
    <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100" />
    <origin/>
  </inertial>

  <visual>
    <origin xyz="0 0 -0.2" />
    <geometry>
      <cylinder radius=".20" length=".6"/>
    </geometry>
    <material name="white"/>
  </visual>

  <collision>
    <origin xyz="0 0 0.2" />
    <geometry>
      <cylinder radius=".20" length=".6"/>
    </geometry>
    <contact_coefficients mu="0" kp="1000.0" kd="1.0"/>
  </collision>
</link>

<joint name="tilt" type="revolute">
  <parent link="axis"/>
  <child link="body"/>
  <origin xyz="0 0 0" rpy="0 0 0" />
  <axis xyz="0 1 0" />
  <limit upper="0" lower="-.5" effort="10" velocity="10" />
</joint>

<link name="head">
  <inertial>
    <mass value="1"/>
    <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100" />
    <origin/>
  </inertial>

  <visual>
    <geometry>
      <sphere radius=".4" />
    </geometry>
    <material name="white" />
  </visual>

  <collision>
    <origin/>
    <geometry>
      <sphere radius=".4" />
    </geometry>
    <contact_coefficients mu="0" kp="1000.0" kd="1.0"/>
  </collision>
</link>

<joint name="swivel" type="continuous">
  <origin xyz="0 0 0.1" />
  <axis xyz="0 0 1" />
  <parent link="body"/>
  <child link="head"/>
</joint>

<link name="rod">
  <inertial>
    <mass value="1"/>
    <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100" />
    <origin/>
  </inertial>

  <visual>
    <origin xyz="0 0 -.1" />
    <geometry>
      <cylinder radius=".02" length=".2" />
    </geometry>
    <material name="gray" />

  </visual>

  <collision>
    <origin/>
    <geometry>
      <cylinder radius=".02" length=".2" />
    </geometry>
    <contact_coefficients mu="0" kp="1000.0" kd="1.0"/>
  </collision>
</link>

<joint name="periscope" type="prismatic">
  <origin xyz=".12 0 .15" />
  <axis xyz="0 0 1" />
  <limit upper="0" lower="-.5" effort="10" velocity="10" />
  <parent link="head"/>
  <child link="rod"/>
</joint>

<link name="box">
  <inertial>
    <mass value="1"/>
    <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100" />
    <origin/>
  </inertial>

  <visual>
    <geometry>
      <box size=".05 .05 .05" />
    </geometry>
    <material name="blue" >
      <color rgba="0 0 1 1" />
    </material>
  </visual>

  <collision>
    <origin/>
    <geometry>
      <box size=".05 .05 .05" />
    </geometry>
    <contact_coefficients mu="0" kp="1000.0" kd="1.0"/>
  </collision>
</link>
<joint name="boxconnect" type="fixed">
  <origin xyz="0 0 0" />
  <parent link="rod"/>
  <child link="box"/>
</joint>

</robot>

在CMakeList.txt文件的末尾添加：

include_directories(include ${catkin_INCLUDE_DIRS})
add_executable(state_publisher src/state_publisher.cpp)
target_link_libraries(state_publisher ${catkin_LIBRARIES})

然后切换到工作空间的第一级目录下运行catkin_make

如果成功，则如下图所示