ROS学习（二十）--tf/写broadcaster和listener

创建learning_tf包

cd catkin_ws/src
catkin_create_pkg learning_tf tf roscpp rospy turtlesim
cd catkin_ws
catkin_make
source ./devel/setup.bash

发布坐标变换（transforms）

在learning_tf的src下创建turtle_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){
static tf::TransformBroadcaster br;
tf::Transform transform;
transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.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, "my_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;
};

代码解释

#incldue<tf/transform_broadcaster.h>

这个包提供了一个TransformBroadcaster的实现帮助发布坐标变换的工作更加就简单。

static tf::TransformBroadcaster br;

这里我们创建一个TransformBroadcaster对象，稍后用来发送变换信息

tf::Transform transform;
transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );
tf::Quaternion q;
q.setRPY(0, 0, msg->theta);
transform.setRotation(q);

Transform对象，将信息从2D的乌龟复制到3D的坐标系,设置旋转

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

使用TransformBroadcaster发送transform需要四个参数

1， transform本身 2，给transform一个时间戳，就是用当前时间3，父框架的名字4，子框架的名字

运行broadcaster

打开CMakeLists.txt,将下列两行添加到底部

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

然后cd到catkin_ws

catkin_make

正常情况下到devel/lib/learning_tf folder下会发现turtle_tf_broadcaster，然后我们新建文件start_demo.launch，复制粘贴

<launch>
<!-- Turtlesim Node-->
<node pkg="turtlesim" type="turtlesim_node" name="sim"/>

<node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>
<!-- Axes -->
<param name="scale_linear" value="2" type="double"/>
<param name="scale_angular" value="2" type="double"/>

<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>

保存运行

roslaunch learning_tf start_demo.launch

编写listener

learning_tf包下新建文件turtle_tf_listener.cpp,复制粘贴

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <turtlesim/Velocity.h>
#include <turtlesim/Spawn.h>

int main(int argc, char** argv){
ros::init(argc, argv, "my_tf_listener");

ros::NodeHandle node;

ros::service::waitForService("spawn");
ros::ServiceClient add_turtle =
node.serviceClient<turtlesim::Spawn>("spawn");
turtlesim::Spawn srv;
add_turtle.call(srv);

ros::Publisher turtle_vel =
node.advertise<turtlesim::Velocity>("turtle2/command_velocity", 10);

tf::TransformListener listener;

ros::Rate rate(10.0);
while (node.ok()){
tf::StampedTransform transform;
try{
listener.lookupTransform("/turtle2", "/turtle1",
ros::Time(0), transform);
}
catch (tf::TransformException ex){
ROS_ERROR("%s",ex.what());
ros::Duration(1.0).sleep();
}

turtlesim::Velocity vel_msg;
vel_msg.angular = 4.0 * atan2(transform.getOrigin().y(),
transform.getOrigin().x());
vel_msg.linear = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) +
pow(transform.getOrigin().y(), 2));
turtle_vel.publish(vel_msg);

rate.sleep();
}
return 0;
};

代码解释

try{
listener.lookupTransform("/turtle2", "/turtle1",
ros::Time(0), transform);
}

四个参数：1跟2是一起的，表示我们想要1坐标系到2坐标系的转变（We want the transform from this frame1 to this frame2)3，时间戳4，存transform的地方

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);

这里的transform根据turtle1离turtle2的距离跟角度被用来计算turtle2新的角速度跟线速度。新的速度发布在话题turtle2/cmd_vel。

打开CMakeList.txt，复制粘贴

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

打开终端，source一下先（如果遇到找不到文件这种问题的source）

catkin_make

成功了后在/devel/lib/learning_tf下会出现turtle_tf_listener,这样就可以运行了

roslaunch learning_tf start_demo.launch

可以看到多了一只乌龟跟随另一只乌龟移动，但是出现错误，说turtle2这个框架不存在。这是因为listener在计算坐标变换时turtle2的信息还没到。tf_frame需要花些时间生成和广播