ROS series: common components (1)

Five, ROS common components

Some relatively useful tools are built into ROS; this chapter mainly introduces the following built-in components of ROS:

TF coordinate transformation, realize the conversion between different types of coordinate systems;

rosbag records the execution process of ROS nodes and realizes the effect of replaying the process;

rqt toolbox, a graphical debugging tool;

1. TF coordinate transformation

The robot system has multiple sensors to perceive the surrounding environment information, but it is wrong to directly equate the coordinate information of the object relative to the sensor with the coordinate orientation information between the robot system and the object, and it needs to go through a conversion process.

The conversion between different coordinates is relatively complicated and cumbersome, and related modules are directly encapsulated in ROS: Coordinate Transformation (TF);

**Concept: **tf: coordinate transformation;

**Coordinate system: **ROS calibrates the object through the coordinate system, and calibrates through the right-hand coordinate system;

[External link image transfer failed, the source site may have an anti-leeching mechanism, it is recommended to save the image and upload it directly (img-YOtVV6YC-1668337713252) (C:\Users\Haotian\AppData\Roaming\Typora\typora-user-images \1668314087211.png)]

**Function: **Used in ROS to realize the conversion of points or vectors between different coordinate systems;

**Description:**tf is deprecated, and now tf2 is used, which is simple and efficient, and the corresponding function packages are:

tf2_geometry_msgs: ROS message is converted to TF2 message;

tf2: encapsulates common messages of coordinate changes;

tf2_ros: provides roscpp and rospy bindings for tf2, and encapsulates common APIs for coordinate transformation;

1.1 Coordinate msg message

---

The commonly used data transmission carrier msg in coordinate transformation is:

geometry_msgs/TransformStamped

geometry_msgs/PointStamped

The former is used to transmit the position information related to the coordinate system, and the latter is used to transmit the information of coordinate points in a certain coordinate system;

• geometry_msgs/TransformStamped

Terminal input:

rosmsg info geometry_msgs/TransformStamped 

std_msgs/Header header   
  uint32 seq
  time stamp
  string frame_id						#坐标id
string child_frame_id					#子坐标系的id
geometry_msgs/Transform transform		#坐标信息
  geometry_msgs/Vector3 translation  #偏移量
    float64 x
    float64 y
    float64 z
  geometry_msgs/Quaternion rotation   #四元数
    float64 x
    float64 y
    float64 z
    float64 w
    

explain:

1. A and B two coordinate systems, discuss the relationship between the A coordinate system and the B coordinate system, at this time A is child_frame_id, and B is frame_id.
2. Quaternion is another form of Euler angle (flip, pitch, rotation), but there is a singularity in Euler angle, and there will be bugs when writing code, so choose a quaternion that looks like a complex number instead.

• geometry_msgs/PointStamped

Terminal input:

rosmsg info geometry_msgs/PointStamped 

std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
geometry_msgs/Point point
  float64 x
  float64 y
  float64 z

1.2 Static coordinate transformation

---

Static coordinate transformation, the relative position between two coordinate systems is fixed.

Basic implementation logic:

1. The relative relationship of the coordinate system, sent by the publisher
2. According to the coordinate system relationship of the subscription publisher, the subscriber passes in the coordinate point information (can be hard-coded), realizes the coordinate transformation with the help of tf, and outputs the result.

Basic implementation process:

1. Function package, add tf dependency
2. Write the publisher
3. Write the subscriber

Option A: C++

experiment:

1. Create a new function package demo04_ws, and create a new .cpp file under its src file that should contain the following dependent packages:

roscpp rospy std_msgs tf2 tf2_ros tf2_geometry_msgs geometry_msgs

2. Configure the new code to quickly compile the tasks.json file:

