ROS TF2静态坐标发布

我们经常需要进行坐标的变换，往往很多时候需要发布一个固定的坐标系转换例如从ENU转换到NED。对于这种转换是固定不变的，能减少资源的占用就最好，TF提供了静态坐标转换的方式，但是这种方式相当于静态发布一个转换，不是很好，下面介绍一下TF2经过优化过后的TF转换。

安装

防止后面依赖没有，把tf2的所有包都装一下，文件也大。

sudo apt-get install ros-$ROS_DISTRO-tf2-*

CPP方式

对于根据输入决定的固定坐标方式，例如本地坐标系和大地坐标系转换的时候，我们采用这种方式。
首先新建一个工作空间

mkdir -p  ~/tf_ws/src
cd ~/tf_ws/src

然后新建一个包

catkin_create_pkg learning_tf2 tf2 tf2_ros roscpp rospy turtlesim

然后在包下的src文件夹建立一个static_turtle_tf2_broadcaster.cpp文件，粘贴下面的内容后保存退出。

#include <ros/ros.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <geometry_msgs/TransformStamped.h>
#include <cstdio>
#include <tf2/LinearMath/Quaternion.h>


std::string static_turtle_name;

int main(int argc, char **argv)
{
  ros::init(argc,argv, "my_static_tf2_broadcaster");
  if(argc != 8)
  {
    ROS_ERROR("Invalid number of parameters\nusage: static_turtle_tf2_broadcaster child_frame_name x y z roll pitch yaw");
    return -1;
  }
  if(strcmp(argv[1],"world")==0)
  {
    ROS_ERROR("Your static turtle name cannot be 'world'");
    return -1;

  }
  static_turtle_name = argv[1];
  static tf2_ros::StaticTransformBroadcaster static_broadcaster;
  geometry_msgs::TransformStamped static_transformStamped;

  static_transformStamped.header.stamp = ros::Time::now();
  static_transformStamped.header.frame_id = "world";
  static_transformStamped.child_frame_id = static_turtle_name;
  static_transformStamped.transform.translation.x = atof(argv[2]);
  static_transformStamped.transform.translation.y = atof(argv[3]);
  static_transformStamped.transform.translation.z = atof(argv[4]);
  tf2::Quaternion quat;
  quat.setRPY(atof(argv[5]), atof(argv[6]), atof(argv[7]));
  static_transformStamped.transform.rotation.x = quat.x();
  static_transformStamped.transform.rotation.y = quat.y();
  static_transformStamped.transform.rotation.z = quat.z();
  static_transformStamped.transform.rotation.w = quat.w();
  static_broadcaster.sendTransform(static_transformStamped);
  ROS_INFO("Spinning until killed publishing %s to world", static_turtle_name.c_str());
  ros::spin();
  return 0;
};

然后打开CMakeLists.txt添加下面的内容后保存后退出。

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

然后编译

catkin_make

编译完了运行就OK。
先开一个终端

roscore

另开一个终端运行节点。

source ~/tf_ws/devel/setup.bash
rosrun learning_tf2 static_turtle_tf2_broadcaster mystaticturtle 0 0 1 0 0 0

我们新开一个终端查看静态发布情况。

扫描二维码关注公众号，回复： 9282364 查看本文章

rostopic echo /tf_static

我们再开一个终端可视化查看tf树。

rosrun rqt_tf_tree rqt_tf_tree

launch文件方式

有时候后静态转换是固定不变的，我们可以在程序开始运行的时候发布这个静态的转换，例如从标准的相机系到机体系的转换。
我们根本就没有必要再写上面这样一段程序，ros为我们提供了现成的包。可以在命令行或者launch文件中实现。终端方式后面讲。
格式如下

static_transform_publisher x y z yaw pitch roll frame_id child_frame_id

还可以通过四元数的方式来实现。

static_transform_publisher x y z qx qy qz qw frame_id child_frame_id

在相应的launch文件夹内新建一个名为static_transforms.launch的launch文件，然后写如下面的内容保存退出。

<launch>
    <node pkg="tf2_ros" type="static_transform_publisher" name="link1_broadcaster" 
    args="1 0 0 0 0 0 1 link1_parent link1" />
</launch>

利用终端的方式实现

因为launch文件不会管各个节点的启动顺序，所以我们可以利用终端的方式来运行。
首先打开一个终端运行roscore

roscore

然后再开一个终端运行静态的坐标转换，格式和launch文件一样。

rosrun tf2_ros static_transform_publisher 0 0 1 0 0 0 link1_parent link1

想要同时运行多个命令相互之间是不会影响的。