项目实训 - 智能车系统 - 第五周记录

最新推荐文章于 2024-02-17 22:14:48 发布

handsome_hhh

最新推荐文章于 2024-02-17 22:14:48 发布

阅读量880

点赞数

分类专栏：智能车系统-项目实训文章标签： 3d c++ linux 自动驾驶

本文链接：https://blog.csdn.net/qq_38170211/article/details/123963379

版权

智能车系统-项目实训专栏收录该内容

14 篇文章 10 订阅

订阅专栏

项目实训 - 智能车系统 - 第五周记录

日期：3.21 – 3.27

项目进度

本周工作进展：

完成TF部分的移植（本周主要完成了TF消息发送、转换、发布的功能）

1、修改imu和map节点中与tf相关的类、函数

首先根据上周摘出来的tf相关代码，配合tf代码文档，进行初步的分析：

tf::TransformListener：此类从Transformer继承并自动订阅ROS转换消息

tf::Transfromer：提供系统中任意两个帧之间的坐标转换的类。

一个成员变量：tf2_buffer_ptr_
- 类型：rf2_ros::Buffer
waitForTransform
- 阻塞进程，直到找到一个转换或者超时
- 参数：string 目标坐标系， string 源坐标系，坐标系的时间，超时时间
- strip_leader_slash() 去掉前导’/’
- 该函数调用了tf2_buffer_ptr_.canTransform

tf2_ros::Buffer:: canTransform

用来测试是否存在这么一个转换

代码分析

bool tf2_ros::Buffer::canTransform

Buffer::canTransform(const std::string& target_frame, const std::string& source_frame, 
                      const ros::Time& time, const ros::Duration timeout, std::string* errstr) const
 {
   // Clear the errstr before populating it if it's valid.
   if (errstr)
     {
       errstr->clear();
     }
  
   if (!checkAndErrorDedicatedThreadPresent(errstr))
     return false;
  
   // poll for transform if timeout is set
   // now_fallback_to_wall：返回系统的当前时间
   ros::Time start_time = now_fallback_to_wall();
    // 阻塞的时间段
   const ros::Duration sleep_duration = timeout * CAN_TRANSFORM_POLLING_SCALE;
   while (now_fallback_to_wall() < start_time + timeout && 
          !canTransform(target_frame, source_frame, time) &&
          (now_fallback_to_wall()+ros::Duration(3.0) >= start_time) &&  //don't wait when we detect a bag loop
          (ros::ok() || !ros::isInitialized())) // Make sure we haven't been stopped (won't work for pytf)
     {
       sleep_fallback_to_wall(sleep_duration);
     }
   bool retval = canTransform(target_frame, source_frame, time, errstr);
   conditionally_append_timeout_info(errstr, start_time, timeout);
   return retval;
 }

最里层的canTransform

                            const ros::Time& time, std::string* error_msg) const
 {
   // Short circuit if target_frame == source_frame
   // 两个坐标系相同
   if (target_frame == source_frame)
     return true;
  
   if (warnFrameId("canTransform argument target_frame", target_frame))
     return false;
   if (warnFrameId("canTransform argument source_frame", source_frame))
     return false;
  
   boost::mutex::scoped_lock lock(frame_mutex_);
  
   CompactFrameID target_id = lookupFrameNumber(target_frame);
   CompactFrameID source_id = lookupFrameNumber(source_frame);
  
   if (target_id == 0 || source_id == 0)
   {
     if (error_msg)
       {
         if (target_id == 0)
         {
           *error_msg += std::string("canTransform: target_frame " + target_frame + " does not exist.");
         }
         if (source_id == 0)
         {
           if (target_id == 0)
           {
               *error_msg += std::string(" ");
           }
           *error_msg += std::string("canTransform: source_frame " + source_frame + " does not exist.");
         }
       }
     return false;
   }
   return canTransformNoLock(target_id, source_id, time, error_msg);
 }

一个想法：让一个函数阻塞三秒，函数内部是重复执行一个东西

那么是否可以：让函数只执行一次，在外面执行阻塞

这样的好处是可以只研究没有timeout的函数

tf::StampedTransform：TF使用的 stamped transform数据类型

StampedTransform只支持平移和旋转的刚性变换，不支持缩放/剪切。它可与Vector3、Quaternion和Matrix3x3线性代数类相结合使用。

tf::TransformException

tf::TransformBroadcaster

