古月居 ROS入门21讲 第十八讲 TF坐标系广播与监听的编程实现

古月居 ROS入门21讲 第十八讲 TF坐标系广播与监听的编程实现

创建功能包

cd ~/catkin_ws/src
catkin_create_pkg learning_tf roscpp rospy tf turtlesim

创建TF广播器代码

• C++
  turtle_tf_broadcaster.cpp

#include
#include
#include

std::string turtle_name;

void poseCallback(const turtlesim::PoseConstPtr& msg)
{
     
    ROS_INFO("pose:%0.2f,%0.2f,%0.2f",msg->x,msg->y,msg->theta);
    static tf::TransformBroadcaster br;
    
    tf::Transform transform;
    transform.setOrigin(tf::Vector3(msg->x,msg->y,0.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");
    if(argc!=2)
    {
     
        ROS_ERROR("need turtle name as argument!");
	return -1;
    }
    turtle_name=argv[1];
    ros::NodeHandle node;
    ros::Subscriber sub = node.subscribe(turtle_name+"/pose",10,poseCallback);
    ros::spin();
    return 0;
}

turtle_tf_listener.cpp

#include
#include
#include
#include

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

    ros::service::waitForService("/spawn");
    ros::ServiceClient add_turtle=node.serviceClient<turtlesim::Spawn>("/spawn");
    turtlesim::Spawn srv;
    add_turtle.call(srv);

    ros::Publisher turtle_vel=node.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel",10);
    tf::TransformListener listener;
    ros::Rate rate(10);
    while(node.ok())
    {
     
        tf::StampedTransform transform;
	try
	{
     
	    //等待变换，也就是等待二者的上线，并且可以变换（在一棵TF树上），超过三秒就报错
	    listener.waitForTransform("/turtle2","/turtle1",ros::Time(0),ros::Duration(3.0));
	    listener.lookupTransform("/turtle2","turtle1",ros::Time(0),transform);
	}
	catch(tf::TransformException &ex)
	{
     
	    ROS_ERROR("%s",ex.what());
	    ros::Duration(1.0).sleep();
	    continue;
	}
	geometry_msgs::Twist vel_msg;
	//通过计算turtle1相对于turtle2的位置的正切值，来计算turtle2需要旋转的角度，使得turtle1位于turtle2单位正前方。4作为一个系数
	vel_msg.angular.z=4.0*atan2(transform.getOrigin().y(),transform.getOrigin().x());
	//x方向（即正前方）的速度与两只海龟之间的距离有关，距离越大速度越大。
	vel_msg.linear.x=0.5*sqrt(pow(transform.getOrigin().x(),2)+pow(transform.getOrigin().y(),2));
	turtle_vel.publish(vel_msg);
	rate.sleep();
    }
    return 0;
}

在CMakelists.txt中添加

add_executable(turtle_tf_broadcaster src/turtle_tf_broadcaster.cpp)
target_link_libraries(turtle_tf_broadcaster ${catkin_LIBRARIES})

add_executable(turtle_tf_listener src/turtle_tf_listener.cpp)
target_link_libraries(turtle_tf_listener ${catkin_LIBRARIES})

编译并运行

cd ~/catkin_ws
catkin_make
source devel/setup.bash
roscore
rosrun turtlesim turtlesim_node
rosrun learning_tf turtle_tf_broadcaster __name:=turtle1_tf_brocaster /turtle1
rosrun learning_tf turtle_tf_broadcaster __name:=turtle2_tf_brocaster /turtle2
rosrun learning_tf turtle_tf_listener

• Python
  turtle_tf_broadcaster.py

#!/usr/bin/python
# -*- codeing: utf-8 -*-

import roslib
roslib.load_manifest('learning_tf')
import rospy

import tf
import turtlesim.msg

def handle_turtle_pose(msg,turtlename):
    br=tf.TransformBroadcaster()
    rospy.loginfo("Pose: %0.2f,%0.2f",msg.x,msg.y)
    br.sendTransform((msg.x,msg.y,0),
            tf.transformations.quaternion_from_euler(0,0,msg.theta),
            rospy.Time.now(),
            turtlename,
            'world')

if __name__=="__main__":
    rospy.init_node('turtle_tf_broadcaster')
    turtlename=rospy.get_param('~turtle')
    rospy.loginfo(turtlename)
    rospy.Subscriber('/%s/pose'%turtlename,
            turtlesim.msg.Pose,
            handle_turtle_pose,
            turtlename)
    rospy.spin()

turtle_tf_listener.py

#!/usr/bin/python
# -*- coding: utf-8 -*-

import roslib
roslib.load_manifest('learning_tf')
import rospy
import math
import tf
import geometry_msgs.msg
import turtlesim.srv

if __name__=='__main__':
    rospy.init_node('turtle_tf_listener')
    listener=tf.TransformListener()

    rospy.wait_for_service('/spawn')
    spawner=rospy.ServiceProxy('/spawn',turtlesim.srv.Spawn)
    spawner(4,2,0,'turtle2')
    rospy.loginfo('Spawn turtle2.')
    turtle_vel=rospy.Publisher('/turtle2/cmd_vel',geometry_msgs.msg.Twist,queue_size=10)
    rate=rospy.Rate(10)
    while not rospy.is_shutdown():
        try:
            (trans,rot)=listener.lookupTransform('turtle2','turtle1',rospy.Time(0))
        except(tf.LookupException,tf.ConnectivityException,tf.ExtrapolationException):
            continue
        angular=4*math.atan2(trans[1],trans[0])
        linear=0.5*math.sqrt(trans[0]**2+trans[1]**2)
        cmd=geometry_msgs.msg.Twist()
        cmd.linear.x=linear
        cmd.angular.z=angular
        turtle_vel.publish(cmd)
        rate.sleep()

运行

rosrun learning_tf turtle_tf_broadcaster.py __name:=turtle1_tf_badcaster _turtle:=turtle1
rosrun learning_tf turtle_tf_broadcaster.py __name:=turtle2_tf_badcaster _turtle:=turtle2
rosrun learning_tf turtle_tf_listener.py