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