ROS数据展示系统

项目介绍

本项目要求使用松灵小车上录制的bag包数据，编写软件，用命令行窗口显示小车的IMU和里程计（odometry）数据；用图形界面显示颜色相机和深度相机的数据（、用OpenCV库）；用图形界面显示激光雷达的点云数据（利用PCL库）；
自行选择一种高级算法（例如语义分割、三维重建、导航定位（SLAM）等），实现该算法，将其集成到系统中。
rosbag内容

项目实现

前言

在本项目的完成过程中使用了带有ROS的Ubuntu 20.04的docker镜像，名为tiryoh/ros-desktop-vnc:melodic.dockerhub地址。在该镜像中已经安装了OpenCV、PCL库。
该项目的各个功能包均写在一个工作空间内，其中高级算法部分使用了第三方的Gmapping库。
项目源码仓库
项目视频地址

IMU、里程计数据显示

根据数据包内容，数据包重现时，通过/imu/data_raw话题发布sensor_msgs/Imu类型的imu数据，通过/odom话题发布nav_msgs/Odometry类型的里程计数据。只需订阅相关话题接收相关信息，调用数据显示回调函数即可实现功能。在编写的节点程序中，关键步骤在编写输出数据的订阅者回调函数。
根据sensor_msgs/Imu与nav_msgs/Odometry消息的内容
sensor/Imu消息内容
nav_msgs/Odometry内容
编写如下回调函数，订阅相关话题时调用即可在命令行显示imu、里程计数据。
Imu数据显示回调函数：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
void callbacki(const sensor_msgs::Imu::ConstPtr &a){
    std::cout<<"std_msgs/Header header"<<std::endl;
    std::cout<<"uint32 seq: "<<a->header.seq<<std::endl;
    std::cout<<"time stamp: "<<a->header.stamp<<std::endl;
    std::cout<<"frame_id: "<<a->header.frame_id<<std::endl;
    std::cout<<"orientation: "<<std::endl;
    std::cout<<"float64 x: "<<a->orientation.x<<std::endl;
    std::cout<<"float64 y: "<<a->orientation.y<<std::endl;
    std::cout<<"float64 z: "<<a->orientation.z<<std::endl;
    std::cout<<"float64 w: "<<a->orientation.w<<std::endl;
    for(int i=0;i<9;i++){
        std::cout<<"orientation_covariance: "<<a->orientation_covariance[i]<<std::endl;
    }
    std::cout<<"angular_velocity: "<<std::endl;
    std::cout<<"float64 x: "<<a->angular_velocity.x<<std::endl;
    std::cout<<"float64 y: "<<a->angular_velocity.y<<std::endl;
    std::cout<<"float64 z: "<<a->angular_velocity.z<<std::endl;
    for(int i=0;i<9;i++){
        std::cout<<"angular_velocity_covariance: "<<a->angular_velocity_covariance[i]<<std::endl;
    }
    std::cout<<"linear_acceleration: "<<std::endl;
    std::cout<<"float64 x: "<<a->linear_acceleration.x<<std::endl;
    std::cout<<"float64 y: "<<a->linear_acceleration.y<<std::endl;
    std::cout<<"float64 z: "<<a->linear_acceleration.z<<std::endl;
    for(int i=0;i<9;i++){
        std::cout<<"linear_acceleration_covariance: "<<a->linear_acceleration_covariance[i]<<std::endl;
    }

    std::cout<<"                 end                 "<<std::endl;
    return ;

}
里程计数据显示回调函数：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
void callbacki(const nav_msgs::Odometry::ConstPtr& a){
    std::cout << "std_msgs/Header header" << std::endl;
    std::cout << "uint32 seq: " << a->header.seq << std::endl;
    std::cout << "time stamp: " << a->header.stamp << std::endl;
    std::cout << "frame_id: " << a->header.frame_id << std::endl;
    std::cout << "child_frame_id: " << a->child_frame_id << std::endl;
    std::cout << "geometry_msgs/PoseWithCovariance pose" << std::endl;
    std::cout << "geometry_msgs/Pose pose" << std::endl;
    std::cout << "geometry_msgs/Point position" << std::endl;
    std::cout << "float64 x: " << a->pose.pose.position.x << std::endl;
    std::cout << "float64 y: " << a->pose.pose.position.y << std::endl;
    std::cout << "float64 z: " << a->pose.pose.position.z << std::endl;
    std::cout << "geometry_msgs/Quaternion orientation" << std::endl;
    std::cout << "float64 x: " << a->pose.pose.orientation.x << std::endl;
    std::cout << "float64 y: " << a->pose.pose.orientation.y << std::endl;
    std::cout << "float64 z: " << a->pose.pose.orientation.z << std::endl;
    std::cout << "float64 w: " << a->pose.pose.orientation.w << std::endl;
    std::cout << "covariance: " << std::endl;
    for (int i = 0; i < 36; ++i) {
        std::cout << a->pose.covariance[i] << " ";
    }
    std::cout<<std::endl;
    std::cout << "geometry_msgs/TwistWithCovariance twist" << std::endl;
    std::cout << "geometry_msgs/Twist twist" << std::endl;
    std::cout << "geometry_msgs/Vector3 linear" << std::endl;
    std::cout << "float64 x: " << a->twist.twist.linear.x << std::endl;
    std::cout << "float64 y: " << a->twist.twist.linear.y << std::endl;
    std::cout << "float64 z: " << a->twist.twist.linear.z << std::endl;
    std::cout << "geometry_msgs/Vector3 angular" << std::endl;
    std::cout << "float64 x: " << a->twist.twist.angular.x << std::endl;
    std::cout << "float64 y: " << a->twist.twist.angular.y << std::endl;
    std::cout << "float64 z: " << a->twist.twist.angular.z << std::endl;
    std::cout << "covariance: " << std::endl;
    for (int i = 0; i < 36; ++i) {
        std::cout << a->twist.covariance[i] << " ";
    }
    std::cout<<std::endl;
    std::cout << "                  end               " << std::endl;
    return ;

}

