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 turtleros::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 turtleros::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----小乌龟之你追我赶的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。