sendTransform()
- Send a StampedTransform The stamped data structure includes frame_id, and time, and parent_id already.
- 如果send的参数是tf::StampedTransform，会先转换成geometry_msgs::StampedTransform，然后调用tf2_broadcaster_.sendTransform(msgtf);
- 该类中定义了成员变量：tf2_ros::TransformBroadcaster tf::TransformBroadcaster::tf2_broadcaster_
- tf2_ros::TransformBroadcaster::sendTransform
- http://docs.ros.org/en/noetic/api/tf2_ros/html/c++/transform__broadcaster_8cpp_source.html#l00045
- tf2_msgs::TFMessage message;
  - 成员：geometry_msgs/TransformStamped[] transforms
- 将geometry_msgs::StampedTransform加入到一个vector中，然后遍历vector，依次将元素加入到message中，然后publish 定义的message
  - publish在tf2_ros::TransformBroadcaster中的构造函数中初始化
- 总结来说，该函数的作用就是：将信息转换成ros类型（tfMessage)，然后发布

tf::Transform map_to_odom

tf::Transform

Transform只支持平移和旋转的刚性变换，不支持缩放/剪切。它可以与Vector 3、四元数和Matrix3x3线性代数类结合使用。
Transform类的构造函数可以由：Quaternion和Matrix3x3来构造
#define TFSIMD_FORCE_INLINE inline
构造函数就给两个成员变量赋值
- 当构造函数中的参数是Quaternion时，也是直接赋值给Matrix3x3（所以就需要给这Quaternion Matrix3x3 Vector3加几个构造函数）
  - Vector3给Vector3赋值 √
  - Matrix3x3给Matrxi3x3赋值 √
  - Quaternion给Matrix3x3赋值 √
Transform类由两个成员变量：
其余的成员函数应该就是转换用的，需要从listern那边看了

tf::createQuaternionFromRPY()

从固定角度构造四元数
参数：double roll, double pitch, yaw
返回一个tf四元数

tf::Vector3

构造函数

tf::createQuaternionMsgFromRollPitchYaw()

tf::poseMsgToTF()

之前image用的：

tf::Quaternion √

tf::Matrix3x3 √

imu中用到的函数：

waitForTransform
lookupTransform
楼上两个函数都是Transformer的成员函数

通过对上面的分析，给我的感觉是：ros实现的tf通信机制比较复杂，如果进行单纯的移植复现的话，有很大的难度，涉及到大量ros::tf的类与函数，以及一些阻塞机制，难以协调。

2、自定义TF通信机制

通过之前的分析，否定了直接移植ros::tf通信的想法。我觉得根据项目中用到的tf的功能，简化的实现一个tf通信以及消息类型转换的框架，来满足项目的需要。

重新梳理一下TF的发布流程

以/tf话题为中心：

–>tf：imu、map
tf–>：imu、rviz

说明imu同时发布、订阅tf话题

现在就先不用管rviz订阅的tf话题

在原有的ros tf通信基础上并行增加一套tf机制，查看效果（目前的工作
后续有了自己的可视化工具后，在根据需要发布（后面的工作

在这里插入图片描述

理一下现在的思路：

一个新的进程
- 维护一个TFTree
- 用来接收其他进程发布的转换关系，并更新自己的TFTree
- 被别的进程订阅frame间的转换关系
send 转换关系
- sendTransform
listen 转换关系
- listener

https://www.cnblogs.com/sxy370921/p/11726691.html

http://docs.ros.org/en/noetic/api/tf/html/c++/namespacetf.html

所以是TFMessage就是TFTree

问题：如何维护之前发布的消息
- 感觉需要先看一下listener
- 好像需要维护若干棵按照时间排序的tftree，每个tftree里有若干个转换
一个想法，不用缓冲区，用一个进程维护，保存所有树
- tf_in_x1 对应每个发布
- tf_in_x2
- …
- tf_out

在重新进行梳理后，感觉思路清晰了许多，可以开始实际的编程工作了。

几个关键部分：

有关tf，发布的是什么
有关tf，接收的是什么
话题中保存的是什么
如何将接收的数据处理成需要的数据

1）发布

send的消息类型是TFMessage

该类型是一个TransformStamped[]，经过分析，数组的作用是考虑到发布的转换关系不止一个
我考虑不需要数组，每次只发布一个TransformStamped
- 该类型中也没有时间戳的数据，所以本次发布的数组内没有关联（应该

TransformStamped

std_msgs/Header header
string child_frame_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

所以发布的内容可以概括为：

