[ROS学习]Euroc数据集真实轨迹的读取与显示

Euroc数据集真实轨迹的读取与显示

1. Euroc数据集真实轨迹数据(csv格式)的读取

1.1 Euroc数据集

Euroc数据集是用于室内MAV的双目+IMU数据集，包含两个场景

（1）机器仓库

（2）普通房间

硬件设备

飞行器机体：AscTec Firefly
双目VIO相机：全局快门，单色，相机频率20Hz，IMU频率200Hz，具备相机和IMU的硬件(hw)同步，双目相机型号MT9V034，IMU型号ADIS16448
VICON0：维肯动作捕捉系统的配套反射标志，叫做marker
LEICA0：是激光追踪器配套的传感器棱镜，叫做prism
Leica Nova MS50: 激光追踪器，测量棱镜prism的位置，毫米精度，帧率20Hz，
Vicon motion capture system: 维肯动作捕捉系统，提供在单一坐标系下的6D位姿测量，测量方式是通过在MAV上贴上一组反射标志，帧率100Hz，毫米精度

关于坐标系变换可参考文献[1]，或者数据集官方页面

1.2 Euroc真实轨迹 groundtruth的格式数据频率为200hz

(1)18位时间戳

timestamp，最小单位为纳秒

(2)当前MAV的三维空间坐标[position]

，R代表该信息是由Leica Nova MS50(激光追踪器)的信息在R坐标系下的值。

S表示Sensor传感器坐标下的信息，因此RS_R表示由本来的信息在S坐标系下，后变换到了R坐标系。

单位[m]

p_RS_R_x
p_RS_R_y
p_RS_R_z

(3)当前位姿[quaternion形式]

其中w为四元数的实部，xyz为三个虚部

q_RS_w
q_RS_x
q_RS_y
q_RS_z

(4)当前速度

，单位[m/s]

v_RS_R_x
v_RS_R_y
v_RS_R_z

(5)当前角速度

，单位[rad/s]

b_w_RS_S_x
b_w_RS_S_y
b_w_RS_S_z

(6)当前线速度

，单位[m/s]

b_a_RS_S_x
b_a_RS_S_y
b_a_RS_S_z

1.3 csv数据的读取

数据读取基础

文件指针


// 打开路径path下的文件
std::ifstream data(path, std::ios::in);

文件指针打开失败判断

if(!data.is_open())
{
    std::cout << "Error: opening file fail" << std::endl;
    std::exit(1);
}

将字符串读入流

// 创建字符串流类型
std::istringstream sin;
// 将字符串line读入流
sin.str(line);

文件数据读取

//直接读取：读取文件指针data指向的文件中的一行以Sring类型的line中
std::getline(data, line);
// 间接读取：将字符串流sin中的字符读入到field字符串中，以逗号为分隔符
std::getline(sin, field, ',')

读取流程(代码设计)

以读取
groundtruth
中的时间戳与位置信息为例设计一个demo测试是否可行

#include <iostream>
#include <istream>
#include <streambuf>
#include <fstream>
#include <sstream>
#include <vector>
#include <string>
#include <stdlib.h>
#include <map>
// 创建数据结构体类型
struct ground_position
{
    double timestamp;
    double x;
    double y;
    double z;
};
int main() 
{
    std::ifstream data("${Your_path}/data.csv", std::ios::in);
    std::string line;
    std::getline(data, line);

    if(!data.is_open())
    {
        std::cout << "Error: opening file fail" << std::endl;
        std::exit(1);
    }
    std::vector<ground_position> data_re;
    std::vector<std::string> fields; //声明一个字符串向量
    std::string field;
    // 读取数据
    // 读取标题行
    std::getline(data, line);
	std::istringstream sin;
    // 按行读取数据
    while(std::getline(data, line)){
        // 清除已读取的，只看当行
        fields.clear();
        sin.clear();
        sin.str(line);

        while (std::getline(sin, field, ',')) //将字符串流sin中的字符读入到field字符串中，以逗号为分隔符
            fields.push_back(field); //将刚刚读取的字符串添加到向量fields中
        ground_position c;
        c.timestamp = std::stod(fields[0]);
        c.x = std::stod(fields[1]);
        c.y = std::stod(fields[2]);
        c.z = std::stod(fields[3]);
        data_re.push_back(c);
    }
    for (size_t i = 0; i < data_re.size(); i++)
    {
        std::cout<<"id: "<<i<<"  timestamp:"<<data_re[i].timestamp<<
                "  x:"<<data_re[i].x<<"  y:"<<data_re[i].y<<"  z:"<<data_re[i].z<<std::endl;
    }
    return 0;
}

