TF2小乌龟实操

我们进行时间，生产两个小乌龟，操纵一个，并使另一个能跟踪前一个小乌龟运动。

我们先写下产生孪生乌龟的文档：

#include "ros/ros.h"
#include "turtlesim/Spawn.h"

int main(int argc, char*argv[]){
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"practice01_pub");
    ros::NodeHandle n;
    ros::ServiceClient client=n.serviceClient<turtlesim::Spawn>("/spawn");
    turtlesim::Spawn spawn;
    spawn.request.x=5.0;
    spawn.request.y=5.0;
    spawn.request.theta=1.57;
    spawn.request.name="turtle2";

    client.waitForExistence();
   bool flag=client.call(spawn);
    if(flag){
        ROS_INFO("New turtle created successfully, name: %s",spawn.request.name.c_str());
    }
    else{
        ROS_INFO("Failed to create");
    }

    return 0;
}

再写发布方文件：

#include "ros/ros.h"
#include "turtlesim/Pose.h"
#include "tf2_ros/transform_broadcaster.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"

std::string  turtlename;

void doPose(const turtlesim::Pose::ConstPtr& pose){
    static tf2_ros::TransformBroadcaster pub;
    geometry_msgs::TransformStamped ts;
    ts.header.frame_id="world";
    ts.header.stamp=ros::Time::now();
    ts.child_frame_id= turtlename;
    ts.transform.translation.x=pose->x;
    ts.transform.translation.y=pose->y;
    ts.transform.translation.z=0.0;

    tf2::Quaternion qtn;
    qtn.setRPY(0,0,pose->theta);
    ts.transform.rotation.x=qtn.getX();
    ts.transform.rotation.y=qtn.getY();
    ts.transform.rotation.z=qtn.getZ();
    ts.transform.rotation.w=qtn.getW();

    pub.sendTransform(ts);
}

int main(int  argc,char *argv[]){
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"practice01_dynamic");
    ros::NodeHandle n;

    if(argc!= 2){
        ROS_ERROR("Please input the right paraments!");
        return 1;
    }
    else{
            turtlename=argv[1];
    }

    ros::Subscriber sub=n.subscribe( turtlename + "/pose" , 100 , doPose );

    ros::spin();
    return 0;
}

再写订阅方文档：

#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "geometry_msgs/PointStamped.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"

int main(int argc,char* argv[]){
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"practice01_sub");
    ros::NodeHandle n;

    tf2_ros::Buffer buffer;
    tf2_ros::TransformListener listener(buffer);
    geometry_msgs::PointStamped ps;
    ps.header.frame_id="turtle1";
    ps.header.stamp=ros::Time(0.0);
    ps.point.x=2.0;
    ps.point.y=3.0;
    ps.point.z=5.0;

    //ros::Duration(1).sleep();
    ros::Rate rate(10);

    while(ros::ok()){
        geometry_msgs::PointStamped ps_out;
        
        try{
            ps_out=buffer.transform(ps,"world");
            ROS_INFO("transformed location:( %.2f , %.2f , %.2f ), the reference frame: %s",
            ps_out.point.x,
            ps_out.point.y,
            ps_out.point.z,
            ps_out.header.frame_id.c_str()
            );
        }
        catch(const std::exception& e)
        {
           ROS_INFO("exception message: %s",e.what());
        }

        rate.sleep();
        ros::spinOnce();
    }

    return 0;
}

最后写控制乌龟的文档：

#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "geometry_msgs/PointStamped.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#include "geometry_msgs/Twist.h"

int  main(int argc,char* argv[]){
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"practice01_control");
    ros::NodeHandle n;

    tf2_ros::Buffer buffer;
    tf2_ros::TransformListener sub(buffer);

    ros::Publisher pub=n.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel",100);

    ros::Rate rate(10);
    while (ros::ok())
    {
        try
        {
            geometry_msgs::TransformStamped son1Toson2 = buffer.lookupTransform("turtle2", "turtle1", ros::Time(0)); //son2:目标坐标系 son1:原坐标系 ros::Time(0):取间隔最短的两个坐标关系帧计算相对关系
            // ROS_INFO("former reference frame: %s, current reference frame: %s, transformation: %.2f, %.2f, %.2f",
            //          son1Toson2.child_frame_id.c_str(),//turtle2
            //          son1Toson2.header.frame_id.c_str(),//turtle1
            //          son1Toson2.transform.translation.x,
            //          son1Toson2.transform.translation.y,
            //          son1Toson2.transform.translation.z);
            
            geometry_msgs::Twist twist;            // x=k1*sqr(x^2+y^2) and z=k2*arctan(y/x)
            twist.linear.x = 0.75 * sqrt(pow(son1Toson2.transform.translation.x,2)+pow(son1Toson2.transform.translation.y,2));//only need to set x of linear velocity and z of angular velocity
            twist.angular.z=4*atan2(son1Toson2.transform.translation.y,son1Toson2.transform.translation.x);
            pub.publish(twist);
        }
        catch(const std::exception& e)
        {
            ROS_INFO("Error warning: %s",e.what());
        }

        rate.sleep();
        ros::spinOnce();
    }

    return 0;
}

然后我们写以下的launch文件：

<launch>
    <group>
        <node pkg="turtlesim" type="turtlesim_node" name="turtle1" output="screen" />
        <node pkg="turtlesim" type="turtle_teleop_key" name="key1" output="screen" />
        <node pkg="practice01_tf2" type="practice01_dynamic" name="pub1" args="/turtle1" output="screen" />
        <node pkg="practice01_tf2" type="practice01_sub" name="sub1" args="/turtle1" output="screen" />
        <node pkg="practice01_tf2" type="practice01_control" name="control" output="screen" />
    </group>

    <group>
        <node pkg="practice01_tf2" type="practice01_pub" name="turtle2" output="screen" />
        <node pkg="practice01_tf2" type="practice01_dynamic" name="pub2" args="/turtle2" output="screen" />
        <node pkg="practice01_tf2" type="practice01_sub" name="sub2" args="/turtle2" output="screen" />
    </group>
</launch>

然后我们直接发布launch文件即可。