父坐标系名称（在header中）
子坐标系名称
时间戳（在header中）
转换关系：四元数和vector

2）接收

这里参照imu里的listener的相关函数

lookupTransform：输入source坐标系和target坐标系，以及一个时间戳（还不知道是干啥的，应该是容忍的最早时间？），得到一个tf::StampedTransform
- tf::Transform类 * tf::StampedTransform类得到一个 tf::Transform类
  - 即一个坐标系 * 一个转换关系得到另一个坐标系
- tf::Transform
  - 用3x3矩阵和vector3存储坐标系信息（3x3矩阵对应四元数）
- tf::StampedTansform
  - 继承了tf::Transform
  - 增加的成员变量：child_frame_id frame_id stamp
所以得到的是StampedTransform

3）保存

TFHandler.h：用来接收其他节点发布的tf消息；保存接收的tf消息；处理tf转换关系。

定义一个新的节点TFChangeProjection，处理TF。

考虑在while循环结束/还是每次for循环结束：将graph写入共享文件中

4）处理数据

写一个处理函数（flyd + 返回转换 /重载？

在项目中的某个地方加入自定义的tf，同步发布订阅，比较结果（不转换+转换

根据上述思路编写代码，部分关键代码见下：

转换类相关定义（tf/tf.h）

    /**
     * @brief 矢量3可以用来表示三维点和矢量。当Vector 3存储在容器中时，它有一个未使用的w组件来适应16字节对齐。
     * 
     */
    class Vector3
    {


        public:
        //前3个对应x y z；最后一位没用到
        double x;
        double y;
        double z;
        double w;


        Vector3() {}
        Vector3(double tx, double ty, double tz)
        {
            x = tx;
            y = ty;
            z = tz;
        }
        Vector3(const Vector3& v)
        {
            x = v.x;
            y = v.y;
            z = v.z;
        }


        void setValue(double tx, double ty,double tz)
        {
            x = tx;
            y = ty;
            z = tz;
        }
    };

    /**
     * @brief 存储坐标系的信息
     * 
     * 
     */
    class Transform
    {
        public:
        /**
         * @brief 先设置public
         * 
         */
        Matrix3x3 m_basis;
        Vector3 m_origin;

        Transform() {}

        /**
         * @brief 3种不同的构造函数
         * 
         */

        explicit inline Transform(const Quaternion& q, const Vector3& c = Vector3(0,0,0)) 
            : m_basis(q),
            m_origin(c)
        {}

        explicit inline Transform(const Matrix3x3& b, const Vector3& c = Vector3(0,0,0))
            : m_basis(b),
            m_origin(c)
        {}
        inline Transform (const Transform& other)
            : m_basis(other.m_basis),
            m_origin(other.m_origin)
        {}



    };



    class StampedTransform : Transform
    {
        public:

        //父坐标系名称
        string frame_id_;
        //子坐标系名称
        string child_frame_id_;
        //时间戳
        msgs_Time stamp_;

        StampedTransform() {}

        StampedTransform(const Transform& input, const msgs_Time& timestamp, const string& frame_id, const string& child_frame_id):
        Transform(input), stamp_(timestamp), frame_id_(frame_id), child_frame_id_(child_frame_id) {}
    

    };

explicit https://blog.csdn.net/guoyunfei123/article/details/89003369

通信类型（ros2struct.h）

    /**
     * @brief TF中用到的类型
     * 
     */


    struct geometry_msgs_Transform
    {
        geometry_msgs_Vector3 translation;
        geometry_msgs_Quaternion rotation;
    };
    
    struct geometry_msgs_TransformStamped
    {
        std_msgs_Header header;
        char child_frame_id[STRING_LENGTH];
        geometry_msgs_Transform transform;
    };

    struct msgs_TransformStampedGraph
    {
        geometry_msgs_TransformStamped tf_graph[MAX_tf][MAX_tf];
        //name 顺序代表id
        char names[MAX_tf][STRING_LENGTH];
        //标记，记录每个id是否被占用
        uint8_t flags[MAX_tf];
        //标记每个对应关系
        uint8_t tf_graph_flags[MAX_tf][MAX_tf];


    };

TFChangeProjection.cpp

#include "tfHandler.h"
//#include "utility.h"
//#include "utility_struct.h"
int main(int argc, char** argv)
{

    ros::init(argc, argv, "lio_sam");


    ROS_INFO("\033[1;32m----> TF Projection Started.\033[0m");
    TransformHandle TH;

    //多线程执行回调函数

    std::thread subTFListenThread(&TransformHandle::tfListenHandler, &TH);
    ros::spin();
    subTFListenThread.join();

    return 0;
}

