ROS----龜界三角戀

ROS中的tf利用一個(gè)節(jié)點(diǎn)發(fā)布兩個(gè)坐標(biāo)之間的變換關(guān)系，然后更新tf樹(shù)，在監(jiān)聽(tīng)中，訂閱該tf樹(shù)，然后從tf樹(shù)中得到所需要的變換關(guān)系。
對(duì)于發(fā)布變換關(guān)系tf_broadcaster，ROS中對(duì)通用的publish進(jìn)行了一定的封裝，但其本質(zhì)仍舊是發(fā)布topic，對(duì)于訂閱tf_listener，對(duì)通用的訂閱進(jìn)行一定的封裝。
今天做個(gè)有趣的實(shí)驗(yàn)，利用三只烏龜，實(shí)現(xiàn)"龜界三角戀"。turtle1追turtle3，turtle2追turtle1，turtle3追turtle2。
先來(lái)理清思路:
在位置變換發(fā)布中:
1、訂閱某只龜?shù)奈粗畔?#xff0c;就是/pose
2、利用/pose中的x，y，theta，計(jì)算得到該龜與world坐標(biāo)系的平移和旋轉(zhuǎn)關(guān)系
3、利用tf包中的sendTransform()函數(shù)將變換發(fā)布。
4、為了節(jié)點(diǎn)的復(fù)用，將代碼中的龜名使用運(yùn)行代碼的傳入?yún)?shù)。

在位置訂閱中:
1、定義服務(wù)客戶(hù)端，然后調(diào)用/spawn服務(wù)創(chuàng)建兩個(gè)烏龜
2、利用tf_listener監(jiān)聽(tīng)并查詢(xún)給定的兩個(gè)龜名的位置變換關(guān)系。
3、根據(jù)位置變換關(guān)系，計(jì)算對(duì)應(yīng)的烏龜?shù)木€(xiàn)速度和角速度。
4、利用對(duì)應(yīng)的publisher，將小烏龜?shù)倪\(yùn)動(dòng)信息發(fā)送出去。

tf_broadcaster:

#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& msg) {static tf::TransformBroadcaster br;tf::Transform transform;transform.setOrigin(tf::Vector3(msg->x,msg->y,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,"my_tf_broadcaster");turtle_name = argv[1];ros::NodeHandle node;ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, poseCallback);ros::spin();return 0;}

tf_listener:

#include <ros/ros.h> #include <tf/transform_listener.h> #include <geometry_msgs/Twist.h> #include <turtlesim/Spawn.h>#include <ctime>int main(int argc,char** argv) {srand((unsigned int)time(NULL));ros::init(argc,argv,"turtle_tf_listener");ros::NodeHandle n;ros::service::waitForService("spawn");ros::ServiceClient add_turtle = n.serviceClient<turtlesim::Spawn>("spawn");turtlesim::Spawn srv;srv.request.x = rand()%10;srv.request.y = rand()%10;add_turtle.call(srv);turtlesim::Spawn srv1;srv1.request.x = rand()%10;srv1.request.y = rand()%10;add_turtle.call(srv1);ros::Publisher pub0 = n.advertise<geometry_msgs::Twist>("turtle1/cmd_vel", 10);ros::Publisher pub = n.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 10);ros::Publisher pub1 = n.advertise<geometry_msgs::Twist>("turtle3/cmd_vel", 10);tf::TransformListener listener;ros::Rate rate(10.0);while(n.ok()){tf::StampedTransform transform1,transform2,transform3;try{/* code for Try */listener.waitForTransform("/turtle2","/turtle1",ros::Time(0),ros::Duration(3.0));listener.lookupTransform("/turtle2","/turtle1",ros::Time(0),transform1);listener.waitForTransform("/turtle3","/turtle2",ros::Time(0),ros::Duration(3.0));listener.lookupTransform("/turtle3","/turtle2",ros::Time(0),transform2);listener.waitForTransform("/turtle1","/turtle3",ros::Time(0),ros::Duration(3.0));listener.lookupTransform("/turtle1","/turtle3",ros::Time(0),transform3);}catch (tf::TransformException &ex){/* code for Catch */ROS_ERROR("%s",ex.what());ros::Duration(1.0).sleep();continue;}geometry_msgs::Twist vel_msg;vel_msg.angular.z = 4.0*atan2(transform1.getOrigin().y(),transform1.getOrigin().x());vel_msg.linear.x = 1*sqrt(pow(transform1.getOrigin().x(),2)+pow(transform1.getOrigin().y(),2));pub.publish(vel_msg);vel_msg.angular.z = 4.0*atan2(transform2.getOrigin().y(),transform2.getOrigin().x());vel_msg.linear.x = 0.5*sqrt(pow(transform2.getOrigin().x(),2)+pow(transform2.getOrigin().y(),2));pub1.publish(vel_msg);vel_msg.angular.z = 4.0*atan2(transform3.getOrigin().y(),transform3.getOrigin().x());vel_msg.linear.x = 0.5*sqrt(pow(transform3.getOrigin().x(),2)+pow(transform3.getOrigin().y(),2));pub0.publish(vel_msg);rate.sleep(); }return 0; }

啟動(dòng)文件:

<launch><node pkg="turtlesim" type="turtlesim_node" name="sim"/><node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>//節(jié)點(diǎn)復(fù)用，利用args，對(duì)每次運(yùn)行的節(jié)點(diǎn)傳入不同的參數(shù)，節(jié)點(diǎn)發(fā)布不同的烏龜與world坐標(biāo)的變換關(guān)系。<node pkg="base" type="turtle_tf_broadcaster" args="/turtle1" name="turtle1_tf_broadcaster"/><node pkg="base" type="turtle_tf_broadcaster" args="/turtle2" name="turtle2_tf_broadcaster"/><node pkg="base" type="turtle_tf_broadcaster" args="/turtle3" name="turtle3_tf_broadcaster"/><node pkg="base" type="turtle_tf_listener" name="listener"/></launch>

總結(jié)

以上是生活随笔為你收集整理的ROS----龟界三角恋的全部?jī)?nèi)容，希望文章能夠幫你解決所遇到的問(wèn)題。

如果覺(jué)得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。