ROS理解:ros中的坐标以及对tf2进行解读

文章目录

  • 1. ROS中的坐标
  • 2. tf到tf2的变化
  • 3. 发布静态tf2
  • 4. 发布动态tf2
  • 5. 监听tf2
  • 6. 增加自己的frame

官网就是最好的教程,如果阅读英文没什么压力,强烈推荐以下链接进行全面了解:

官网tf2教程

1. ROS中的坐标

机器人当中有一套自己的坐标标准,它的直角坐标系表示如下:
ROS理解:ros中的坐标以及对tf2进行解读_第1张图片
相对于机器人来说:

X :朝前 ,红色
Y :朝左,绿色
Z :朝上,蓝色

旋转变换的坐标表示如下:
ROS理解:ros中的坐标以及对tf2进行解读_第2张图片

其中:Roll,Pitch,Yaw的右手法则,大拇指的方向为X,Y,Z的方向。

怎么理解发布的坐标呢?

建立父link到子link的tf变换,就是让父link向子link移动过程中(x, y, z, Roll,Pitch,Yaw)的变化。

2. tf到tf2的变化

tf已经被ROS弃用了,而且tf2包含了tf的所有功能,所以这里不介绍tf了,如果想要了解tf到tf2的变化,可以点击下列链接。

数据类型的变化:http://wiki.ros.org/tf2/Tutorials/Migration/DataConversions
监听模式的变化:http://wiki.ros.org/tf2/Tutorials/Migration/TransformListener
发布模式的变化:http://wiki.ros.org/tf2/Tutorials/Migration/TransformBroadcaster

3. 发布静态tf2

静态tf是不随时间变化的坐标变换,可以有三种发布方式,:launch方式、urdf发布方式和写代码方式,下面分别介绍。

  • 方式一:launch(或run)文件方式发布

这种方式最简单直接,直接发布从父link到子link的坐标变换,代码如下:

1) 欧拉角发布

  1. launch方法
<launch>
<node pkg="tf" type="static_transform_publisher" name="link1_link2_broadcaster" args="x y z yaw pitch roll link1 link2 100" />
</launch>
  1. run方法
# 欧拉角
rosrun tf2_ros static_transform_publisher x y z yaw pitch roll frame_id child_frame_id

2) 四元数发布

  1. launch方法
<launch>
<node pkg="tf" type="static_transform_publisher" name="link1_link2_broadcaster" args="x y z qx qy qz qw link1 link2 100" />
</launch>
  1. run方法
# 四元数
rosrun tf2_ros static_transform_publisher x y z qx qy qz qw frame_id child_frame_id
  • x y z 代表link2相对与link1的位置变换。

  • qx qy qz qw表示link2相对与link1的位姿变换的四元数。

  • 参数100表示每隔100毫秒发布一次。

  • 方式二:urdf方式发布
    待添加
  • 方式三:代码方式发布

创建ros包:

catkin_create_pkg learning_tf2 tf2 tf2_ros roscpp rospy turtlesim

代码以及注释如下:

#include 
#include 
#include 
#include 
#include 
#include 
std::string static_turtle_name;

int main(int argc, char **argv)
{
  ros::init(argc,argv, "my_static_tf2_broadcaster");
  if(argc != 8)//需要输入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)//1号参数不能和world相同,因为它是子link
  {
    ROS_ERROR("Your static turtle name cannot be 'world'");
    return -1;

  }
  static_turtle_name = argv[1];//赋值给子link,argv[0]为命令节点
  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;

  //将char*类型的值转换为double类型,从第2号开始输入值
  //前三位为位置变换
  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} )

运行:

rosrun learning_tf2 static_turtle_tf2_broadcaster mystaticturtle 0 0 1 0 0 0

以上两种方式都可以通过命令查看发布的静态tf2变换,静态tf2的话题为/tf_static,查看静态tf命令如下:

rostopic echo /tf_static

或者使用rqt查看tf树:
ROS理解:ros中的坐标以及对tf2进行解读_第3张图片


4. 发布动态tf2

