ROS----龜界三角戀

ROS中的tf利用一個節點發布兩個坐標之間的變換關系，然后更新tf樹，在監聽中，訂閱該tf樹，然后從tf樹中得到所需要的變換關系。
對于發布變換關系tf_broadcaster，ROS中對通用的publish進行了一定的封裝，但其本質仍舊是發布topic，對于訂閱tf_listener，對通用的訂閱進行一定的封裝。
今天做個有趣的實驗，利用三只烏龜，實現"龜界三角戀"。turtle1追turtle3，turtle2追turtle1，turtle3追turtle2。
先來理清思路:
在位置變換發布中:
1、訂閱某只龜的未知信息，就是/pose
2、利用/pose中的x，y，theta，計算得到該龜與world坐標系的平移和旋轉關系
3、利用tf包中的sendTransform()函數將變換發布。
4、為了節點的復用，將代碼中的龜名使用運行代碼的傳入參數。

在位置訂閱中:
1、定義服務客戶端，然后調用/spawn服務創建兩個烏龜
2、利用tf_listener監聽并查詢給定的兩個龜名的位置變換關系。
3、根據位置變換關系，計算對應的烏龜的線速度和角速度。
4、利用對應的publisher，將小烏龜的運動信息發送出去。

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; }

啟動文件:

<launch><node pkg="turtlesim" type="turtlesim_node" name="sim"/><node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>//節點復用，利用args，對每次運行的節點傳入不同的參數，節點發布不同的烏龜與world坐標的變換關系。<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>

總結

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

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