ROS シリーズ: 第 5 章 共通コンポーネント (2)

五、ROS共通コンポーネント

ROS にはいくつかの比較的便利なツールが組み込まれています。この章では主に、次の ROS の組み込みコンポーネントを紹介します。

TF 座標変換は、異なる種類の座標系間の変換を実現します。

rosbag は ROS ノードの実行プロセスを記録し、プロセスを再生する効果を実現します。

rqt ツールボックス、グラフィカル デバッグ ツール。

1. TF座標変換

ロボットシステムは周囲の環境情報を認識するために複数のセンサーを備えていますが、センサーを基準とした物体の座標情報をロボットシステムと物体の間の座標配向情報と直接同一視するのは誤りであり、センサーを介した物体の座標方位情報を直接同一視することは誤りであり、変換プロセス。

異なる座標間の変換は比較的複雑で扱いにくいため、関連モジュールは ROS に直接カプセル化されています。

**概念: **tf: 座標変換。

**座標系: **ROS は座標系を通じてオブジェクトを校正し、右手座標系を通じて校正します。

[外部リンク画像の転送に失敗しました。ソース サイトにはリーチ防止メカニズムがある可能性があります。画像を保存して直接アップロードすることをお勧めします (img-v0jRw5FA-1668434092634) (C:\Users\Haotian\AppData\Roaming\Typora\) typora-user-images \1668314087211.png)]

**機能: **異なる座標系間での点またはベクトルの変換を実現するために ROS で使用されます。

**説明:**tf は非推奨となり、現在はシンプルで効率的な tf2 が使用されており、対応する関数パッケージは次のとおりです。

tf2_geometry_msgs: ROS メッセージは TF2 メッセージに変換されます。

tf2: 座標変更の一般的なメッセージをカプセル化します。

tf2_ros: tf2 の roscpp および rospy バインディングを提供し、座標変換用の共通 A​​PI をカプセル化します。

1.1 座標メッセージメッセージ


座標変換で一般的に使用されるデータ送信キャリア msg は次のとおりです。

geometry_msgs/TransformStamped
geometry_msgs/PointStamped

前者は座標系に関する位置情報を送信するために使用され、後者は特定の座標系における座標点の情報を送信するために使用されます。

  • geometry_msgs/TransformStamped

端子入力:

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
    

説明する:

  1. A と B の 2 つの座標系について、A 座標系と B 座標系の関係について説明します。このとき、A は child_frame_id、B は Frame_id です。
  2. クォータニオンはオイラー角の別の形式 (反転、ピッチ、回転) ですが、オイラー角には特異点があり、コードを記述するときにバグが発生するため、代わりに複素数のように見えるクォータニオンを選択してください。
  • geometry_msgs/PointStamped

端子入力:

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 静的座標変換


静的座標変換では、2 つの座標系間の相対位置は固定されます。

基本的な実装ロジック:

  1. 出版社から送信された座標系の相対関係
  2. サブスクリプション発行者の座標系関係に従って、サブスクライバーは座標点情報 (ハードコーディング可能) を渡し、tf を使用して座標変換を実現し、結果を出力します。

基本的な実装プロセス:

  1. 関数パッケージ、tf 依存関係を追加
  2. 発行者を書きます
  3. 購読者を書きます

オプション A: C++

実験:

1. 新しい関数パッケージdemo04_wsを作成し、そのsrcファイルの下に次の依存パッケージを含む新しい.cppファイルを作成します。

roscpp rospy std_msgs tf2 tf2_ros tf2_geometry_msgs geometry_msgs

2.tasks.json ファイルをすばやくコンパイルするように新しいコードを構成します。

{
    
    
// 有关 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"
        }
    ]
}
  • 出版社
#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
---

rviz を使用して以下を表示します。

rviz

[外部リンク画像の転送に失敗しました。ソース サイトにはリーチ防止メカニズムがある可能性があります。画像を保存して直接アップロードすることをお勧めします (img-YxcJHdjc-1668434092636) (C:\Users\Haotian\AppData\Roaming\Typora\) typora-user-images \1668324095841.png)]

  • 購読者
#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;
}

結果:

