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