结果

在这里插入图片描述

1.4 ros中读取csv并发布

创建一个基本的ros功能包，需要包含组件
std_msgs
、
roscpp

需要自定义设计
rosmsg
消息类型
ground_truth

float64 timestamp
float32 p_RS_R_x
float32 p_RS_R_y
float32 p_RS_R_z
float32 q_RS_R_w
float32 q_RS_R_x
float32 q_RS_R_y
float32 q_RS_R_z
float32 v_RS_R_x
float32 v_RS_R_y
float32 v_RS_R_z
float32 b_w_RS_S_x
float32 b_w_RS_S_y
float32 b_w_RS_S_z
float32 b_a_RS_S_x
float32 b_a_RS_S_y
float32 b_a_RS_S_z

消息类型的设计可参考赵虚左老师的ros教程，文献[2]

// 1.包含头文件 
#include "ros/ros.h"
#include "std_msgs/String.h" //普通文本类型的消息
#include <iostream>
#include <istream>
#include <streambuf>
#include <fstream>
#include <sstream>
#include <vector>
#include <string>
#include <stdlib.h>
#include "/home/yanbo/rosworkspace/devel/include/get_ground_truth/ground_truth.h"

// 将1.3的读取程序封装为函数
std::vector<get_ground_truth::ground_truth> readcsvdata(std::string path)
{
    std::ifstream data(path, std::ios::in);
    std::string line;
    std::getline(data, line);

    if(!data.is_open())
    {
        std::cout << "Error: opening file fail" << std::endl;
        std::exit(1);
    }
    std::vector<get_ground_truth::ground_truth> data_re;
    std::vector<std::string> fields; //声明一个字符串向量
    std::string field;
    // 读取数据
    // 读取标题行
    std::getline(data, line);
	std::istringstream sin;
    // 按行读取数据
    while(std::getline(data, line)){
        // 清除已读取的，只看当行
        fields.clear();
        sin.clear();
        sin.str(line);

        while (std::getline(sin, field, ',')) //将字符串流sin中的字符读入到field字符串中，以逗号为分隔符
        fields.push_back(field); //将刚刚读取的字符串添加到向量fields中
        
        get_ground_truth::ground_truth c;
        c.timestamp = std::stod(fields[0]);
        c.p_RS_R_x = std::stod(fields[1]);
        c.p_RS_R_y = std::stod(fields[2]);
        c.p_RS_R_z = std::stod(fields[3]);

        c.q_RS_R_w = std::stod(fields[4]);
        c.q_RS_R_x = std::stod(fields[5]);
        c.q_RS_R_y = std::stod(fields[6]);
        c.q_RS_R_z = std::stod(fields[7]);

        c.v_RS_R_x = std::stod(fields[8]);
        c.v_RS_R_y = std::stod(fields[9]);
        c.v_RS_R_z = std::stod(fields[10]);

        c.b_w_RS_S_x = std::stod(fields[11]);
        c.b_w_RS_S_y = std::stod(fields[12]);
        c.b_w_RS_S_z = std::stod(fields[13]);

        c.b_a_RS_S_x = std::stod(fields[14]);
        c.b_a_RS_S_y = std::stod(fields[15]);
        c.b_a_RS_S_z = std::stod(fields[16]);

        data_re.push_back(c);
    }
    return data_re;
}
int main(int argc, char  *argv[])
{   
    //设置编码
    setlocale(LC_ALL,"");

    //2.初始化 ROS 节点:命名(唯一)
    // 参数1和参数2 后期为节点传值会使用
    // 参数3 是节点名称，是一个标识符，需要保证运行后，在 ROS 网络拓扑中唯一
    ros::init(argc,argv,"pub_ground_truth_data");
    //3.实例化 ROS 句柄
    
    ros::NodeHandle nh;//该类封装了 ROS 中的一些常用功能

    //4.实例化 发布者 对象
    //泛型: 发布的消息类型
    //参数1: 要发布到的话题
    //参数2: 队列中最大保存的消息数，超出此阀值时，先进的先销毁(时间早的先销毁)
    ros::Publisher pub = nh.advertise<get_ground_truth::ground_truth>("chatter",10);

    //5.组织被发布的数据，并编写逻辑发布数据
    //数据(动态组织)
	// 创建数据读取对象
    std::vector<get_ground_truth::ground_truth> data_raw;
    data_raw = readcsvdata("${Your_path}/data.csv");
    int count = 0; //消息计数器

    //逻辑(一秒10次)
    // 参数 freq
    ros::Rate r(100);

    // 保持节点生存周期
    while (ros::ok())
    {
        //节点终止条件
        if(count >= data_raw.size()) break;
        //发布消息
        pub.publish(data_raw[count]);
        //加入调试，打印发送的消息
        ROS_INFO("已发送的id: %d, timestamp: %.2f, x: %f, y: %f, z: %f",
                                            count,data_raw[count].timestamp,
                                            data_raw[count].p_RS_R_x,
                                            data_raw[count].p_RS_R_y,
                                            data_raw[count].p_RS_R_z);
        //根据前面制定的发送频率自动休眠 休眠时间 = 1/频率；
        r.sleep();
        count++;//循环结束前，让 count 自增
        ros::spinOnce();
    }
    return 0;
}