里程计数据显示效果
Imu数据显示效果

点云数据显示

由数据包内容可知，数据包通过/rslidar_points话题发布sensor_msgs/PointCloud2类型的点云数据消息，在节点代码中，订阅/rslidar_points话题，使用PCL库，将将接收到的sensor_msgs::PointCloud2类型的消息转换为PCL库中的pcl::PCLPointCloud2类型。然后，使用pcl::fromPCLPointCloud2函数将pcl::PCLPointCloud2类型转换为pcl::PointCloudpcl::PointXYZ类型的点云数据。创建一个CloudViewer对象，利用该对象显示点云数据，即可实现功能。
关键步骤在于订阅者程序的回调函数。

1
2
3
4
5
6
7
8
9
10
void callbacki(const sensor_msgs::PointCloud2::ConstPtr &a){
        pcl::PointCloud<pcl::PointXYZ> cloud;
        pcl::PCLPointCloud2 pcl_pc;
        pcl_conversions::toPCL(*a, pcl_pc);
        pcl::fromPCLPointCloud2(pcl_pc, cloud);
        viewer.showCloud(cloud.makeShared());
        // viewer_timer = nh.createTimer(ros::Duration(0.1), &cloudHandler::timerCB, this);

        return ;
}
编写如上函数在订阅/rslidar_points话题时调用即可实现功能。

点云显示效果

深度相机与颜色相机数据显示

由数据包内容知，数据包重现时通过/camera/color/image_raw与/camera/depth/image_rect_raw话题发布颜色相机与深度相机数据信息消息。

定义如下ImageConverter类：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
class ImageConverter
{
    ros::NodeHandle nh_;
    image_transport::ImageTransport it_;
    image_transport::Subscriber image_sub_color;
    image_transport::Subscriber image_sub_depth;
    image_transport::Publisher image_pub_color;
    image_transport::Publisher image_pub_depth;
 
public:
    ImageConverter()
        : it_(nh_)
    {
        
        image_sub_color = it_.subscribe("/camera/color/image_raw", 1, &ImageConverter::imageCb, this);
        image_sub_depth = it_.subscribe("/camera/depth/image_rect_raw", 1, &ImageConverter::depthCb, this);
        image_pub_color = it_.advertise("/image_converter/output_video/color", 1);
        image_pub_depth = it_.advertise("/image_converter/output_video/depth", 1);

        cv::namedWindow(OPENCV_WINDOW_COLOR);
        cv::namedWindow(OPENCV_WINDOW_DEPTH);
        
    }
 
    ~ImageConverter()
    {
        cv::destroyWindow(OPENCV_WINDOW_COLOR);
        cv::destroyWindow(OPENCV_WINDOW_DEPTH);

        
    }

    void imageCb(const sensor_msgs::ImageConstPtr &msg)
    {
        cv_bridge::CvImagePtr cv_ptr;
        try
        {
            cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
        }
        catch (cv_bridge::Exception &eee)
        {
            ROS_ERROR("cv_bridge exception: %s", eee.what());
            return;
        }
 
        
        if (cv_ptr->image.rows > 60 && cv_ptr->image.cols > 60)
        {
            cv::circle(cv_ptr->image, cv::Point(50, 50), 10, CV_RGB(255, 0, 0));
        }

        cv::imshow(OPENCV_WINDOW_COLOR, cv_ptr->image);
        cv::waitKey(3);
 

        image_pub_color.publish(cv_ptr->toImageMsg());
    }
 
