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

文章目录

  • 一、前言
  • 二、订阅与发布
  • 三、回调
  • 四、可视化
    • 4.1、初始化参数
    • 4.2、初始化图片
    • 4.3、画结果
    • 4.4、可视化结果

一、前言

  1. 感知与规划控制是无人驾驶算法重要算法，在交付测试阶段也最容易引起摩擦，这也是司空见惯的现象。有时候可能是接口对齐问题、有时候可能是版本管理问题、有时候可能是第三方如云平台问题、有时候车子硬件自身问题。
  2. 当然有时候也可能是感知算法与规控算法自身问题，所以拥有"自证清白"的能力是十分必要的。比如今天博主分享的感知结果实时可视化。具体实现可见下文。
  3. 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 时要注意两点

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

我们可以自己定义一个 .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);
}

四、可视化

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

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);
}

4.4、可视化结果

编译节点，启动传感器与各个节点算法，打开 rviz。添加相应话题观察。打开前相机图像与我们可视化的图像。

图片名称

画的比较单调(笑脸.jpg)，各位这里自行添加元素，不过有了这些元素可以足够"甩锅"(滑稽.jpg)。

本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若转载，请注明出处：/a/531266.html

如若内容造成侵权/违法违规/事实不符，请联系我们进行投诉反馈qq邮箱809451989@qq.com，一经查实，立即删除！

相关文章

AI绘画与建筑大师共创出的作品，震惊了？！

AI绘画与建筑大师共创出的作品，震惊了？！

在CAD制图盛行的今天&#xff0c;手绘依然是许多建筑大师首选的灵感记录方式。建筑大师西扎曾说过&#xff1a;草图能迅速的记录下他思维的瞬间&#xff0c;并再一次激发他更深入的思考。 看完这些建筑大师的手稿&#xff0c;不得不让人表示&#xff1a;这和医生处方手迹简直有…
阅读更多...
【满满干货】聚合接口—自动化工具㊣

【满满干货】聚合接口—自动化工具㊣

背景 在介绍接口自动化之前先给大家分享一下我所理解的“业务中台”的概念&#xff1a;业务中台是将企业的核心能力以数字化形式沉淀为各种服务中心&#xff0c;其目的是“提供企业能够快速&#xff0c;低成本创新的能力”。 例如公司内部的业务a、业务b同时有订单、登录等功…
阅读更多...
企业网盘私有化部署和本地私有化部署的区别

企业网盘私有化部署和本地私有化部署的区别

在当今数据量激增的背景下&#xff0c;企业如何高效、安全地管理和传输大量数据成为了一个关键问题。企业网盘作为一种解决方案&#xff0c;其部署方式直接影响到数据的安全性、工作效率的提升以及运营成本的控制。私有化部署与本地化部署是两种主流的企业网盘部署策略&#xf…
阅读更多...
C语言_文件操作

C语言_文件操作

文件基础 什么是文件 文件是在计算机中以实现某种功能、或某个软件的部分功能为目的而定义的一个单位。磁盘上的文件是文件。但是在程序设计中&#xff0c;我们一般谈的文件有两种&#xff1a;程序文件、数据文件&#xff08;从文件功能的角度来分的&#xff09;。 程序文件 …
阅读更多...
【MATLAB源码-第33期】matlab基于遗传算法的多层编码柔性作业车间调度问题仿真

【MATLAB源码-第33期】matlab基于遗传算法的多层编码柔性作业车间调度问题仿真

操作环境&#xff1a; MATLAB 2022a 1、算法描述 1. 遗传算法&#xff1a; 遗传算法是一种基于自然选择和遗传遗传学的优化算法。它模拟了生物进化的过程&#xff0c;通过对问题解的编码&#xff08;通常以染色体或基因型的形式&#xff09;、交叉、变异等操作来生成新的解。…
阅读更多...
Coze 识别用户意图

Coze 识别用户意图

文章目录 Coze 识别用户意图 Coze 识别用户意图 本文将通过 LLM 节点、Condition 节点和插件节点构建一个用于识别用户意图的工作流。 效果示例 本文构建的示例工作流概览如下。 在该工作流中&#xff1a; 使用 LLM 节点将用户输入数据分为 1&#xff08;天气&#xff09;、…
阅读更多...
Flume实时读取目录文件到HDFS案例

Flume实时读取目录文件到HDFS案例

【尚硅谷】大数据技术之Flume教程从入门到实战_哔哩哔哩_bilibili 目录 flume简介 flume案例 1、监控端口数据官方案例 2、实时读取目录文件到HDFS案例 flume简介 Flume是Cloudera提供的一个高可用的&#xff0c;高可靠的&#xff0c;分布式的海量日志采集、聚合和传输的系…
阅读更多...
【UE Niagara】烟雾特效

【UE Niagara】烟雾特效

效果 步骤 1. 创建一个材质&#xff0c;这里命名为“M_Smoke” 设置混合模式为半透明&#xff0c;着色模型为无光照 连接如下节点 其中纹理采样节点所使用的纹理为引擎自带的“T_SmokeSubUV_8x8” 2. 新建一个Niagara发射器&#xff0c;模板使用“Empty”&#xff0c;这里命名…
阅读更多...
MLeaksFinder报错

MLeaksFinder报错

1.报错&#xff1a;FBClassStrongLayout.mm 文件&#xff1a;layoutCache[currentClass] ivars; 解决&#xff1a;替换为layoutCache[(id)currentClass] ivars; 2.编译正常但运行时出现crash indirect_symbol_bindings[i] cur->rebinding FBRetainCycleDetector iOS15 …
阅读更多...
亚马逊运营必看！如何运用自养号测评获得买家评论转销量？