因为ros的小乌龟可以使用键盘控制它的移动,所以我们尝试订阅一个小乌龟位姿,并发布它到world的tf。

直接上代码和注释:

#include 
#include 
#include 
#include 
#include 
#include 
using namespace std;

std::string turtle_name;
void poseCallback(const turtlesim::PoseConstPtr& msg){
  static tf2_ros::TransformBroadcaster br;
  geometry_msgs::TransformStamped transformStamped;
  
  transformStamped.header.stamp = ros::Time::now();
  transformStamped.header.frame_id = "world";
  transformStamped.child_frame_id = turtle_name;
  transformStamped.transform.translation.x = msg->x;
  transformStamped.transform.translation.y = msg->y;
  transformStamped.transform.translation.z = 0.0;
  tf2::Quaternion q;
  q.setRPY(0, 0, msg->theta);
  transformStamped.transform.rotation.x = q.x();
  transformStamped.transform.rotation.y = q.y();
  transformStamped.transform.rotation.z = q.z();
  transformStamped.transform.rotation.w = q.w();

  br.sendTransform(transformStamped);//发布tf
}
int main(int argc, char** argv){
  ros::init(argc, argv, "my_tf2_broadcaster");
  ros::NodeHandle private_node("~");
  if (! private_node.hasParam("turtle"))
  {
    if (argc != 2){ROS_ERROR("need turtle name as argument"); return -1;};
    turtle_name = argv[1];
    cout<<turtle_name<<endl;
  }
  else
  {
    private_node.getParam("turtle", turtle_name);
  }
  ros::NodeHandle node;
  //订阅乌龟的位姿,系统发布出来的是小乌龟话题为:/turtle1/pose,键盘:/turtle1/cmd_vel
  ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);
  ros::spin();
  return 0;
};

注意: 模拟的小乌龟位姿为:/turtle1/pose,模拟键盘为:/turtle1/cmd_vel。所以这里只订阅了一个小乌龟。

添加CMakeList.txt文件

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

在包的src同一目录添加launch文件,并在它的文件内添加一下内容:

  <launch>
     <!-- 模拟乌龟接口-->
    <node pkg="turtlesim" type="turtlesim_node" name="sim"/>
	<!-- 模拟键盘控制-->
    <node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>
    <!-- 键盘控制的线速度与角速度变化尺度 -->
    <param name="scale_linear" value="2" type="double"/>
    <param name="scale_angular" value="2" type="double"/>
    
	<!-- 启动两个节点,它们的可执行文件同名-->
    <node pkg="learning_tf2" type="turtle_tf2_broadcaster"
          args="/turtle1" name="turtle1_tf2_broadcaster" />
    <node pkg="learning_tf2" type="turtle_tf2_broadcaster"
          args="/turtle2" name="turtle2_tf2_broadcaster" />
  </launch>

注意:

  1. 虽然是两个节点,但是生成的可执行文件相同。
  2. args为传入的不同话题参数,但是只有turtle1/pose会被订阅。

加入launch文件配置,这样就可以直接运行相应包下的launch文件了。

例如:roslaunch learning_tf2 start_demo.launch

install(FILES
 start_demo.launch
 DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)

查看tf变换:

rosrun tf tf_echo /world /turtle1

这里同样可以采用rqt下的tf树进行查阅。


5. 监听tf2

上面我们开启了两个节点进行订阅,其中一个订阅了小乌龟,另一个也有订阅接口但是没有发布者。所以我们尝试添加一只小乌龟和一个控制指令,定义他们的话题名为turtle2/pose和turtle/cmd_vel。这样的话就建立了从turtle2到world的tf。我们尝试让turtle2随着turtle1移动。

因为turtle1和turtle2都建立了到world的tf,所以我们可以监听turtle1到turtle2的tf来实现我们的需求。

直接上代码和注释:

#include 
#include 
#include 
#include 
#include 

