Tutorial2

一.写一个tf2的broadcaster

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

1.创建一个learning_tf2包

catkin_create_pkg learning_tf2 tf2 tf2_ros roscpp rospy turtlesim

本文要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;
}

 

添加一个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>

修改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}
)

然后编译之.

启动:

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

CMakeLists.txt

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

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>

启动launch文件

 $ roslaunch learning_tf2 start_demo.launch