ROS学习历程（tf）--实现tf广播器

2024-03-29 09:13:40

本例采用turtle_tf例程

sudo apt-get install ros-kinetic-turtle-tf

首先创建一个功能包`
（该软件包依赖于tf，roscpp，rospy和turtlesim，笔者操作时未直接添加，后在package.xml文件中手动添加）

catkin_creat_pkg k_tf

回到工作空间catkin_make

鼠标创建创建src文件夹
鼠标创建 k_tf_broadcaster.cpp

完整代码：

#include<ros/ros.h>
#include<tf/transform_broadcaster.h>
#include<turtlesim/Pose.h>

std::string turtle_name;

//回调函数
void poseCallback(const turtlesim::PoseConstPtr& msg)
{
    //创建tf广播器
    //类似于创建一个发布者
    static tf::TransformBroadcaster br;

    //定义存放转换信息（平动、转动）的变量
    tf::Transform transform;

    //设置坐标原点
    //即子坐标系的坐标原点在父坐标系下的坐标
    transform.setOrigin(tf::Vector3(msg->x,msg->y,0.0));

    //定义旋转
    //roll绕x轴，pitch绕y轴，yaw绕z轴
    tf::Quaternion q;
    q.setRPY(0,0,msg->theta);

    //设置旋转，Set the rotational element by Quaternion.
    transform.setRotation(q);

    br.sendTransform(tf::StampedTransform(transform,ros::Time::now(),"world",turtle_name));
    //sendtransform函数发布Tf变换,需要四个参数：转换本身（存储变换关系的变量），广播时的时间戳，父坐标系，子坐标系
}



int main(int argc,char ** argv)
{
    //初始化节点
    ros::init(argc, argv, "k_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;
    };

详细注释

主函数

ros::init(argc, argv, "k_tf_broadcaster");

    if(argc != 2)
    {
        ROS_ERROR("need turtle name as argument");
        return -1;
    }
    turtle_name = argv[1];

初始化ros节点，节点名称为 “k_tf_broadcaster”

 ros::NodeHandle node;   
    ros::Subscriber sub = node.subscribe(turtle_name+"/pose",10,&poseCallback);
    ros::spin();
    return 0;

创建节点句柄，句柄管理节点资源，比如订阅、发送等。

订阅乌龟的pose信息，话题名称为乌龟名+“/pose” 此处乌龟名(turtle_name)是自定义的一个参数，后面会介绍从launch文件中传入参数。

队列长度10，回调函数poseCallback。

回调函数

static tf::TransformBroadcaster br;

创建tf广播器，类似于创建一个发布者

tf::Transform transform;

定义存放转换信息（平动、转动）的变量

transform.setOrigin(tf::Vector3(msg->x,msg->y,0.0));

设置坐标原点，即子坐标系的坐标原点在父坐标系下的坐标，此例程为二维空间，故z轴无变换

tf::Quaternion q;
    q.setRPY(0,0,msg->theta);

定义旋转，roll绕x轴，pitch绕y轴，yaw绕z轴，同样，x，y轴无变换

transform.setRotation(q)

设置旋转，Set the rotational element by Quaternion.

br.sendTransform(tf::StampedTransform(transform,ros::Time::now(),"world",turtle_name));

这是真正的工作完成的地方。
使用TransformBroadcaster发送转换需要四个参数。
首先，我们传递转换本身。
现在我们需要给被发布的变换一个时间戳，我们只是用当前时间戳它，ros::Time::now().
然后，我们需要传递我们创建的链接的父框架的名称，在这种情况下为“world”
最后，我们需要传递我们正在创建的链接的子框架的名称，在这种情况下，这是乌龟本身的名称。
注意：sendTransform和StampedTransform具有父对象和子对象的相反顺序。
StampedTransform（）是一个默认构造函数，构造出sendTransform（）对应的数据结构，后进行广播。

cmakelist修改内容

笔者创建功能包时未添加功能包依赖。

find_package(catkin REQUIRED COMPONENTS
  geometry_msgs
  roscpp
  tf
  turtlesim)

include_directories(
# include
 ${catkin_INCLUDE_DIRS}
)

添加

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

package.xml修改内容

添加

<buildtool_depend>catkin</buildtool_depend>

 <build_depend>geometry_msgs</build_depend>
  <build_depend>roscpp</build_depend>
  <build_depend>tf</build_depend>
  <build_depend>turtlesim</build_depend>
  <build_export_depend>geometry_msgs</build_export_depend>
  <build_export_depend>roscpp</build_export_depend>
  <build_export_depend>tf</build_export_depend>
  <build_export_depend>turtlesim</build_export_depend>
  <exec_depend>geometry_msgs</exec_depend>
  <exec_depend>roscpp</exec_depend>
  <exec_depend>tf</exec_depend>
  <exec_depend>turtlesim</exec_depend>

编写launch文件

创建launch文件夹
创建star_demo.launch文件

<launch>
<!--海龟仿真器-->
<node pkg="turtlesim" type="turtlesim_node" name="sim"/>

<!-- 键盘控制 -->
 <node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>


<!-- 两只海龟的tf广播 -->
 <node pkg="learning_tf" type="turtle_tf_broadcaster"
          args="/turtle1" name="turtle1_tf_broadcaster" />
<node pkg="learning_tf" type="turtle_tf_broadcaster"
          args="/turtle2" name="turtle2_tf_broadcaster" />

</launch>

在launch文件中，用args=""将乌龟名传给了main函数，所以在main函数中定义一个数组存放乌龟名，可在launch文件中传入不同参数，多次广播。

测试

roslaunch k_tf start_demo.launch

现在，使用tf_echo工具检查龟的姿势是否真正得到广播到tf：

 rosrun tf tf_echo /world /turtle1

注释

主函数中代码：

turtle_name = argv[1];

对argv[1]的补充：

main(int argc,char *argv[ ])

1.argc为整数

2.argv为指针的指针（可理解为：char **argv or: char *argv[] or: char argv[][] ，argv是一个指针数组）

注：main()括号内是固定的写法。

3.下面给出一个例子来理解这两个参数的用法：

假设程序的名称为prog，

当只输入prog，则由操作系统传来的参数为：

argc=1,表示只有一程序名称。

argc只有一个元素，argv[0]指向输入的程序路径及名称：./prog

当输入prog para_1，有一个参数，则由操作系统传来的参数为：

argc=2，表示除了程序名外还有一个参数。

argv[0]指向输入的程序路径及名称。

argv[1]指向参数para_1字符串。

当输入prog para_1 para_2 有2个参数，则由操作系统传来的参数为：

argc=3，表示除了程序名外还有2个参数。

argv[0]指向输入的程序路径及名称。

argv[1]指向参数para_1字符串。

argv[2]指向参数para_2字符串。

4.void main( int argc, char *argv[] )

char *argv[] : argv 是一个指针数组，他的元素个数是argc，存放的是指向每一个参数的指针

所以，argv[1]存放的才是turtle1的名字。

参考：
https://blog.csdn.net/Start_From_Scratch/article/details/50762293
https://www.cnblogs.com/yanglai/p/6927151.html

https://www.ncnynl.com/archives/201702/1310.html

码农公寓