int main(int argc, char** argv){
  ros::init(argc, argv, "my_tf2_listener");
  ros::NodeHandle node;
  ros::service::waitForService("spawn");
  ros::ServiceClient spawner =
    node.serviceClient<turtlesim::Spawn>("spawn");
  turtlesim::Spawn turtle;
  turtle.request.x = 4;
  turtle.request.y = 2;
  turtle.request.theta = 0;
  turtle.request.name = "turtle2";
  spawner.call(turtle);//使用服务器来开启另一只小乌龟。

  ros::Publisher turtle_vel =
    node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 10);//小乌龟的控制指令

  tf2_ros::Buffer tfBuffer;
  tf2_ros::TransformListener tfListener(tfBuffer);

  ros::Rate rate(10.0);
  while (node.ok()){
    geometry_msgs::TransformStamped transformStamped;
    try{
      transformStamped = tfBuffer.lookupTransform("turtle2", "turtle1",
                               ros::Time(0));//监听turtle2到turtle1到变换,不能监听ros::Time::now()
    }
    catch (tf2::TransformException &ex) {
      ROS_WARN("%s",ex.what());
      ros::Duration(1.0).sleep();
      continue;
    }
    geometry_msgs::Twist vel_msg;
    //根据两只小乌龟的相对位姿来定义控制命令
    vel_msg.angular.z = 4.0 * atan2(transformStamped.transform.translation.y,
                                    transformStamped.transform.translation.x);
    vel_msg.linear.x = 0.5 * sqrt(pow(transformStamped.transform.translation.x, 2) +
                                  pow(transformStamped.transform.translation.y, 2));
    turtle_vel.publish(vel_msg);
    rate.sleep();
  }
  return 0;
};

上述代码中请注意:

  1. 我们只能订阅最新的tf变换,而不能订阅当下的tf变换,也就是说只能监听ros::Time(0)时刻最新的,而不能监听ros::Time::now(),因为建立tf是需要时间的。
  2. 我们还可以设置等待项目(等待这一时刻tf建立),它是一个可选项,如果设置了等待项目,则可以用now();如:
    transformStamped = tfBuffer.lookupTransform(“turtle2”, “turtle1”, ros::Time::now(),
    ros::Duration(3.0));

在上一个start_launch.launch中添加:

  <launch>
    ...
    <node pkg="learning_tf2" type="turtle_tf2_listener"
          name="listener" />
  </launch>

运行:

roslaunch learning_tf2 start_demo.launch

刚启动launch的时候会出现以下界面,这是因为刚开始没有turtle2的变换,需要等到服务建立起来后,才能监听trutle2的变换。

在这里插入图片描述


6. 增加自己的frame

其实上文懂了的话,就没必要看这一节了,因为上面已经创建过turtle1和turtle2的frame了。

代码与注释:

#include 
#include 
#include 

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

   tf2_ros::TransformBroadcaster tfb;
  geometry_msgs::TransformStamped transformStamped;
  
  transformStamped.header.frame_id = "turtle1";//建立自己的frame与已知frame之间的tf
  transformStamped.child_frame_id = "carrot1";
  transformStamped.transform.translation.x = 0.0;
  transformStamped.transform.translation.y = 2.0;
  transformStamped.transform.translation.z = 0.0;
  tf2::Quaternion q;
        q.setRPY(0, 0, 0);
  transformStamped.transform.rotation.x = q.x();
  transformStamped.transform.rotation.y = q.y();
  transformStamped.transform.rotation.z = q.z();
  transformStamped.transform.rotation.w = q.w();

  ros::Rate rate(10.0);
  while (node.ok()){
    transformStamped.header.stamp = ros::Time::now();
    tfb.sendTransform(transformStamped);//发布tf
    rate.sleep();
    printf("sending\n");
  }
};

添加CMakeList.txt文件:

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

运行:

roslaunch learning_tf2 start_demo.launch

使用rqt查看建立起来的tf树:
ROS理解:ros中的坐标以及对tf2进行解读_第4张图片
上面建立的是静态tf,我们还可以修改代码建立动态tf:

transformStamped.transform.translation.x = 2.0*sin(ros::Time::now().toSec());
transformStamped.transform.translation.y = 2.0*cos(ros::Time::now().toSec());

你可能感兴趣的:(ROS,linux,ROS,TF与TF2)