2 真实轨迹数据在rviz中的显示

2.1 数据显示在rviz中的逻辑

使轨迹数据(坐标+位姿)在rviz中显示的关键是将(坐标+位姿)的数据以rviz可以订阅的消息类型进行发布。

ROS的数据在rviz中显示的接口主要在
geometry_msgs
与
nav_msgs
中

其中
nav_msgs::path
可以显示轨迹路径

// 轨迹的发布
ros::Publisher path_pub = ph.advertise<nav_msgs::Path>("trajectory", 1, true);
// 获取当前时间戳
ros::Time current_time, last_time;
current_time = ros::Time::now();
last_time = ros::Time::now();
// 创建rviz数据对象
nav_msgs::Path path;
//nav_msgs::Path path;
path.header.stamp = current_time;
// 指定数据帧的 标头
path.header.frame_id = "odom";

// 创建rviz几何发布对象
geometry_msgs::PoseStamped this_pose_stamped;
// 创建rviz几何发布对象
geometry_msgs::PoseStamped this_pose_stamped;
// 轨迹信息
this_pose_stamped.pose.position.x = data_raw[count].p_RS_R_x;
this_pose_stamped.pose.position.y = data_raw[count].p_RS_R_y;
this_pose_stamped.pose.position.z = data_raw[count].p_RS_R_z;
// 四元数的四个信息一个实部、三个虚部
this_pose_stamped.pose.orientation.x = data_raw[count].q_RS_R_x;
this_pose_stamped.pose.orientation.y = data_raw[count].q_RS_R_y;
this_pose_stamped.pose.orientation.z = data_raw[count].q_RS_R_z;
this_pose_stamped.pose.orientation.w = data_raw[count].q_RS_R_w;

this_pose_stamped.header.stamp = current_time;
this_pose_stamped.header.frame_id = "odom";
path.poses.push_back(this_pose_stamped);
// 发布
path_pub.publish(path);

geometry_msgs
可以显示点的位置

// 点位置发布
ros::Publisher point_pub = ph.advertise<geometry_msgs::PointStamped>("point", 1, true);
// 获取当前时间戳
ros::Time current_time, last_time;
current_time = ros::Time::now();
last_time = ros::Time::now();

// 创建rviz几何发布对象
geometry_msgs::PointStamped this_point_stamped;
//点信息
this_point_stamped.header.stamp = current_time;
this_point_stamped.header.frame_id = "odom";
this_point_stamped.point.x = data_raw[count].p_RS_R_x;
this_point_stamped.point.y = data_raw[count].p_RS_R_y;
this_point_stamped.point.z = data_raw[count].p_RS_R_z;
// 发布
point_pub.publish(this_point_stamped);

2.2 整体程序设计

整体的程序设计分为三个部分：

csv数据读取
创建发布对象、整合发布数据
数据发布

部分库与csv读取函数参考上文

