首先应该是读取配置文件,比如我们常说的.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 时要注意两点
我们可以自己定义一个 .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);
}
void ViewResult::Init(std::shared_ptr<YAML::Node> parm)
{
viewImage.create(imageHeight, imageWidth, CV_8UC3);
YAML::Node perceptionConfig = *parm->GetYaml();
InitImage();
}
画栅格图与 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);
距离转化为像素画包络框;不同颜色信号灯用不同颜色实心圆表示
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);
}
编译节点,启动传感器与各个节点算法,打开 rviz。添加相应话题观察。打开前相机图像与我们可视化的图像。
画的比较单调(笑脸.jpg),各位这里自行添加元素,不过有了这些元素可以足够"甩锅"(滑稽.jpg)。