【ros】结果实时在线可视化

读书猿

发布于 2024-04-14 09:20:00

850

发布于 2024-04-14 09:20:00

文章被收录于专栏：无人驾驶感知无人驾驶感知

一、前言

感知与规划控制是无人驾驶算法重要算法，在交付测试阶段也最容易引起摩擦，这也是司空见惯的现象。有时候可能是接口对齐问题、有时候可能是版本管理问题、有时候可能是第三方如云平台问题、有时候车子硬件自身问题。
当然有时候也可能是感知算法与规控算法自身问题，所以拥有"自证清白"的能力是十分必要的。比如今天博主分享的感知结果实时可视化。具体实现可见下文。
ros 订阅 perception 结果进行在线可视化。发布 sensor_msgs::Image 结果，通过 rviz 实时在线观察结果。这里包含了 ros 订阅话题、回调函数、发布消息、图片绘制结果等。

二、订阅与发布

基于 ros 开发，我们通讯基本上是靠发布 topic 与订阅 topic 实现。ros 底层设计已经非常成熟，也获得我们开发人员们一致认可，我们直接拿来用。
由于大家的消息接口并不一致，涉及到具体业务接口这里不方便提供。不过整体逻辑与思路还是比较清晰。
这里我举个例子，比如感知发布多个 topic，我们要绘制结果就需要订阅所有 topic，这里我只举出两个 topic 的例子。

首先应该是读取配置文件，比如我们常说的.yaml 文件，这里可以包含一些传感器标定信息与各自算法的一些初始化参数配置等。

这里只提供示例代码，全手敲，可能有误。读取配置文件获取两个需要订阅的topic，各自订阅回调。

void RosNode::Run(int argc, char **argv)
{
    YAML::Node pYaml = YAML::Node YAML::LoadFile(yaml); // 读取配置文件
    ros::NodeHandle  mRosHandle; // ros handle
    
    std::string subscribeTopic1 = pYaml["node1"]["publish_topic"].as<std::string>(); // 订阅topic1
    // 队列里全设置1，确保实时性
    ros::Subscriber subscriber1 = mRosHandle.subscribe(subscribeTopic1, 1, &RosNode::Topic1Callback, this);
    
    std::string subscribeTopic2 = pYaml["node2"]["publish_topic"].as<std::string>(); // 订阅topic2
    ros::Subscriber subscriber2 = mRosHandle.subscribe(subscribeTopic2, 1, &RosNode::Topic2Callback, this);
    
    std::string publishTopic = pYaml["result"]["publish_topic"].as<std::string>(); // 发布的topic
    ros::Publisher mRosPublish = mRosHandle.advertise<sensor_msgs::Image>(publishTopic, 1);
    
    ros::spin(); // 进入循环
    
    // 手动释放资源
    subscriber1.shutdown();
    subscriber2.shutdown();
    mRosPublish.shutdown();
}

三、回调

订阅一个 topic 时不必多说，当订阅多个 topic 时要注意两点

以某个回调发布消息为主，或者设置ros::Time。明确规定去发布消息，最好别冲突。
时间戳是否需要对齐，队列是否需要保留消息。

我们可以自己定义一个 .msg 文件，通过 ros 编译可以生成一个 .h 文件。

“示例代码”：

假设 topic1 输出的是感知检测障碍物，topic2 输出的是感知检测红绿灯。

viewResult 为我们定义的一个类，具体内容下面讲。

// 处理目标的回调
void RosNode::Topic1Callback(const Result::Objects::ConstPtr &message)
{
    viewResult.DrawObjectsResult(cv::Scalar(0, 255, 0), message);
    PublishMessage(viewResult.GetViewImage());
    viewResult.InitImage();
}

void RosNode::Topic2Callback(const Result::Lights::ConstPtr &message)
{
    viewResult.DrawLightsResult(message);
}

// 发布结果信息
void RosNode::PublishMessage(cv::Mat image)
{
    sensor_msgs::ImagePtr publishmessage;
    publishmessage = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image).toImageMsg();
    mRosPublish.publish(publishmessage);
}

