zoukankan      html  css  js  c++  java
  • ros之tf坐标系广播与监听的编程实现

    创建功能包-learning_tf

    $ cd ~/catkin_ws/src

    $ catkin_create_pkg learning_tf roscpp rospy tf turtlesim

    如何创建一个tf广播器

    *定义TF广播器(TransformBroadcaster)

    *创建坐标变换值

    *发布坐标变换(sendTransform)

    创建tf广播器代码(C++)如下

     1 /**                                                                 
     2  * 该例程产生tf数据,并计算、发布turtle2的速度指令
     3  */
     4 
     5 #include <ros/ros.h>
     6 #include <tf/transform_broadcaster.h>
     7 #include <turtlesim/Pose.h>
     8 
     9 std::string turtle_name;
    10 
    11 void poseCallback(const turtlesim::PoseConstPtr& msg)
    12 {
    13     // 创建tf的广播器
    14     static tf::TransformBroadcaster br;
    15 
    16     // 初始化tf数据
    17     tf::Transform transform;
    18     transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );
    19     tf::Quaternion q;
    20     q.setRPY(0, 0, msg->theta);
    21     transform.setRotation(q);
    22 
    23     // 广播world与海龟坐标系之间的tf数据
    24     br.sendTransform(tf::StampedTransform(transform, ros::Time::now(   ), "world", turtle_name));
    25 }
    26 
    27 int main(int argc, char** argv)
    28 {
    29     // 初始化ROS节点
    30     ros::init(argc, argv, "my_tf_broadcaster");
    31 
    32     // 输入参数作为海龟的名字
    33     if (argc != 2)
    34     {
    35         ROS_ERROR("need turtle name as argument"); 
    36         return -1;
    37     }
    38 
    39     turtle_name = argv[1];
    40 
    41     // 订阅海龟的位姿话题
    42     ros::NodeHandle node;
    43     ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &p   oseCallback);
    44 
    45     // 循环等待回调函数
    46     ros::spin();
    47 
    48     return 0;
    49 };
    50 
    

     首先程序要注意头文件的书写,能够是海龟正常运行,不管是海龟turtle1或海龟turtle2与word坐标系之间的位置关系都是通用的

    程序,在程序中通过输入参数确定当前小海龟的名称,并且去跟word坐标系去建立关系,因此程序会运行两遍,但是我们需要注意的是,任意的一个ros节点它的节点名在整个ros环境中只能有一个,如果程序运行两遍,会因为节点名字一样冲突,因此用到了重映射。后面会详细说明

    程序中将创建一个tf广播器,初始化tf数据,广播word与海龟坐标系之间的tf数据时需要设置一个时间戳(一般为10秒)。

    创建tf监听器

    如何实现一个tf监听器

    *定义TF监听器(TransformListener)

    *查找坐标变换(waitForTransform 、lookupTransform)

    创建tf监听器代码(C++)

     1 /**                                                                 
     2  * 该例程监听tf数据,并计算、发布turtle2的速度指令
     3  */
     4 
     5 #include <ros/ros.h>
     6 #include <tf/transform_listener.h>
     7 #include <geometry_msgs/Twist.h>
     8 #include <turtlesim/Spawn.h>
     9 
    10 int main(int argc, char** argv)
    11 {
    12     // 初始化ROS节点
    13     ros::init(argc, argv, "my_tf_listener");
    14 
    15     // 创建节点句柄
    16     ros::NodeHandle node;
    17 
    18     // 请求产生turtle2
    19     ros::service::waitForService("/spawn");
    20     ros::ServiceClient add_turtle = node.serviceClient<turtlesim::Sp   awn>("/spawn");
    21     turtlesim::Spawn srv;
    22     add_turtle.call(srv);
    23 
    24     // 创建发布turtle2速度控制指令的发布者
    25     ros::Publisher turtle_vel = node.advertise<geometry_msgs::Twist>   ("/turtle2/cmd_vel", 10);
    26 
    27     // 创建tf的监听器
    28     tf::TransformListener listener;
    29 
    30     ros::Rate rate(10.0);
    31     while (node.ok())
    32     {
    33         // 获取turtle1与turtle2坐标系之间的tf数据
    34         tf::StampedTransform transform;
    35         try
    36         {
    37             listener.waitForTransform("/turtle2", "/turtle1", ros::T   ime(0), ros::Duration(3.0));
    38             listener.lookupTransform("/turtle2", "/turtle1", ros::Ti   me(0), transform);
    39         }
    40         catch (tf::TransformException &ex) 
    41         {
    42             ROS_ERROR("%s",ex.what());
    43             ros::Duration(1.0).sleep();
    44             continue;
    45         }
    46 
    47         // 根据turtle1与turtle2坐标系之间的位置关系,发布turtle2的速
       度控制指令
    48         geometry_msgs::Twist vel_msg;
    49         vel_msg.angular.z = 4.0 * atan2(transform.getOrigin().y(),  
    50                                         transform.getOrigin().x());
    51         vel_msg.linear.x = 0.5 * sqrt(pow(transform.getOrigin().x(),    2) +
    52                                       pow(transform.getOrigin().y(),    2));
    53         turtle_vel.publish(vel_msg);
    54 
    55         rate.sleep();
    56     }
    57     return 0;
    58 };            
    

     通过tf监听器去监听turtle1和turtle2之间的坐标系关系,根据turtle1与turtle2坐标之间的关系,发布turtle2的速度控制指令。

    配置tf广播器和监听器代码编译规则

    配置CMakeLists.txt中的编译规则

    *设置需要编译的代码和生成的可执行文件

    *设置链接库

    在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

    $ roscore

    $ rosrun turtlesim turtlesim_node

    $ rosrun learning_tf turtle_tf_broadcaster__name:=turtle1_tf_broadcaster/turtle1

    $ rosrun learning_tf turtle_tf_broadcaster__name:=turtle1_tf_broadcaster/turtle2

    $ rosrun learning_tf turtle_tf_listener

    $ rosrun turtlesim turtle_teleop_key

    下面展示一下(python)的代码实现,其编译是一样的

    创建tf广播器

     1 #!/usr/bin/env python                                               
     2 # -*- coding: utf-8 -*-
     3 # 该例程将请求/show_person服务,服务数据类型learning_service::Person
     4 
     5 import roslib
     6 roslib.load_manifest('learning_tf')
     7 import rospy
     8 
     9 import tf
    10 import turtlesim.msg
    11 
    12 def handle_turtle_pose(msg, turtlename):
    13     br = tf.TransformBroadcaster()
    14     br.sendTransform((msg.x, msg.y, 0),
    15                      tf.transformations.quaternion_from_euler(0, 0,    msg.theta),
    16                      rospy.Time.now(),
    17                      turtlename,
    18                      "world")
    19 
    20 if __name__ == '__main__':
    21     rospy.init_node('turtle_tf_broadcaster')
    22     turtlename = rospy.get_param('~turtle')
    23     rospy.Subscriber('/%s/pose' % turtlename,
    24                      turtlesim.msg.Pose,
    25                      handle_turtle_pose,
    26                      turtlename)
    27     rospy.spin()
    

     创建tf监听器

     1 #!/usr/bin/env python                                               
     2 # -*- coding: utf-8 -*-
     3 # 该例程将请求/show_person服务,服务数据类型learning_service::Person
     4 
     5 import roslib
     6 roslib.load_manifest('learning_tf')
     7 import rospy
     8 import math
     9 import tf
    10 import geometry_msgs.msg
    11 import turtlesim.srv
    12 
    13 if __name__ == '__main__':
    14     rospy.init_node('turtle_tf_listener')
    15 
    16     listener = tf.TransformListener()
    17 
    18     rospy.wait_for_service('spawn')
    19     spawner = rospy.ServiceProxy('spawn', turtlesim.srv.Spawn)
    20     spawner(4, 2, 0, 'turtle2')
    21 
    22     turtle_vel = rospy.Publisher('turtle2/cmd_vel', geometry_msgs.ms   g.Twist,queue_size=1)
    23 
    24     rate = rospy.Rate(10.0)
    25     while not rospy.is_shutdown():
    26         try:
    27             (trans,rot) = listener.lookupTransform('/turtle2', '/t   urtle1', rospy.Time(0))
    28         except (tf.LookupException, tf.ConnectivityException, tf.E   xtrapolationException):
    29             continue
    30 
    31         angular = 4 * math.atan2(trans[1], trans[0])
    32         linear = 0.5 * math.sqrt(trans[0] ** 2 + trans[1] ** 2)
    33         cmd = geometry_msgs.msg.Twist()
    34         cmd.linear.x = linear
    35         cmd.angular.z = angular
    36         turtle_vel.publish(cmd)
    37                                                                   
    38         rate.sleep()
    39 
    40 
                                           
    
  • 相关阅读:
    [Hyper-V]使用操作系统模板创建新的虚拟机
    [Hyper-V]给Hyper-V创建两块网卡备用
    [Hyper-V]在Windows 8.1 操作系统中启用Hyper-V功能
    [MSSQL]SCOPE_IDENTITY,IDENT_CURRENT以及@@IDENTITY的区别
    [Microsoft Test Manager]CodeUI初探
    [MSSQL] Useful SQL Scripts
    Run Android on Your Netbook or Desktop
    How Delete File with Readonly Permission?
    Getting the first day in a week with T-SQL
    [HtmlUnit]Fetch Dynamic Html/Content Created By Javascript/Ajax
  • 原文地址:https://www.cnblogs.com/tanshengjiang/p/12246871.html
Copyright © 2011-2022 走看看