[ 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

補充:

座標系間の相対位置が固定されると、必要なパラメータも固定されます: 親座標名、子座標系名、x オフセット、y オフセット、z オフセット、x ロール角、y ピッチ角、z ヨー角、実装ロジックは同じですが、パラメータが異なる場合、ROS システムはすでに特別なノードをパッケージ化しており、使用方法は次のとおりです。

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

例:

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 

変換効果は rviz で見ることができ、静的座標系の相対情報解放を直接実現するためにこの方法を使用することも推奨されます。

1.3 動的座標変換


前のセクションの座標変換リリースが必要です

および動的座標サブスクリプション

  • パブリッシャーが実装する
#include <ros/ros.h>
#include "turtlesim/Pose.h"
#include "tf2_ros/transform_broadcaster.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"

/*
    发布方:需要订阅乌龟的位资信息,转换成想对于窗体的一个坐标关系,并发布
    准备:
        话题:/turtle1/pose

        消息:turtlesim/Pose
    liucheng流程:
        1、包含头文件
        2、设置编码、初始化、nodehandle
        3、创建订阅对象、订阅/turtle1/pose
        4、回调函数处理订阅的消息。将位资信息转换成坐标相对关系并发布
        5、spin()

*/
void doPose(const turtlesim::Pose::ConstPtr& pose)//const 防止函数修改引用内容   传引用提高效率
{
    
    
    //获取位资信息,转换成坐标系想对关系(核心)并发布
    //创建TF发布对象
    static tf2_ros::TransformBroadcaster pub; // static修饰  使得每次回调函数是哟你pub这个函数;锁定pub
    //组织被发布的数据
    geometry_msgs::TransformStamped ts;
    ts.header.frame_id = "world";
    ts.header.stamp = ros::Time::now();
    ts.child_frame_id = "turtle1";
    //坐标系偏移量
    ts.transform.translation.x = pose->x;
    ts.transform.translation.y = pose->y;
    ts.transform.translation.z = 0;
    //四元数
    //位资信息之中  没有四元数   有个z轴的偏航角度  而乌龟是2D 没有翻滚和府仰角 所以可以得出乌龟的欧拉角为:0 0 theta
    tf2::Quaternion qtn;
    qtn.setRPY(0,0,pose->theta);
    ts.transform.rotation.x = qtn.getX();
    ts.transform.rotation.y = qtn.getY();
    ts.transform.rotation.z = qtn.getZ();
    ts.transform.rotation.w = qtn.getW();

    //发布
    pub.sendTransform(ts);
}
int main(int argc, char* argv[])
{
    
    
    // 2、设置编码、初始化、nodehandle
    setlocale(LC_ALL,"");
    ros::init(argc, argv, "dynamic_pub");
    ros::NodeHandle nh;
    // 3、创建订阅对象、订阅/turtle1/pose
    ros::Subscriber sub = nh.subscribe("turtle1/pose",100,doPose);
    // 4、回调函数处理订阅的消息。将位资信息转换成坐标相对关系并发布
    // 5、spin()
    ros::spin();
    return 0;
}

結果を示す:

  • サブスクライバーの実装
#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 = "turtle1";
    //时间戳
    ps.header.stamp = ros::Time(0.0);//动态坐标转换的时候buffer生成会有时间戳,时间戳不对代表坐标变化,因此报错
    // ps.header.stamp = ros::Time::now();

    ps.point.x = 2.0;
    ps.point.y = 3.0;
    ps.point.z = 0.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, "world");
            //  * 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;
}
  • 結果を示す

[外部リンク画像の転送に失敗しました。ソース サイトにはリーチ防止メカニズムがある可能性があります。画像を保存して直接アップロードすることをお勧めします (img-qymlplVl-1668434092640) (C:\Users\Haotian\AppData\Roaming\Typora\) typora-user-images \1668426497090.png)]

概要: サブスクライバの実装プロセスは、前のセクションの静的サブスクライバの実装プロセスとほぼ同じです。必要なのは、対応する座標名を変更することと、バッファのタイムスタンプが現在のものではなく、変更する必要があることだけです。 0.0まで;

1.4 複数座標変換


**要件: **複数の座標間の変換。前の 2 つのセクションは 2 つの座標間の相対位置の変換です。このセクションでは、複数の座標間の変換を紹介します。親座標ワールド、2 つの子座標son1、son2、世界を基準としたson1、および世界を基準としたson2が既知である、son2でson1の原点の座標を見つける、またはすでにSon1 の点の座標がわかったら、その参照座標 Son2 の座標を見つけます。

分析します:

1. ワールドを基準にしてson1をパブリッシュし、ワールドを基準にしてson2をパブリッシュします。