亚马逊运营必看！如何运用自养号测评获得买家评论转销量？

作为亚马逊卖家&#xff0c;相信大家对亚马逊的产品星级评分 (Rating) 都不陌生&#xff0c;这几颗亮眼的星星&#xff0c;不仅可以让你的Listing脱颖而出&#xff0c;获得足够多、足够高的产品评分&#xff0c;也是促使消费者下单的重要因素之一。 那么&#xff0c;亚马逊运营…
阅读更多...
DepthFormer论文详解

DepthFormer论文详解

摘要 本文旨在解决有监督单目深度估计的问题&#xff0c;我们从一项细致的试点研究开始&#xff0c;以证明远程相关性对于准确的深度估计至关重要。我们建议使用Transformer以有效地注意力机制对这种全局上下文进行建模。我们还采用一个额外的卷积分支来保留局部信息&#xff0…
阅读更多...
NPU编译MultiScaleDeformableAttention

NPU编译MultiScaleDeformableAttention

NPU对pytorch&#xff0c;想将检测模型在NPU上训练&#xff0c;存在编译MultiScaleDeformableAttention的需求。 然而&#xff0c;原dino模型https://github.com/IDEA-Research/DINO/tree/main/models/dino/ops/src 仅包含CPU版本和GPU版本&#xff1a; 是不是就真的无法解决…
阅读更多...
2024/4/5—力扣—在排序数组中查找元素的第一个和最后一个位置

2024/4/5—力扣—在排序数组中查找元素的第一个和最后一个位置

代码实现&#xff1a; 思路&#xff1a;二分法 方法一&#xff1a;分别查找左右侧边界 /*** Note: The returned array must be malloced, assume caller calls free().*/ int GetTargetFirstPosition(int *nums, int numsSize, int target) {int l 0, r numsSize - 1;while …
阅读更多...
【北京迅为】《iTOP-3588开发板开发板系统编程手册》第3章 标准IO

【北京迅为】《iTOP-3588开发板开发板系统编程手册》第3章 标准IO

RK3588是一款低功耗、高性能的处理器&#xff0c;适用于基于arm的PC和Edge计算设备、个人移动互联网设备等数字多媒体应用&#xff0c;RK3588支持8K视频编解码&#xff0c;内置GPU可以完全兼容OpenGLES 1.1、2.0和3.2。RK3588引入了新一代完全基于硬件的最大4800万像素ISP&…
阅读更多...
蓝桥杯复习笔记

蓝桥杯复习笔记

文章目录 gridflexhtml表格合并单元格 表单表单元素input类型 select h5文件上传拖拽apiweb Storage css块元素和行内元素转换positionfloat溢出显示隐藏外边距过渡和动画动画变形选择器属性选择伪类选择器 css3边框圆角边框阴影渐变text-overflow与word-wrap jsdom操作documen…
阅读更多...
STL容器之unordered_set类

STL容器之unordered_set类

文章目录 STL容器之unordered_set类1、unordered系列关联式容器2、unordered_set2.1、unordered_set介绍2.2、unordered_set的使用2.2.1、unordered_set的常见构造2.2.2、unordered_set的迭代器2.2.3、unordered_set的容量2.2.4、unordered_set的增删查2.2.5、unordered_set的桶…
阅读更多...
C++--this指针

C++--this指针

this 指针是一个隐含于每一个成员函数中的特殊指针。它是指向一个正操作该成员函数的对象。当对一个对象调用成员函数时&#xff0c;编译程序先将对象的地址赋予this指针&#xff0c;然后调用成员函数。每次成员函数存取数据成员时&#xff0c;C编译器将根据 this 指针所指向的…
阅读更多...
由于找不到msvcp100.dll,无法继续执行代码要如何处理？正确的msvcp100.dll修复

由于找不到msvcp100.dll,无法继续执行代码要如何处理？正确的msvcp100.dll修复

由于找不到msvcp100.dll,无法继续执行代码要如何处理&#xff1f;其实要处理这种dll文件丢失的问题&#xff0c;还是比较简单的&#xff0c;只要我们了解清楚这个msvcp100.dll文件&#xff0c;那么就可以快速的解决&#xff0c;好了&#xff0c;废话不多说&#xff0c;我们一起…
阅读更多...
证件照小于30kb怎么弄？这个工具三步搞定

证件照小于30kb怎么弄？这个工具三步搞定

当我们需要将照片上传到各种平台时&#xff0c;常常会遇到图片文件大小限制的问题。无论是社交媒体平台还是工作需求&#xff0c;如果照片文件过大&#xff0c;系统会提示上传失败或无法上传。想要解决的这个问题&#xff0c;可以选择将图片压缩指定大小&#xff0c;比如图片压…
阅读更多...
git操作码云(gitee)创建仓库到上传到远程仓库

git操作码云(gitee)创建仓库到上传到远程仓库

想必有的小伙伴在为上传到码云远程仓库而感到烦恼吧&#xff01;本篇为大家详细讲解实现过程&#xff0c;跟着我的步伐一步一步来。 我就当大家已经注册好了码云 一、在码云上需要的操作 接下来我们需要使用到 git 了 二、git 上的操作 到了咋们的git了&#xff0c;开整 首…
阅读更多...
最新文章

Copyright @ 2022~2023