启动fishbot建图程序后应该从哪里提取定位数据？

砖头

最近看了tf树的使用，上网冲浪的时候也搜到了evo这种slam评价工具。因为我在fishbot跑建图时有看到tf的话题，我就想着用evo在fishbot上评价一下定位精度。

先说结果。我监听了tf里的laser_frame相对于map的位姿，并把他与小车发布的/odom话题进行比较，得到了相当离谱的图：

这里我把/odom话题的数据当作真值ground truth，tf树里laser_frame的位姿当作算法给的位姿。可以看到算法给的彩色轨迹相当的夸张（我跑了一圈我的宿舍，大致是长方形的，所以图里的ground truth是大致符合我印象里走的路线的）我想问是哪里出了问题呢？是不是不应该监听tf广播里的laser_frame？应该从哪里提取数据当作算法的位姿呢？

因为evo能跑起来给出结果，所以我感觉我写的提取数据的程序应该是没问题。以防万一在这里放一下过程：

使用了的命令行：

sudo docker run -it --rm -v /dev:/dev -v /dev/shm:/dev/shm --privileged --net=host microros/micro-ros-agent:$ROS_DISTRO udp4 --port 8888 -v6    #MicroRos

xhost + && sudo docker run  -it --rm  -v /dev:/dev -v /dev/shm:/dev/shm --privileged  -v /tmp/.X11-unix:/tmp/.X11-unix --device /dev/snd -e DISPLAY=unix$DISPLAY -p 8889:8888 registry.cn-hangzhou.aliyuncs.com/fishros/fishbot_laser #启动雷达按2

然后启动我自己的提取数据的节点，生成evo排版的文件：
提取/odom数据：

#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import rclpy
from rclpy.node import Node
from nav_msgs.msg import Odometry
from rclpy.clock import Clock

class NodeToEvo(Node):
    def __init__(self,name):
        super().__init__(name)

        self.get_logger().info("transformation starts")
        self.subscribe = self.create_subscription(Odometry,"odom",self.callback,rclpy.qos.qos_profile_sensor_data)
        self.path = "/home/cxw"
        self.file = self.path + "/ground_truth_odom" + '.txt'
        self.writter = open(self.file,'w')


    def callback(self,msg):
        time_stamp = Clock().now()
        position = msg.pose.pose.position
        orientation = msg.pose.pose.orientation
        self.writter.write(#str(msg.header.stamp.sec)+'.'+str(msg.header.stamp.nanosec)+' '+
                           str(time_stamp.to_msg().sec)+'.'+str(time_stamp.to_msg().nanosec)+' '+
                           str(position.x)+' '+
                           str(position.y)+' '+
                           str(position.z)+' '+
                           str(orientation.x)+' '+
                           str(orientation.y)+' '+
                           str(orientation.z)+' '+
                           str(orientation.w)+'\n')
        self.get_logger().info("odom written successfully")


def main(args=None):
    rclpy.init(args=args)
    node = NodeToEvo("odom_to_evo")
    rclpy.spin(node)
    node.destroy_node() 
    rclpy.shutdown()

提取tf广播的laser_frame:

#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import rclpy                                              # ROS2 Python接口库
from rclpy.node import Node                               # ROS2 节点类
import tf_transformations                                 # TF坐标变换库
from tf2_ros import TransformException                    # TF左边变换的异常类
from tf2_ros.buffer import Buffer                         # 存储坐标变换信息的缓冲类
from tf2_ros.transform_listener import TransformListener  # 监听坐标变换的监听器类
from rclpy.clock import Clock



class tf(Node):

    def __init__(self,name):
        super().__init__(name)
        self.tf_buffer=Buffer()
        self.tf_lisenter = TransformListener(self.tf_buffer,self)

        self.get_logger().info("tf_to_evo starts")
        self.path = "/home/cxw"
        self.file = self.path + "/tf_laser_frame" + '.txt'
        self.writter = open(self.file,'w')

        self.declare_parameter('source_frame', 'map')             # 创建一个源坐标系名的参数
        self.source_frame = self.get_parameter(                     # 优先使用外部设置的参数值，否则用默认值
            'source_frame').get_parameter_value().string_value

        self.declare_parameter('target_frame', 'laser_frame')             # 创建一个目标坐标系名的参数
        self.target_frame = self.get_parameter(                     # 优先使用外部设置的参数值，否则用默认值
            'target_frame').get_parameter_value().string_value
        
        self.timer = self.create_timer(0.05, self.on_timer)          # 创建一个固定周期的定时器，处理坐标信息

    def on_timer(self):
        try:
            time_stamp = Clock().now()
            now = rclpy.time.Time()                                 # 获取ROS系统的当前时间
            trans = self.tf_buffer.lookup_transform(                # 监听当前时刻源坐标系到目标坐标系的坐标变换
                self.target_frame,
                self.source_frame,
                now)
        except TransformException as ex:                            # 如果坐标变换获取失败，进入异常报告
            self.get_logger().info(
                f'Could not transform {self.target_frame} to {self.source_frame}: {ex}')
            return
        
        position  = trans.transform.translation                          # 获取位置信息
        orientation = trans.transform.rotation                             # 获取姿态信息（四元数） 
        self.writter.write(str(time_stamp.to_msg().sec)+'.'+str(time_stamp.to_msg().nanosec)+' '+
                           str(position.x)+' '+
                           str(position.y)+' '+
                           str(position.z)+' '+
                           str(orientation.x)+' '+
                           str(orientation.y)+' '+
                           str(orientation.z)+' '+
                           str(orientation.w)+'\n')
        self.get_logger().info("tf written successfully")