四、可视化

rviz 可以直接查看 image 与 cloudPoint。那么可视化可以通过图片直观展示，自然是在 xy 平面下（俯视图）。
画出自身车子，画出感知所有结果，这里只举障碍物与红绿灯的例子。背景图用栅格划分区分距离。

4.1、初始化参数

void ViewResult::Init(std::shared_ptr<YAML::Node> parm)
{
    viewImage.create(imageHeight, imageWidth, CV_8UC3);
    YAML::Node perceptionConfig = *parm->GetYaml();
    InitImage();
}

4.2、初始化图片

画栅格图与 x、y 轴。

    // 绘制水平和垂直线条  
    for (int i = 0; i <= imageHeight; i += gridSpacing) {  
        cv::line(viewImage, cv::Point(0, i), cv::Point(imageWidth, i), gridColor, gridLineWidth);  
    }  
    for (int i = 0; i <= imageWidth; i += gridSpacing) {  
        cv::line(viewImage, cv::Point(i, 0), cv::Point(i, imageHeight), gridColor, gridLineWidth);  
    }

    // 设置栅格说明文本的内容和位置  
    std::string gridInfoText = "A grid represents 10m";  
    cv::Point textPosition(10, imageHeight - 10);  // 放置在图像底部附近  
    cv::putText(viewImage, gridInfoText, textPosition, cv::FONT_HERSHEY_SIMPLEX, 0.5, gridColor, 1, cv::LINE_AA);
    
    int centerX = imageWidth / 2;  
    int centerY = imageHeight / 2;  
    int arrowSize = 100;
    // 绘制X轴
    cv::line(viewImage, cv::Point(0, centerY), cv::Point(imageWidth, centerY), arrowColor, gridLineWidth); 
    cv::Point startPoint(0 + arrowSize, centerY);  
    cv::Point endPoint(0, centerY);  
    cv::arrowedLine(viewImage, startPoint, endPoint, arrowColor, gridLineWidth);    // 绘制X轴箭头  
    // 绘制Y轴 
    cv::line(viewImage, cv::Point(centerX, 0), cv::Point(centerX, imageHeight), arrowColor, gridLineWidth);  
    startPoint = cv::Point(centerX, imageHeight - arrowSize);  
    endPoint = cv::Point(centerX, imageHeight);  
    cv::arrowedLine(viewImage, startPoint, endPoint, arrowColor, gridLineWidth);  
  
    // 添加文字说明  
    float fontScale = 0.5;  
    cv::Point xTextPosition(20, centerY + 20);  
    cv::putText(viewImage, "X", xTextPosition, cv::FONT_HERSHEY_SIMPLEX, fontScale, gridColor, gridLineWidth, cv::LINE_AA);  
    // Y轴文字（注意：Y轴文字需要翻转，因为Y轴是垂直的）  
    cv::Size text_size = cv::getTextSize("Y", cv::FONT_HERSHEY_SIMPLEX, fontScale, gridLineWidth, 0);  
    cv::Point yTextPosition(centerX + 10, imageHeight - 10);  
    cv::putText(viewImage, "Y", yTextPosition, cv::FONT_HERSHEY_SIMPLEX, fontScale, gridColor, gridLineWidth, cv::LINE_AA);

其实最开始构图的时候我是先用 python 写的(方便调试)。

import cv2
import numpy as np

# 设置栅格的大小和数量
grid_size = 240  # 每个栅格的像素大小
grid_color = (0, 0, 0)
arrow_color = (0, 0, 255)

# 创建一个空白的图像，大小为栅格数量乘以栅格大小
image_height = grid_size * 4
image_width = grid_size * 8
image = np.zeros((image_height, image_width, 3), dtype=np.uint8) + 255  # 创建一个白色背景的图像
line_width = 1

# 绘制栅格线
for i in range(0, image_height, grid_size):
    cv2.line(image, (0, i), (image_width, i), grid_color, 1)  # 绘制水平线
