这里写目录标题
本节的目的是,实现上一节的底层代码, 即通过程序代码实现小海龟turtle2跟随turtle1。
一、创建功能包
$ cd ~/catkin_ws/src
$ catkin_create_pkg learning_tf roscpp rospy tf turtlesim
二、创建tf代码
把古月的turtle_tf_broadcaster.cpp文件和turtle_tf_listener.cpp文件放到catkin_ws/src/learning_tf/src文件夹下。(python版本见2.2节)
2.1 cpp
2.1.1 创建tf广播器代码
turtle_tf_broadcaster.cpp文件
/**
* 该例程产生tf数据,并计算、发布turtle2的速度指令
*/
#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数据
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);
// 广播world与海龟坐标系之间的tf数据
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
}
int main(int argc, char** argv)
{
// 初始化ROS节点
ros::init(argc, argv, "my_tf_broadcaster");
// 输入参数作为海龟的名字
if (argc != 2)
{
ROS_ERROR("need turtle name as argument");
return -1;
}
turtle_name = argv[1];
// 订阅海龟的位姿话题