{
    
    
// 有关 tasks.json 格式的文档，请参见
    // https://go.microsoft.com/fwlink/?LinkId=733558
    "version": "2.0.0",
    "tasks": [
        {
    
    
            "label": "catkin_make:debug", //代表提示的描述性信息
            "type": "shell",  //可以选择shell或者process,如果是shell代码是在shell里面运行一个命令，如果是process代表作为一个进程来运行
            "command": "catkin_make",//这个是我们需要运行的命令
            "args": [],//如果需要在命令后面加一些后缀，可以写在这里，比如-DCATKIN_WHITELIST_PACKAGES=“pac1;pac2”
            "group": {
    
    "kind":"build","isDefault":true},
            "presentation": {
    
    
                "reveal": "always"//可选always或者silence，代表是否输出信息
            },
            "problemMatcher": "$msCompile"
        }
    ]
}

• Publisher

#include "ros/ros.h"
#include "tf2_ros/static_transform_broadcaster.h"
#include "geometry_msgs/TwistWithCovarianceStamped.h"
#include "tf2/LinearMath/Quaternion.h"//欧拉角

/**
 * @brief 需求：该节点需要发布两个坐标系之间的关系
 * 
 * 流程：
 * 1、包含头文件
 * 2、设置编码 节点初始化  nodehandle
 * 3、创建发布对象
 * 4、组织被发布的消息
 * 5、发布数据
 * 6、spin（）
 *
 */
int main(int argc, char* argv[])
{
    
    
    //  * 2、设置编码 节点初始化  nodehandle
    setlocale(LC_ALL,"");
    ros::init(argc, argv, "static_pub");
    ros::NodeHandle nh;
    //  * 3、创建发布对象
    tf2_ros::StaticTransformBroadcaster pub;

    //  * 4、组织被发布的消息
    geometry_msgs::TransformStamped tfs;
    tfs.header.stamp = ros::Time::now();
    tfs.header.frame_id = "base_link";//相对关系之中被参考的那个  参考坐标系
    tfs.child_frame_id = "laser";
    tfs.transform.translation.x = 0.2;
    tfs.transform.translation.y = 0.0;
    tfs.transform.translation.z = 0.5;
    //需要参考欧拉角转换
    tf2::Quaternion qtn;//创建 四元数 对象
    //向该对象设置欧拉角 这个对象可以将欧拉角转化为四元数
    qtn.setRPY(0, 0, 0); //目前研究对象雷达相对于小车是固定的，所以没有翻滚，俯仰，偏航 等因此设为0,0,0  欧拉角的单位是弧度
    tfs.transform.rotation.x = qtn.getX();
    tfs.transform.rotation.y = qtn.getY();
    tfs.transform.rotation.z = qtn.getZ();
    tfs.transform.rotation.w = qtn.getW();

    //  * 5、发布数据
    pub.sendTransform(tfs);
    //  * 6、spin（）
    ros::spin();
    return 0;
}

rostopic list

/rosout
/rosout_agg
/tf_static

rostopic echo /tf_static 

transforms: 
  - 
    header: 
      seq: 0
      stamp: 
        secs: 1668322909
        nsecs: 686407571
      frame_id: "base_link"
    child_frame_id: "laser"
    transform: 
      translation: 
        x: 0.2
        y: 0.0
        z: 0.5
      rotation: 
        x: 0.0
        y: 0.0
        z: 0.0
        w: 1.0
---

Use rviz to display:

rviz

[External link image transfer failed, the source site may have an anti-leeching mechanism, it is recommended to save the image and upload it directly (img-dOI1x1PP-1668337713255) (C:\Users\Haotian\AppData\Roaming\Typora\typora-user-images \1668324095841.png)]

• Subscriber

#include <ros/ros.h>
#include "tf2_ros/transform_listener.h"//订阅数据
#include "tf2_ros/buffer.h"//缓存
#include "geometry_msgs/PointStamped.h"//坐标点信息
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"

/**
 * @brief 订阅方
 * 需求：
 *订阅发布的坐标系相对关系，传入一个坐标点，调用tf实现转换
头发》订阅坐标系相对关系
 * 4、组织一个坐标点的数据
 * 5、转换算法实现   调用tf内置实现
 * 6、输出转换结果
 */