#include <ros/ros.h>
#include <ros/console.h>
#include <nav_msgs/Path.h>
#include <std_msgs/String.h>
#include <geometry_msgs/Quaternion.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PointStamped.h>
#include <tf/transform_broadcaster.h>
#include <tf/tf.h>
#include <cstdlib>
int main(int argc, char **argv)
{
    ros::init(argc, argv, "my_trace");
    // 从数据集的state_groundtruth_estimate0读取真实轨迹
    std::vector<get_ground_truth::ground_truth> data_raw;
    data_raw = readcsvdata("${Your_path}/data.csv");

    // 创建发布对象
    ros::NodeHandle ph;
    // 轨迹的发布
    ros::Publisher path_pub = ph.advertise<nav_msgs::Path>("trajectory", 1, true);
    // 点位置发布
    ros::Publisher point_pub = ph.advertise<geometry_msgs::PointStamped>("point", 1, true);

    // 获取当前时间戳
    ros::Time current_time, last_time;

    current_time = ros::Time::now();
    last_time = ros::Time::now();

    // 创建rviz数据对象
    nav_msgs::Path path;
    //nav_msgs::Path path;
    path.header.stamp = current_time;
    // 指定数据帧的 标头
    path.header.frame_id = "odom";


    // 计数,当count达到数据的最大id时结束发布进程
    int32_t count = 0;
    // 设置数据发送频率
    ros::Rate loop_rate(100);
    while (ros::ok())
    {   
        // 是否结束发布进程
        if(count >= data_raw.size()) break;

        current_time = ros::Time::now();

        // 创建rviz几何发布对象
        geometry_msgs::PoseStamped this_pose_stamped;
        geometry_msgs::PointStamped this_point_stamped;

        this_pose_stamped.pose.position.x = data_raw[count].p_RS_R_x;
        this_pose_stamped.pose.position.y = data_raw[count].p_RS_R_y;
        this_pose_stamped.pose.position.z = data_raw[count].p_RS_R_z;

        this_point_stamped.header.stamp = current_time;
        this_point_stamped.header.frame_id = "odom";
        this_point_stamped.point.x = data_raw[count].p_RS_R_x;
        this_point_stamped.point.y = data_raw[count].p_RS_R_y;
        this_point_stamped.point.z = data_raw[count].p_RS_R_z;

        ROS_INFO("current_x: %f", data_raw[count].p_RS_R_x);
        ROS_INFO("current_y: %f", data_raw[count].p_RS_R_y);

        this_pose_stamped.pose.orientation.x = data_raw[count].q_RS_R_x;
        this_pose_stamped.pose.orientation.y = data_raw[count].q_RS_R_y;
        this_pose_stamped.pose.orientation.z = data_raw[count].q_RS_R_z;
        this_pose_stamped.pose.orientation.w = data_raw[count].q_RS_R_w;

        this_pose_stamped.header.stamp = current_time;
        this_pose_stamped.header.frame_id = "odom";
        path.poses.push_back(this_pose_stamped);
        path_pub.publish(path);
        point_pub.publish(this_point_stamped);
        count++;
        ros::spinOnce(); // check for incoming messages
        last_time = current_time;
        loop_rate.sleep();
    }
    return 0;
}

需要注意的点是创建功能包时需要导入如下几个功能包

nav_msgs
roscpp
sensor_msgs
std_msgs
tf

2.2 rviz中的设置

在终端中输入rviz打开rviz的GUI

(1)修改
Global Option
中的
Fixed Frame
由
map
改为
odom
具体看轨迹发布程序中
frame id
设置的是什么，本文这里设置是
odom

在这里插入图片描述

(2)在下方的 add 选项中的如下两项添加至主界面

在这里插入图片描述

(3)运行 2.1 写的轨迹发布程序，分别将图中的 path->topic与PointStamped->topic设置为图中所示。注意，只有发布程序运行才能会出现可选的选项。该选项名称可自由设置。

在这里插入图片描述

end！！！

参考

[1]

SLAM_数据集_EuRoC数据集简介与使用

[收费了建议看官方的吧]

[2]

ros理论与实践

[3]

ROS在rviz中实时显示轨迹和点

[4]

euroc数据集官方页面

原文链接：https://blog.csdn.net/qq_43256088/article/details/129180047