2. 情報を購読して解放し、tf2 を使用して Son1 と Son2 の座標関係を変換します。

3. 座標点間の変換を実現する

プロセス:

1. 新しい関数パッケージを作成し、関連する依存関係を追加します。

2. 座標相対関係パブリッシャーを作成する

3. 座標相対関係サブスクライバを作成する

  • 発行者は静的座標を使用して起動ファイルを変換します
<?xml version="1.0"?>
<launch>
<!-- 发布son1 相对于world 以及 son2相对于world 的坐标关系  -->
    <node name="son1" pkg="tf2_ros" type="static_transform_publisher" args="5 0 0 0 0 0 /world /son1" output="screen"/>
    <node name="son2" pkg="tf2_ros" type="static_transform_publisher" args="3 0 0 0 0 0 /world /son2" output="screen"/>
    
</launch>
  • 購読者
#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"
#include "geometry_msgs/TransformStamped.h"

/**
 * @brief
 * 订阅方实现:1.计算son1与2之间的相对关系2.计算son1的某个坐标点对于son2的坐标值
 * 流程:
 * 1、包含头文件
 * 2、编码初始化nodehandle
 * 3、创建订阅对象,
 * 4、编写解析逻辑
 * 5、spinOnce
 *
 */
int main(int argc, char *argv[])
{
    
    
    //  * 2、编码初始化nodehandle
    setlocale(LC_ALL,"");

    ros::init(argc, argv, "tfs_sub");
    ros::NodeHandle nh;
    
    //  * 3、创建订阅对象,
    tf2_ros::Buffer buffer;
    tf2_ros::TransformListener sub(buffer);

    //  * 4、编写解析逻辑
    //创建坐标点
    geometry_msgs::PointStamped psAtson1;
    psAtson1.header.stamp = ros::Time::now();
    psAtson1.header.frame_id = "son1";
    psAtson1.point.x = 1.0;
    psAtson1.point.y = 2.0;
    psAtson1.point.z = 3.0;

    ros::Rate rate(10);
    while (ros::ok())
    {
    
       
        //核心
        try
        {
    
    
            //1.计算son1与son2的相对关系
            /*
                A 相对 B 坐标关系

                参数1:目标坐标系     B
                参数2:源坐标系       A
                参数3:ros::Time(0.0)  取时间间隔最短的两个坐标关系计算相对关系
                返回值:geometry_msgs::TransformStamped 源坐标系相对于目标坐标系的相对关系

            */
            geometry_msgs::TransformStamped son1ToSon2 = buffer.lookupTransform("son2","son1",ros::Time(0));
            ROS_INFO("son1相对son2相对关系信息:父级:%s,子级:%s 偏移量(%.2f,%.2f,%.2f)",
                     son1ToSon2.header.frame_id.c_str(), // son2
                     son1ToSon2.child_frame_id.c_str(),  // son1
                     son1ToSon2.transform.translation.x,
                     son1ToSon2.transform.translation.y,
                     son1ToSon2.transform.translation.z


            );
            // 2.计算son1坐标点在son2的坐标值
            geometry_msgs::PointStamped psAtson2= buffer.transform(psAtson1,"son2");
            ROS_INFO(
                "坐标点在son2中的值为(%.2f,%.2f,%.2f)",psAtson2.point.x,psAtson2.point.y,psAtson2.point.z
            );
        }
        catch(const std::exception& e)
        {
    
    
            // std::cerr << e.what() << '\n';

        }

        //  * 5、spinOnce
        rate.sleep();
        ros::spinOnce();
    }
    

    return 0;
}

起動ファイルの下のノードタグ内の args パラメータを再度確認し、座標点と新しい関数buffer.lookupTraansform("", "", ros::Time(0.0)) を生成します。

1.5 座標系の関係を確認する


  1. tf2_tools がインストールされているかどうかを確認します:「 rospack find tf2_tools 」
  2. インストールコマンド: 「sudo apt install ros-noetic-tf2-tools」
  3. roslaunch tf03_tfs tfs_c.launch
  4. rosrun tf2_tools view_frames.py は、frame.pdf ファイルを生成します
  5. エビンスフレーム.pdf
  6. 効果:
    ここに画像の説明を挿入

1.6 TF座標変換の具体的な操作

1.7 TF2 と TF

1.8 概要

おすすめ

転載: blog.csdn.net/TianHW103/article/details/127856536