int main(int argc, char* argv[])
{
    
    
    //  * 2、编码 初始化 nodehandle 必须有nodehandle的
    setlocale(LC_ALL,"");
    ros::init(argc, argv, "static_sub");
    ros::NodeHandle nh;
    //  * 3、创建一个订阅对象   ----》订阅坐标系相对关系
    //3-1 创建一个buffer缓存
    tf2_ros::Buffer buffer;

    // 3-2 创建一个监听对象 （监听对象可以将被订阅的的数据存入缓存buffer之中）
    tf2_ros::TransformListener listener(buffer);

    //  * 4、组织一个坐标点的数据
    geometry_msgs::PointStamped ps;
    ps.header.frame_id = "laser";
    ps.header.stamp = ros::Time::now();
    ps.point.x = 2.0;
    ps.point.y = 3.0;
    ps.point.z = 5.0;
    //添加休眠  等待发布方
    // ros::Duration(2).sleep();
    //  * 5、转换算法实现   调用tf内置实现
    ros::Rate rate(10);
    while(ros::ok())
    {
    
    
        //核心代码  ----将 ps 转换成相对于 base——link的坐标点
        geometry_msgs::PointStamped ps_out;
        /*
            调用了buffer的转换函数 transform
            参数1：被转换的坐标点
            参数2：目标坐标系
            返回值：输出的坐标点

            注意：调用是必须包含头文件 tf2_geometry_msgs/
            注意：运行时候出现报错，提示base_link不存在 
                 原因分析：是因为发布方可能还没有发布 ，订阅数据是一个耗时操作，可能调用转换时，坐标系的相对关系还没有订阅
                 解决办法：方案1：调用转换函数的前面加一个休眠
                         方案2：异常处理 (建议使用)
        */
       try
       {
    
    
           ps_out = buffer.transform(ps, "base_link");
           //  * 6、输出转换结果
           ROS_INFO(
               "转换后的坐标值:(%.2f,%.2f,%.2f),参考的坐标系:%s",
               ps_out.point.x,
               ps_out.point.y,
               ps_out.point.z,
               ps_out.header.frame_id.c_str());
       }
       catch(const std::exception& e)
       {
    
    
            //std::cerr << e.what() << '\n';
            ROS_INFO("异常消息:%s", e.what());
       }
    //    ps_out = buffer.transform(ps, "base_link");
    //    //  * 6、输出转换结果
    //    ROS_INFO(
    //        "转换后的坐标值:(%.2f,%.2f,%.2f),参考的坐标系:%s",
    //        ps_out.point.x,
    //        ps_out.point.y,
    //        ps_out.point.z,
    //        ps_out.header.frame_id.c_str());
       rate.sleep();
       ros::spinOnce();
    }

    return 0;
}

Results of the:

[ INFO] [1668329057.735620630]: 异常消息:"base_link" passed to lookupTransform argument target_frame does not exist. 
[ INFO] [1668329057.835439384]: 转换后的坐标值:(2.20,3.00,5.50),参考的坐标系:base_link

Replenish:

When the relative position between coordinate systems is fixed, the required parameters are also fixed: parent coordinate name, child coordinate system name, x offset, y offset, z offset, x roll angle, y pitch Angle, z yaw angle, the implementation logic is the same, but the parameters are different, then the ROS system has already packaged a special node, and the usage method is as follows:

rosrun tf2_ros static_transform_publisher x偏移量 y偏移量 z偏移量 z偏航角度 y俯仰角度 x翻滚角度 父级坐标系 子级坐标系

Example:

rosrun tf2_ros static_transform_publisher 0.2 0 0.5 0 0 0 /base_link /camera 

rviz

rosrun tf2_ros static_transform_publisher 0.1 0.0 0.3 1.0 0 0 /base_link /camera 

The conversion effect can be seen in rviz, and it is also recommended to use this method to directly realize the relative information release of the static coordinate system.