def main(args=None):
    rclpy.init(args=args)
    node = tf('tfevo')
    rclpy.spin(node)
    node.destroy_node() 
    rclpy.shutdown()

最后调用evo画图：

evo_ape tum ground_truth_odom.txt  tf_laser_frame.txt -va --plot --plot_mode xy

是不是不应该监听tf广播里的laser_frame？应该从哪里提取数据当作算法的位姿呢？

砖头

@siviechen 再附带一个我当时建的图，这个算还可以吗？我感觉形状是对的。左边是入口和厕所门，中间是桌子的桌角和杂物。

怎么laser_frame会那么离谱呢？

小鱼

@siviechen 里程计数据是 odom到 base_link 之间的位姿变换

砖头

@小鱼小鱼的意思是我应该用/odom到/laser_frame的位姿变换和/odom话题消息进行比较吗

小鱼

@siviechen 看你这个像直接跑的carto的纯定位，这样一来你其实对比的是雷达定位和里程计定位的精度，而非地图精度。
因为用docker选2建图其实只用了雷达数据而已

砖头

@小鱼嗯，我只是开启了docker里的雷达建图然后选2，仅此而已，然后提取了数据来做这个对比。这个选2有在建图啊，应该不是纯定位吧？

‘’这样一来你其实对比的是雷达定位和里程计定位的精度，而非地图精度‘’这句话我理解了。

另外想问一下，小鱼是怎么修改carto里的雷达订阅策略的？我按教程写的launch文件，然后跳出了雷达策略不对的日志：

[WARN] [1692271382.825165765] [ydlidar_node]: New subscription discovered on topic '/scan', requesting incompatible QoS. No messages will be sent to it. Last incompatible policy: RELIABILITY_QOS_POLICY

因为carto源码是ros1的，没有qos，我也没找出来怎么改（carto的代码也很有难度）。佩服小鱼的docker里的选2是能跑起来的。能否指教一下？求求有偿也可以，真的很想知道

教程链接：

https://fishros.com/d2lros2/#/humble/chapt10/get_started/3.%E9%85%8D%E7%BD%AEFishBot%E8%BF%9B%E8%A1%8C%E5%BB%BA%E5%9B%BE

砖头

@siviechen 补充一下，我的操作过程有（我没用gazebo仿真环境而是用实车）：
启动microros的Agent：

sudo docker run -it --rm -v /dev:/dev -v /dev/shm:/dev/shm --privileged --net=host microros/micro-ros-agent:$ROS_DISTRO udp4 --port 8888 -v6

驱动雷达按1：

xhost + && sudo docker run  -it --rm  -v /dev:/dev -v /dev/shm:/dev/shm --privileged  -v /tmp/.X11-unix:/tmp/.X11-unix --device /dev/snd -e DISPLAY=unix$DISPLAY -p 8889:8888 registry.cn-hangzhou.aliyuncs.com/fishros/fishbot_laser

启动教程里的launch文件：

ros2 launch fishbot_cartographer cartographer.launch.py

小鱼

@siviechen 在启动fishbot建图程序后应该从哪里提取定位数据？中说：

个选2有在建图啊，应该不是纯定位吧？

2 是纯雷达建图

小鱼

@siviechen 你可以尝试slam_toolbox ，相比carto更好用。

ros2 launch slam_toolbox online_async_launch.py

砖头

@小鱼我是想自己学写一写slam算法的。cartographer我已经把多少看了一遍了，所以我想了解一些cartographer实现方面的问题，比如上面这些。能教一教吗

小鱼

@siviechen 按照教程应该能跑起来，你遇到什么错误了

砖头

@小鱼 emm，是这样的：教程里是教我们下载carto源码，然后改配置文件.lua和launch文件，然后启动gazebo仿真，然后跑cartographer，这样做是没问题的，我的也没问题。

而我现在是把启动gazebo的仿真这一步换成了启动fishbot实车，启动了agent来获取scan，odom和imu数据。（在这同时把launch文件里的'use_sim_time'改成了’false‘，因为不是在跑仿真了）

然后我放一张rviz的图：

和一张rqt_graph的图：

我对其的理解是map没有数据，用ros2 topic echo去试也是没有数据跳出来的

不知道要如何才能像小鱼的雷达docker选2那样跑起来

小鱼

@siviechen 看一下tf,还有frameid对不对

砖头

@小鱼

ros2 run tf2_tools view_frames

输入了这个命令行，好像没有tf的样子

小鱼

@siviechen 再刷新看看，没有就有大问题了

砖头

@小鱼你这么说我突然紧张起来了

我输入这四条（雷达选1），重试了一下

结果仍然是No tf data received

小鱼

@siviechen 用rqt_tf_tree 看

砖头

@小鱼 ros2好像没有这个功能，rqt_tf_tree: command not found

小鱼

@siviechen 有的，搜一下安装下

砖头

@小鱼找到了，贴一下过程：

sudo apt install ros-humble-rqt-tf-tree
rm ~/.config/ros.org/rqt_gui.ini #删除缓存，不然报错
ros2 run rqt_tf_tree rqt_tf_tree

但是结果还是一样的：

会和车的雷达要把fixed frame改成laser_frame有关系吗？默认好像是map