TFHandler.h（不展示代码细节，只展示接口）


//最多存储10个坐标系
#define MAX_tf 10 

// #define GlobalFrameName map  定义世界坐标系的名称是map

class TransformHandle : ToolsServer
{

    private:

    //直接创建一个2维数组用来存储坐标系转换关系 (数组便于通信)
    msgs_TransformStampedGraph transform_graph;
    //pthread_mutex_t mutex_graph;

    //与id相关的变量
    //pthread_mutex_t mutex_id;
    map<string, int> frame_name_to_id;
    //默认给map坐标系分配0
    int id_allocate = 1;

    bool floyd_need_update;
    double floyd_last_time;

    //存储floyd路径
    int* path = nullptr;
    double *time_difference = nullptr;
    int floyd_array_size = 0;


    public:
int cnm = 0;
    MemoryDefinition* pubTFGraph;

    MemoryDefinition* sub_share[100];



    /**
     * @brief Construct a new Transform Handle object
     * 无参构造函数
     */
    TransformHandle()

    ~TransformHandle()

    /**
     * @brief Construct a new Transform Handle object
     * 
     * @param graphIn 
     * 通过其他转换类来构造新的转换类
     * 用于其他节点查询转换关系
     */
    TransformHandle(msgs_TransformStampedGraph graphIn)

    /**
     * @brief 监听函数，用来接收其他节点发布的tf转换消息，并存储起来
     * 
     */
    void tfListenHandler()

    /**
     * @brief Get the id object
     * 
     * @param frame_name 
     * @return int 
     * 获取每个坐标系的id，如果没有则分配一个新的id
     */
    int get_id(string &frame_name)
 
    /**
     * @brief 将坐标系转换关系加入到graph中
     * 
     * @param msgIn 
     */
    void add_stransform(geometry_msgs_TransformStamped& msgIn)

    /**
     * @brief 通过floyd算法维护转换关系地图，求出两个坐标系之间的最新转换关系
     * 
     */
    #define INDEX(i,j) (i * size + j)
    void floyd_path_planning(double current_time)
   
    /**
     * @brief 在a转换中叠加b转换，结果存储在a中
     * 
     * @param tf_a 
     * @param tf_b 
     */
    void change_frame(geometry_msgs_TransformStamped& tf_a, geometry_msgs_TransformStamped& tf_b)
   

    /**
     * @brief Get the transform object
     * 
     * @param out_transform 
     * @param from_frame_name 
     * @param to_frame_name 
     * @param current_time 
     * @param max_time 
     * @return true 
     * @return false 
     * 给定两个坐标系名称，返回转换关系
     */
    bool get_transform(geometry_msgs_TransformStamped &out_transform,string from_frame_name, string to_frame_name,double current_time = DBL_MAX - 1, double max_time = DBL_MAX - 1)
  

};

3、测试TF通信机制

在原本使用ros进行TF发布坐标转换的地方，加上成自定义的发布方式（不直接替换，方便进行比较）
在某个地方同时用两种方式接收TF坐标转换，进行对比

之前忘记截图了，这里放一张自定义方式的运行截图（由于算法实现的差异，在精度上有10^-3左右的误差，从效果上来看可以忽略）

在这里插入图片描述

到此为止，TF的全部框架就已经搭建完毕，并且经过了测试。后面就可以继续进行话题的移植了

技术难点

TF相关内容的学习和代码的编写

bug记录

本周没有什么典型的bug，就不记录了

其他学习TF的相关资料阅读：

https://blog.csdn.net/a356337092/article/details/69945897

handsome_hhh

关注

0
点赞
踩
1

收藏

觉得还不错? 一键收藏
0
评论
项目实训 - 智能车系统 - 第五周记录

项目实训 - 智能车系统 - 第五周记录日期：3.21 – 3.27项目进度本周工作进展：完成TF部分的移植（本周主要完成了TF消息发送、转换、发布的功能）1、修改imu和map节点中与tf相关的类、函数首先根据上周摘出来的tf相关代码，配合tf代码文档，进行初步的分析：tf::TransformListener：此类从Transformer继承并自动订阅ROS转换消息tf::Transfromer：提供系统中任意两个帧之间的坐标转换的类。一个成员变量：tf2_buffer_ptr
复制链接

扫一扫

专栏目录