ROS----小乌龟之你追我赶

生成八个小乌龟，分别命名为

/turtle1、/turtle2、/turtle3、/turtle4、/turtle5、/turtle6、/turtle7、/turtle8

然后实现turtle1自己运动，然后turtle2追turtle1，记为

turtle2->turtle1

。最终实现

turtle8->turtle7->turtle6->turtle5->turtle4->turtle3->turtle2->turtle1

这其中涉及到tf变换相关的内容:

在该项目中有三个文件

pursue_turtle_control.cpp、pursue_turtle.cpp、pursue_turtle.launch

其中，pursue_turtle.cpp用来发布某一个小乌龟和世界坐标的tf变换，根据传入的参数，确定到底是哪一个小乌龟。

pursue_turtle_control.cpp中产生所需要的小乌龟，同时查询tf树，确定相邻两个乌龟之间的相对方向，并发布对应乌龟的运动topic。

pursue_turtle.launch是launch文件主要用来配置一些参数和同时启动多个节点。

在pursue_turtle.cpp中，首先订阅传入名字小乌龟的/pose，得到对应小乌龟的位置(x，y)和角度theta。然后在对应的消息订阅回调函数中得到的信息进行相应的计算，得到平移和旋转向量，最后打包成固定的格式发布出去。

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

std::string turtle_name;
void poseCallback(const turtlesim::PoseConstPtr& pose)
{
    static tf::TransformBroadcaster br;
    tf::Transform transform;
    transform.setOrigin(tf::Vector3(pose->x,pose->y,0));
    tf::Quaternion q;
    q.setRPY(0,0,pose->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_life");
    turtle_name = argv[1];
    ros::NodeHandle n;
    ros::Subscriber sub = n.subscribe(turtle_name+"/pose", 10, poseCallback);
    ros::spin();
    return 0;
}

在pursue_turtle_control.cpp中，首先是生成所需要的乌龟，乌龟的位置随机产生。

ros::service::waitForService("spawn");

    // generation turtle
    ros::ServiceClient client = n.serviceClient<turtlesim::Spawn>("spawn");
    ros::ServiceClient cl = n.serviceClient<std_srvs::Empty>("clear");
    std_srvs::Empty e;
   
    turtlesim::Spawn turtle;
    srand((unsigned int)time(NULL));
    for(int i = 2;i < 9;i++)
    {
        turtle.request.x = rand()%8+1;
        turtle.request.y = rand()%8+1;
        client.call(turtle);
    }

接下来在tf树中查找相应的小乌龟之间的变换关系:

tf::TransformListener listener;
   
    while(n.ok())
    {
         cl.call(e);
         tf::StampedTransform transform2,transform3,transform4,transform5,transform6,transform7,transform8;
        try{
            listener.waitForTransform("/turtle2","/turtle1",ros::Time(0),ros::Duration(3.0));
            listener.lookupTransform("/turtle2","/turtle1",ros::Time(0),transform2);

             listener.waitForTransform("/turtle3","/turtle2",ros::Time(0),ros::Duration(3.0));
            listener.lookupTransform("/turtle3","/turtle2",ros::Time(0),transform3);

             listener.waitForTransform("/turtle4","/turtle3",ros::Time(0),ros::Duration(3.0));
            listener.lookupTransform("/turtle4","/turtle3",ros::Time(0),transform4);

             listener.waitForTransform("/turtle5","/turtle4",ros::Time(0),ros::Duration(3.0));
            listener.lookupTransform("/turtle5","/turtle4",ros::Time(0),transform5);

             listener.waitForTransform("/turtle6","/turtle5",ros::Time(0),ros::Duration(3.0));
            listener.lookupTransform("/turtle6","/turtle5",ros::Time(0),transform6);

             listener.waitForTransform("/turtle7","/turtle6",ros::Time(0),ros::Duration(3.0));
            listener.lookupTransform("/turtle7","/turtle6",ros::Time(0),transform7);

             listener.waitForTransform("/turtle8","/turtle1",ros::Time(0),ros::Duration(3.0));
            listener.lookupTransform("/turtle8","/turtle1",ros::Time(0),transform8);
        }catch(tf::TransformException& ex)
        {
            ROS_ERROR("%s",ex.what());
            ros::Duration(1.0).sleep();
            continue;
        }

最后将查询到的变换关系转换成相应的运动:

geometry_msgs::Twist t;
        t.angular.z = 1.0*atan2(transform2.getOrigin().y(),transform2.getOrigin().x());;
        t.linear.x =0.5*sqrt(pow(transform2.getOrigin().x(),2)+pow(transform2.getOrigin().y(),2));;
        pub2.publish(t);

         t.angular.z = 1.0*atan2(transform3.getOrigin().y(),transform3.getOrigin().x());;
        t.linear.x = 0.5*sqrt(pow(transform3.getOrigin().x(),2)+pow(transform3.getOrigin().y(),2));;
        pub3.publish(t);

         t.angular.z = 1.0*atan2(transform4.getOrigin().y(),transform4.getOrigin().x());;
        t.linear.x = 0.5*sqrt(pow(transform4.getOrigin().x(),2)+pow(transform4.getOrigin().y(),2));;
        pub4.publish(t);

         t.angular.z = 1.0*atan2(transform5.getOrigin().y(),transform5.getOrigin().x());;
        t.linear.x = 0.5*sqrt(pow(transform5.getOrigin().x(),2)+pow(transform5.getOrigin().y(),2));;
        pub5.publish(t);

         t.angular.z = 1.0*atan2(transform6.getOrigin().y(),transform6.getOrigin().x());;
        t.linear.x = 0.5*sqrt(pow(transform6.getOrigin().x(),2)+pow(transform6.getOrigin().y(),2));;
        pub6.publish(t);

         t.angular.z = 1.0*atan2(transform7.getOrigin().y(),transform7.getOrigin().x());;
        t.linear.x = 0.5*sqrt(pow(transform7.getOrigin().x(),2)+pow(transform7.getOrigin().y(),2));;
        pub7.publish(t);

         t.angular.z = 1.0*atan2(transform8.getOrigin().y(),transform8.getOrigin().x());;
        t.linear.x = 0.5*sqrt(pow(transform8.getOrigin().x(),2)+pow(transform8.getOrigin().y(),2));;
        pub8.publish(t);

完整的代码:

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

#include <std_srvs/Empty.h>

#include <ctime>

int main(int argc,char** argv)
{
    ros::init(argc,argv,"control");
    ros::NodeHandle n;

    ros::service::waitForService("spawn");

    // generation turtle
    ros::ServiceClient client = n.serviceClient<turtlesim::Spawn>("spawn");
    ros::ServiceClient cl = n.serviceClient<std_srvs::Empty>("clear");
    std_srvs::Empty e;
   
    turtlesim::Spawn turtle;
    srand((unsigned int)time(NULL));
    for(int i = 2;i < 9;i++)
    {
        turtle.request.x = rand()%8+1;
        turtle.request.y = rand()%8+1;
        client.call(turtle);
    }

    ros::Publisher pub1 = n.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 10);
    ros::Publisher pub2 = n.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel", 10);
    ros::Publisher pub3 = n.advertise<geometry_msgs::Twist>("/turtle3/cmd_vel", 10);
    ros::Publisher pub4 = n.advertise<geometry_msgs::Twist>("/turtle4/cmd_vel", 10);
    ros::Publisher pub5 = n.advertise<geometry_msgs::Twist>("/turtle5/cmd_vel", 10);
    ros::Publisher pub6 = n.advertise<geometry_msgs::Twist>("/turtle6/cmd_vel", 10);
    ros::Publisher pub7 = n.advertise<geometry_msgs::Twist>("/turtle7/cmd_vel", 10);
    ros::Publisher pub8 = n.advertise<geometry_msgs::Twist>("/turtle8/cmd_vel", 10);
   

    tf::TransformListener listener;
   
    while(n.ok())
    {
         cl.call(e);
         tf::StampedTransform transform2,transform3,transform4,transform5,transform6,transform7,transform8;
        try{
            listener.waitForTransform("/turtle2","/turtle1",ros::Time(0),ros::Duration(3.0));
            listener.lookupTransform("/turtle2","/turtle1",ros::Time(0),transform2);

             listener.waitForTransform("/turtle3","/turtle2",ros::Time(0),ros::Duration(3.0));
            listener.lookupTransform("/turtle3","/turtle2",ros::Time(0),transform3);

             listener.waitForTransform("/turtle4","/turtle3",ros::Time(0),ros::Duration(3.0));
            listener.lookupTransform("/turtle4","/turtle3",ros::Time(0),transform4);

             listener.waitForTransform("/turtle5","/turtle4",ros::Time(0),ros::Duration(3.0));
            listener.lookupTransform("/turtle5","/turtle4",ros::Time(0),transform5);

             listener.waitForTransform("/turtle6","/turtle5",ros::Time(0),ros::Duration(3.0));
            listener.lookupTransform("/turtle6","/turtle5",ros::Time(0),transform6);

             listener.waitForTransform("/turtle7","/turtle6",ros::Time(0),ros::Duration(3.0));
            listener.lookupTransform("/turtle7","/turtle6",ros::Time(0),transform7);

             listener.waitForTransform("/turtle8","/turtle1",ros::Time(0),ros::Duration(3.0));
            listener.lookupTransform("/turtle8","/turtle1",ros::Time(0),transform8);
        }catch(tf::TransformException& ex)
        {
            ROS_ERROR("%s",ex.what());
            ros::Duration(1.0).sleep();
            continue;
        }

        geometry_msgs::Twist t;
        t.angular.z = 1.0*atan2(transform2.getOrigin().y(),transform2.getOrigin().x());;
        t.linear.x =0.5*sqrt(pow(transform2.getOrigin().x(),2)+pow(transform2.getOrigin().y(),2));;
        pub2.publish(t);

         t.angular.z = 1.0*atan2(transform3.getOrigin().y(),transform3.getOrigin().x());;
        t.linear.x = 0.5*sqrt(pow(transform3.getOrigin().x(),2)+pow(transform3.getOrigin().y(),2));;
        pub3.publish(t);

         t.angular.z = 1.0*atan2(transform4.getOrigin().y(),transform4.getOrigin().x());;
        t.linear.x = 0.5*sqrt(pow(transform4.getOrigin().x(),2)+pow(transform4.getOrigin().y(),2));;
        pub4.publish(t);

         t.angular.z = 1.0*atan2(transform5.getOrigin().y(),transform5.getOrigin().x());;
        t.linear.x = 0.5*sqrt(pow(transform5.getOrigin().x(),2)+pow(transform5.getOrigin().y(),2));;
        pub5.publish(t);

         t.angular.z = 1.0*atan2(transform6.getOrigin().y(),transform6.getOrigin().x());;
        t.linear.x = 0.5*sqrt(pow(transform6.getOrigin().x(),2)+pow(transform6.getOrigin().y(),2));;
        pub6.publish(t);

         t.angular.z = 1.0*atan2(transform7.getOrigin().y(),transform7.getOrigin().x());;
        t.linear.x = 0.5*sqrt(pow(transform7.getOrigin().x(),2)+pow(transform7.getOrigin().y(),2));;
        pub7.publish(t);

         t.angular.z = 1.0*atan2(transform8.getOrigin().y(),transform8.getOrigin().x());;
        t.linear.x = 0.5*sqrt(pow(transform8.getOrigin().x(),2)+pow(transform8.getOrigin().y(),2));;
        pub8.publish(t);
        
        
        t.angular.z = rand()%3;
        t.linear.x = rand()%7;
        pub1.publish(t);

        ros::spinOnce();
    }
    return 0;
}

最后看一下pursue_turtle.launch文件:

<launch>
	//用来启动显示小乌龟的界面
    <node pkg="turtlesim" type="turtlesim_node" name="star"/>

	//启动pursue_turtle_control节点，产生小乌龟
    <node pkg="base" type="pursue_turtle_control" name="road"/>
	
	//启动八个节点，发布每个小乌龟和世界坐标之间的变换关系。
    <node pkg="base" type="pursue_turtle" args="/turtle1" name="turtle1_world_broadcaster"/>
    <node pkg="base" type="pursue_turtle" args="/turtle2" name="turtle2_world_broadcaster"/>
    <node pkg="base" type="pursue_turtle" args="turtle3" name="turtle3_world_broadcaster"/>
    <node pkg="base" type="pursue_turtle" args="turtle4" name="turtle4_world_broadcaster"/>
    <node pkg="base" type="pursue_turtle" args="turtle5" name="turtle5_world_broadcaster"/>
    <node pkg="base" type="pursue_turtle" args="turtle6" name="turtle6_world_broadcaster"/>
    <node pkg="base" type="pursue_turtle" args="turtle7" name="turtle7_world_broadcaster"/>
    <node pkg="base" type="pursue_turtle" args="turtle8" name="turtle8_world_broadcaster"/>

</launch>

ROS----小乌龟之你追我赶ROS----小乌龟之你追我赶

ROS----小乌龟之你追我赶

继续阅读

ROS----rospy编写turtlesim程序

ROS基础四之roscpp/rospy节点编写

ROS学习（十四）：ROS Spinning

ROS命名空间

关于ros的参数服务器ros param和命名空间

ROS中工作空间的建立

【ROS】移动机器人导航仿真（3）——定位（amcl）和路径规划（move_base）

ROS Navigation-----amcl简介

C++实现RVIZ 2D POSE ESTIMATE 功能设置机器人初始坐标1. 首先查看设置初始坐标的话题为 /INTIALPOSE ，查看消息类型和格式从而决定怎么给它发数据2.监听RVIZ发出的数据格式3.仿照RVIZ消息格式4.编写发布节点程序

ROS2(二) - nav2_amcl节点概况

amcl ROS1.算法2.举例3.节点3.1.6Transforms

ROS Navigation之amcl源码解析（完全详解）0. 写在最前面1. amcl是干什么的2. 总体情况3. amcl_node.cpp参考

or frame [laser]: No transform to fixed frame [map]

ros安装-Ubuntu14.04

Python：利用turtle库绘画的眨眼睛的皮卡丘Pikaqiu 用turtle绘画的皮卡丘展示

ROS Stage学习