    void depthCb(const sensor_msgs::ImageConstPtr &msg)
    {
        cv_bridge::CvImagePtr cv_ptr;
        try
        {
            cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::TYPE_16UC1);
        }
        catch (cv_bridge::Exception &e)
        {
            ROS_ERROR("cv_bridge exception: %s", e.what());
            return;
        }
 

        if (cv_ptr->image.rows > 60 && cv_ptr->image.cols > 60)
            cv::circle(cv_ptr->image, cv::Point(50, 50), 10, CV_RGB(255, 0, 0));
 

        cv::imshow(OPENCV_WINDOW_DEPTH, cv_ptr->image);
        cv::waitKey(3);
 
    
        image_pub_depth.publish(cv_ptr->toImageMsg());
    }
};

在主函数中创建该类的一个对象即可实现功能。该类包括一个ROS节点句柄对象、一个图像数据传输对象、四个图像传输相关的成员变量。利用构造函数初始化ROS节点句柄对象与图像传输对象，创建四个图像传输相关的订阅者和发布者。当接受到彩色图像数据时调用imageCb函数处理接收到的消息，将ROS消息转换为OpenCV的CvImagePtr类型，后将图像显示在OpenCV窗口中，并发布处理后的彩色图像消息。当接受到深度图像数据时调用depthCb函数处理接收到的消息，同样的将ROS消息转换为OpenCV的CvImagePtr类型，将图像显示在OpenCV窗口中，发布处理后的彩色图像消息。
关键步骤在于imageCb函数与depthCb函数的编写。

实现效果：

深度相机与颜色相机效果

Slam建图显示

在该功能中，使用了第三方开源的Gmapping库，利用该库订阅数据包发布的/scan、/odom话题，接收雷达信息与里程计信息，进行坐标转换，发布/map话题。最后利用OpenCV库绘制出/map话题对应地图数据即可实现Slam建图功能。
使用Gmapping库时需要注意先将slam_gmapping节点源码内的订阅者、发布者队列相应增大1000倍。经过实际操作，由于数据包量大、发布速度快，源码初始的队列长度会导致丢包率过大的问题。并且开启slam_gmapping节点时需注意让rosbag停留长于默认0.2s的时间保证slam_grmapping节点可以成功接收相关话题消息。在rosbag play后添加-d选项实现暂停。
在显示图像节点源码中编写如下回调函数：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
cv::Mat map;

void mapCallback(const nav_msgs::OccupancyGrid::ConstPtr& msg)
{

    double scale = 1.0;
    int width = 1200;
    int height = 1200;
    cv::Point offset = {-1600, -1600};
    map = cv::Mat::zeros(cv::Size(width, height), CV_8UC3);

    for (int i = 0; i < msg->info.width * msg->info.height; ++i) {
        int x = (i % msg->info.width + offset.x) * scale;
        int y = (i / msg->info.width + offset.y) * scale;
        if (msg->data[i] == -1) {
            cv::circle(map, cv::Point(x, y), 1, cv::Scalar(255, 255, 255), -1);
        } else if (msg->data[i] >= 80) {
            cv::circle(map, cv::Point(x, y), 3, cv::Scalar(0, 0, 0), -1);
        } else {
            cv::circle(map, cv::Point(x, y), 3, cv::Scalar(64, 64, 64), -1);
        }
    }

    cv::imshow("My Slam Map", map);
    cv::waitKey(1000);
}

收到/map话题的消息时调用该函数。定义一个cv::Mat类型的变量map，用于存储生成的地图图像。mapCallback函数接收nav_msgs::OccupancyGrid类型的指针作为输入参数。设置地图缩放比例为1.0，宽高默认1200，地图坐标的偏移量默认为{-1600, -1600}。使用cv::Mat::zeros函数创建一个大小为width×height的黑色图像作为地图。使用一个循环遍历msg->data数组中的每个元素，根据元素的值来决定在地图上画圆的位置和颜色：计算当前元素对应的坐标x和y，根据地图的缩放比例和偏移量进行计算。如果元素的值为-1，表示未知区域，使用白色画一个半径为1的圆。如果元素的值大于等于80，表示占据区域，使用黑色画一个半径为3的圆。否则，表示自由区域，使用灰色画一个半径为3的圆。将生成的地图图像显示在名为”My Slam Map”的OpenCV窗口中，并等待1000毫秒。
实现效果：
Slam建图效果

后记

由衷感谢两位任课老师在软件设计课程中的辛勤奉献！
鸽了好久…之前的自平衡桌面助手仍在完善，后续更新…

Linux ROS Ubuntu
下一篇:
霍尔编码器的使用方法

    • Copyright © 2023 Lloydttt | Powered by Hexo | Theme Bamboo

    本站总访问量： 次 |