for j in range(0, image_width, grid_size):
    cv2.line(image, (j, 0), (j, image_height), grid_color, 1)  # 绘制垂直线

# 设置栅格说明文本的内容和位置
grid_info_text = "A grid represents 10m"
text_position = (10, image_height - 10)  # 放置在图像底部附近

# 在图像上添加栅格说明文本
cv2.putText(image, grid_info_text, text_position, cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 0), 1, cv2.LINE_AA)

# 计算图像的中心点，用于绘制X轴和Y轴
center_x = image.shape[1] // 2
center_y = image.shape[0] // 2

arrow_size = 100
# 绘制X轴
cv2.line(image, (0, center_y), (image.shape[1], center_y), arrow_color, line_width)

# 绘制X轴箭头
cv2.arrowedLine(image, (0 + arrow_size, center_y), (0, center_y), arrow_color, line_width)

# 绘制Y轴
cv2.line(image, (center_x, 0), (center_x, image.shape[0]), arrow_color, line_width)

# 绘制Y轴箭头（注意：OpenCV的arrowedLine默认是向下或向右的箭头，所以我们需要调整起点和终点来得到向上的箭头）
cv2.arrowedLine(image, (center_x, image.shape[0] - arrow_size), (center_x, image.shape[0]), arrow_color, line_width)

# 添加文字说明
font = cv2.FONT_HERSHEY_SIMPLEX
font_scale = 0.5
font_color = (0, 0, 0)
thickness = 1
# X轴文字
cv2.putText(image, 'X', (20, center_y + 20), font, font_scale, font_color, thickness, cv2.LINE_AA)

# Y轴文字（注意：Y轴文字需要翻转，因为Y轴是垂直的）
text_size, _ = cv2.getTextSize('Y', font, font_scale, thickness)
cv2.putText(image, 'Y', (center_x + 10, image.shape[0] - 10), font, font_scale, font_color, thickness, cv2.LINE_AA)

center_coordinates = (200, 200)  # (x, y) 坐标，位于图像中心
radius = 10  # 半径为20像素

# 参数：图像，中心坐标，半径，颜色，线宽（负值表示填充）
cv2.circle(image, center_coordinates, radius, (255, 0, 0), -1)

# 显示图像
cv2.imshow("Grid with Info", image)
cv2.waitKey(0)
cv2.destroyAllWindows()

整体比较空旷，再添加一个自身车子。

// 画车
cv::Point pt1 = GetPoint(carBoxMaxPoint[0], carBoxMinPoint[1]);
cv::Point pt2 = GetPoint(carBoxMinPoint[0], carBoxMaxPoint[1]);
cv::rectangle(viewImage, pt1, pt2, carColor, 2);

4.3、画结果

距离转化为像素画包络框；不同颜色信号灯用不同颜色实心圆表示

void ViewResult::DrawObjectsResult(cv::Scalar color, const Result::Objects::ConstPtr &message)
{
    const int lineType = 8;     // 线型  
    const int thickness = 2;    // 线条粗细  
    for (auto msg:message->objects) {
        std::vector<cv::Point> polygonPoints; 
        for (auto point:msg.envelope) {
            polygonPoints.push_back(GetPoint(point));
        }
        cv::polylines(viewImage, polygonPoints, true, color, thickness, lineType);   // 绘制多边形轮廓
    }
}



void ViewResult::DrawLightsResult(const Result::Lights::ConstPtr &message)
{
    int radius = 10;  // 半径像素
    for (auto msg:message->trafficLights) {
        cv::Point point = GetPoint(msg.x, msg.y);
        cv::Scalar color = trafficLabelColor[msg.type];
        cv::circle(viewImage, point, radius, color, -1); 
    }
}

cv::Point ViewResult::GetPoint(float DistanceX, float DistanceY)
{
    float onePixelDistance = gridSpacing / distance; // 1m = 多少像素 
    int x = int(imageWidth / 2 - DistanceX * onePixelDistance);
    int y = int(imageHeight / 2 + DistanceY * onePixelDistance);
    return cv::Point(x, y);
}