ROS 乌龟TF坐标变换实现跟随

使用TF树实现坐标变换，以下是小海龟列子的广播与监听实现。

创建作TF广播：turtle_broadcaster.cpp

#include<ros/ros.h>
#include<tf/transform_broadcaster.h>
#include<turtlesim/Pose.h>
using namespace 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, "tf_broadcaster");
    if(argc !=2)
    {
        ROS_ERROR("need turtle name");
        return -1;
    }
    turtle_name=argv[1];

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

创建TF监听：turtle_listener.cpp

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

int main(int argc, char * argv[])
{
    ros::init(argc, argv, "tf_listener");
    ros::NodeHandle node;
    ros::service::waitForService("spawn");
    ros::ServiceClient add_turtle;
    add_turtle = node.serviceClient<turtlesim::Spawn>("spawn");
    turtlesim::Spawn srv;
    add_turtle.call(srv);
    ros::Publisher turtle_vel=node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel",10);

    tf::TransformListener listener;

    ros::Rate rate(10.0);
    while(node.ok())
    {
        tf::StampedTransform transform;
        try
        {
            listener.waitForTransform("/turtle2","/turtle1",ros::Time(0),ros::Duration(3.0));
            listener.lookupTransform("/turtle2","/turtle1",ros::Time(0),transform);
        }
        catch(tf::TransformException &ex)
        {
            ROS_ERROR("%s",ex.what());
            ros::Duration(1.0).sleep();
            continue;
        }
        
        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);
        rate.sleep();
    }

    return 0;
}

编译依赖：CMakeLists.txt

find_package(catkin REQUIRED COMPONENTS
  geometry_msgs
  roscpp
  rospy
  std_msgs
  tf
)

add_executable(turtle_broadcaster src/turtle_broadcaster.cpp)
add_executable(turtle_listener src/turtle_listener.cpp)

add_dependencies(turtle_broadcaster ${PROJECT_NAME}_gencpp)
add_dependencies(turtle_listener ${PROJECT_NAME}_gencpp)

target_link_libraries(turtle_broadcaster
${catkin_LIBRARIES}
 )
target_link_libraries(turtle_listener
${catkin_LIBRARIES}
 )

更新环境变量：

source devel/setup.bash

catkin_make编译源码

 运行代码：

roscore
rosrun ros_tf turtle_broadcaster turtle2
rosrun ros_tf turtle_listener 

launch 文件编写：start_demo_with_listen.launch

<launch>
    <!--海龟仿真器-->
    <node pkg="turtlesim" type="turtlesim_node" name="sim"/>
    <!--键盘控制-->
    <node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>
    <!--两只海龟的TF广播-->
    <node pkg="ros_tf" type="turtle_broadcaster" args="/turtle1" name="turtle1_broadcaster"/>
    <node pkg="ros_tf" type="turtle_broadcaster" args="/turtle2" name="turtle2_broadcaster"/>
    <!--监听TF广播，并且控制移动-->
    <node pkg="ros_tf" type="turtle_listener" name="listener"/>

</launch>

启动launch文件：

roslaunch ros_tf start_demo_with_listen.launch

 

 成功实现。

最后，写一个调试过程中产生的错误：

[ERROR] [1653806128.251266926]: "turtle2" passed to lookupTransform argument target_frame does not exist.

仔细检查后发现在请求产生第二个海龟时，服务名大小写写错了。所以大家要注意，另外，waitForTransform("/turtle2","/turtle1",ros::Time(0),ros::Duration(3.0))也必须有，不然也会有上述bug。