zoukankan      html  css  js  c++  java
  • Tutorial2

    一.写一个tf2的broadcaster

    本教程关于怎样broadcast一个机器人的坐标系到tf2上.

    1.创建一个learning_tf2包

    catkin_create_pkg learning_tf2 tf2 tf2_ros roscpp rospy turtlesim
    View Code

    本文要broadcast变化中的turtles的坐标系.

    新建文件src/turtle_tf2_broadcaster.cpp

    //
    // Created by gary on 2019/9/4.
    //
    
    #include <ros/ros.h>
    #include <tf2/LinearMath/Quaternion.h>
    #include <tf2_ros/transform_broadcaster.h>
    #include <geometry_msgs/TransformStamped.h>
    #include <turtlesim/Pose.h>
    
    std::string turtle_name;
    
    void poseCallback(const turtlesim::PoseConstPtr& msg)
    {
        static tf2_ros::TransformBroadcaster br;
        geometry_msgs::TransformStamped transformStamped;
    
        transformStamped.header.stamp    = ros::Time::now();
        transformStamped.header.frame_id = "world";
        transformStamped.child_frame_id  = turtle_name;
        transformStamped.transform.translation.x = msg->x;
        transformStamped.transform.translation.y = msg->y;
        transformStamped.transform.translation.z = 0.0;
    
        tf2::Quaternion q;
        q.setRPY(0, 0, msg->theta);
        transformStamped.transform.rotation.x = q.x();
        transformStamped.transform.rotation.y = q.y();
        transformStamped.transform.rotation.z = q.z();
        transformStamped.transform.rotation.w = q.w();
        br.sendTransform(transformStamped);
    }
    
    int main(int argc, char**argv)
    {
        ros::init(argc, argv, "my_tf2_broadcaster");
        ros::NodeHandle private_node("~");
        if(!private_node.hasParam("turtle"))
        {
            if(argc != 2)
            {
                ROS_ERROR("need turtle name as argument");
                return -1;
            }
            turtle_name = argv[1];
        }
        else
        {
            private_node.getParam("turtle", turtle_name);
        }
        ros::NodeHandle node;
        ros::Subscriber sub = node.subscribe(turtle_name + "/pose", 10, &poseCallback);
    
        ros::spin();
        return 0;
    }
    View Code

    添加一个launch文件

    start_demo.launch

    <launch>
         <!-- Turtlesim Node-->
        <node pkg="turtlesim" type="turtlesim_node" name="sim"/>
    
        <node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>
        <!-- Axes -->
        <param name="scale_linear" value="2" type="double"/>
        <param name="scale_angular" value="2" type="double"/>
    
        <node pkg="learning_tf2" type="turtle_tf2_broadcaster"
              args="/turtle1" name="turtle1_tf2_broadcaster" />
        <node pkg="learning_tf2" type="turtle_tf2_broadcaster"
              args="/turtle2" name="turtle2_tf2_broadcaster" />
    
      </launch>
    View Code

    修改CMakeLists.txt,添加如下

    add_executable(turtle_tf2_broadcaster src/turtle_tf2_broadcaster.cpp)
    target_link_libraries(turtle_tf2_broadcaster
     ${catkin_LIBRARIES}
    )
    
    ## Mark other files for installation (e.g. launch and bag files, etc.)
    install(FILES
     start_demo.launch
     # myfile2
     DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
    )
    View Code

    然后编译之.

    启动:

     $ roslaunch learning_tf2 start_demo.launch

    启动之后,就可以在turtle sim中看到一个turtle.

    2.查看结果

    $ rosrun tf tf_echo /world /turtle1

    通过上面的命令可以看到turtle1在world中的位置信息.目前turtle2的信息没有.

    二. 写一个tf2的listener

    创建文件 src/turtle_tf2_listener.cpp

    //
    // Created by gary on 2019/9/4.
    //
    #include <ros/ros.h>
    #include <tf2_ros/transform_listener.h>
    #include <geometry_msgs/TransformStamped.h>
    #include <geometry_msgs/Twist.h>
    #include <turtlesim/Spawn.h>
    
    int main(int argc, char** argv)
    {
        ros::init(argc, argv, "my_tf2_listener");
        ros::NodeHandle node;
    
        ros::service::waitForService("spawn");
        ros::ServiceClient spawner = node.serviceClient<turtlesim::Spawn>("spawn");
        turtlesim::Spawn turtle;
        turtle.request.x = 4;
        turtle.request.y = 2;
        turtle.request.theta = 0;
        turtle.request.name = "turtle2";
    
        spawner.call(turtle);
        ros::Publisher turtle_vel =
                node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 10);
        tf2_ros::Buffer tfBuffer;
        tf2_ros::TransformListener tfListener(tfBuffer);
    
        ros::Rate rate(10.0);
    
        while(node.ok())
        {
            geometry_msgs::TransformStamped transformStamped;
            try
            {
                transformStamped = tfBuffer.lookupTransform("turtle2", "turtle1",ros::Time(0));
            }
            catch (tf2::TransformException &ex)
            {
                ROS_WARN("%s",ex.what());
                ros::Duration(1.0).sleep();
                continue;
            }
            geometry_msgs::Twist vel_msg;
            vel_msg.angular.z = 4.0 * atan2(transformStamped.transform.translation.y,
                    transformStamped.transform.translation.x);
            vel_msg.linear.x = 0.5 * sqrt(pow(transformStamped.transform.translation.x,2) +
                    pow(transformStamped.transform.translation.y, 2));
            turtle_vel.publish(vel_msg);
            rate.sleep();
    
        }
        return 0;
    }
    View Code

    CMakeLists.txt

    add_executable(turtle_tf2_listener src/turtle_tf2_listener.cpp)
    target_link_libraries(turtle_tf2_listener
            ${catkin_LIBRARIES}
            )
    View Code
    tf2_ros::Buffer tfBuffer;
     tf2_ros::TransformListener tfListener(tfBuffer);

    说明一下:

    创建了一个TransformListener对象,一旦TransformListener被创建,它就开始接收tf2的变换,且会默认保存10s的变换.

    transformStamped = tfBuffer.lookupTransform("turtle2", "turtle1",
                                ros::Time(0));

    参数说明:

    1. We want the transform to this frame (target frame) ...
    2. ... from this frame (source frame).
    3. The time at which we want to transform. Providing ros::Time(0) will just get us the latest available transform.

    4. Duration before timeout. (optional, default=ros::Duration(0.0))

    在launch文件中添加如下

    <launch>
      <!--Turtlesim Node-->
      <node pkg = "turtlesim" type = "turtlesim_node" name="sim"/>
      <node pkg = "turtlesim" type = "turtle_teleop_key" name = "teleop" output = "screen"/>
    
       <!-- Axes -->
      <param name="scale_linear" value="2" type="double"/>
      <param name="scale_angular" value="2" type="double"/>
    
      <node pkg = "learning_tf2" type = "turtle_tf2_broadcaster"
            args = "/turtle1" name ="turtle1_tf2_broadcaster" output = "screen" />
    
      <node pkg = "learning_tf2" type = "turtle_tf2_broadcaster"
            args = "/turtle2" name ="turtle2_tf2_broadcaster" output = "screen" />
      <node pkg="learning_tf2" type="turtle_tf2_listener"
                name="listener" />
    </launch>
    View Code

    启动launch文件

     $ roslaunch learning_tf2 start_demo.launch
  • 相关阅读:
    占满屏幕的宽高,当把textarea换成其他标签的时候,怎么才能编辑?
    鼠标点击文本框后,里面的文字就消失或全选中
    jquery中选取兄弟节点的方法
    文本出现省略号
    滚动条的样式
    省略号的样式。
    input的placeholder在ie9下不兼容的结局办法。
    [CF1097D] Makoto and a Blackboard
    [CF552C] Vanya and Scales
    [CF1353E] K-periodic Garland
  • 原文地址:https://www.cnblogs.com/gary-guo/p/11460477.html
Copyright © 2011-2022 走看看