launch
<!-- YOLO Detection configuration --> <!-- YOLO 检测配置 -->
<arg name="weights" default="/home/heven/pred_ws/src/pred_fusion/fusion_prediction/yolo.pt"/> <!-- 权重文件路径 -->
<arg name="data" default="/home/heven/pred_ws/src/pred_fusion/fusion_prediction/src/yolov5/data/coco128.yaml"/> <!-- 数据文件路径 -->
<arg name="confidence_threshold" default="0.6"/> <!-- 置信度阈值 -->
<arg name="iou_threshold" default="0.45"/> <!-- 交并比阈值 -->
<arg name="maximum_detections" default="1000"/> <!-- 最大检测数量 -->
<arg name="device" default="0"/> <!-- 设备编号 -->
<arg name="agnostic_nms" default="true"/> <!-- agnostic_nms选项开关 -->
<arg name="line_thickness" default="3"/> <!-- 线条粗细 -->
<arg name="dnn" default="true"/> <!-- dnn选项开关 -->
<arg name="half" default="false"/> <!-- half选项开关 -->
<arg name="inference_size_h" default="640"/> <!-- 推理图片的高度 -->
<arg name="inference_size_w" default="640"/> <!-- 推理图片的宽度 -->
<arg name="view_image" default="true"/> <!-- 是否查看图像 -->
<arg name="input_image_topic" default="/carmaker_vds_client_node/image_raw/compressed"/> <!-- 输入图像主题 -->
<arg name="output_topic" default="/yolov5/detections"/> <!-- 输出主题 -->
<arg name="publish_image" default="true"/> <!-- 是否发布图像 -->
<arg name="output_image_topic" default="/yolov5/image_out"/><!-- 输出图像主题 -->
<!-- PointPillars configurations --> <!-- PointPillars配置 -->
<arg name="config_path" default="/home/heven/pred_ws/src/pred_fusion/OpenPCDet/tools/cfgs/kitti_models/pointpillar.yaml"/> <!-- 配置文件路径 -->
<arg name="ckpt_path" default="/home/heven/pred_ws/src/pred_fusion/fusion_prediction/pillars.pth"/> <!-- 检查点文件路径 -->
<arg name="input_lidar_topic" default="/pointcloud/os1_pc2"/> <!-- 输入激光雷达主题 -->
<arg name="score_threshold" default="0.6"/> <!-- 分数阈值 -->
<!-- Fusion configurations --> <!-- 融合配置 -->
<arg name="fusion_iou_threshold" default="0.2"/> <!-- 融合交并比阈值 -->
<!-- Tracker configurations --> <!-- 追踪器配置 -->
<arg name="tracker_iou_threshold" default="0.1"/> <!-- 追踪器交并比阈值 -->
<!-- Predictor configurations --> <!-- 预测器配置 -->
<arg name="crat_pred_path" default="/home/heven/pred_ws/src/pred_fusion/fusion_prediction/crat.ckpt"/> <!-- 预测器路径 -->
<arg name="sensor_direction" default="rear"/> <!-- 传感器方向 -->
<!-- 接下来定义各个节点 -->
<node pkg="fusion_prediction" name="detector" type="detector.py" output="screen"> <!-- 检测节点 -->
<!-- 各个参数定义略 -->
</node>
<node pkg="fusion_prediction" name="pillar_node" type="pillars_detect.py" output="screen"> <!-- PointPillars节点 -->
<!-- 各个参数定义略 -->
</node>
<node pkg="fusion_prediction" name="fusion_node" type="fusion.py" output="screen"> <!-- 融合节点 -->
<!-- 各个参数定义略 -->
</node>
<node pkg="rviz" name="rviz" type="rviz" args="-d $(find fusion_prediction)/launch/fusion_display.rviz" /> <!-- rviz可视化节点 -->
<node pkg="fusion_prediction" name="tracker_node" type="lidar_tracker.py" output="screen"> <!-- 激光雷达追踪节点 -->
<!-- 各个参数定义略 -->
</node>
<node pkg="fusion_prediction" name="prediction_node" type="predict.py" output="screen"> <!-- 预测节点 -->
<!-- 各个参数定义略 -->
</node>
</launch>
display.rviz
Panels:
- Class: rviz/Displays # 定义了一个Displays的类
Help Height: 78 # 帮助栏的高度
Name: Displays # Displays的名字
Property Tree Widget: # 属性树的小部件配置
Expanded: # 展开的属性
- /Global Options1
- /Status1
- /PointCloud21
- /BoundingBoxArray1
Splitter Ratio: 0.5 # 分割比例
Tree Height: 235 # 树的高度
- Class: rviz/Selection # 定义了一个Selection的类
Name: Selection # Selection的名字
- Class: rviz/Tool Properties # 定义了一个Tool Properties的类
Expanded: # 展开的属性
- /2D Pose Estimate1
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties # Tool Properties的名字
Splitter Ratio: 0.5886790156364441 # 分割比例
- Class: rviz/Views # 定义了一个Views的类
Expanded: # 展开的属性
- /Current View1
Name: Views # Views的名字
Splitter Ratio: 0.5 # 分割比例
- Class: rviz/Time # 定义了一个Time的类
Name: Time # Time的名字
SyncMode: 0 # 同步模式
SyncSource: "" # 同步源
Preferences:
PromptSaveOnExit: true # 退出时提示保存
Toolbars:
toolButtonStyle: 2 # 工具按钮样式
Visualization Manager: # 可视化管理器
Class: "" # 类
Displays: # 显示设置
- Alpha: 0.5 # 透明度
Cell Size: 1 # 单元格大小
Class: rviz/Grid # 定义了一个Grid的类
Color: 160; 160; 164 # 颜色
Enabled: true # 启用
Line Style: # 线型样式
Line Width: 0.029999999329447746 # 线宽
Value: Lines # 值
Name: Grid # Grid的名字
Normal Cell Count: 0 # 正常单元格数量
Offset: # 偏移量
X: 0
Y: 0
Z: 0
Plane: XY # 平面
Plane Cell Count: 10 # 平面单元格数量
Reference Frame: <Fixed Frame> # 参考框架
Value: true # 值
...
Window Geometry: # 窗口几何设置
Displays: # 显示设置
collapsed: false # 折叠设置
Height: 1011 # 高度
Hide Left Dock: false # 隐藏左侧停靠
Hide Right Dock: false # 隐藏右侧停靠
...
fusion.py
#! /usr/bin/env python3 # 使用python3环境执行此脚本
import rospy # 导入ROS Python包
import numpy as np # 导入numpy库，用于数值计算
import cv2 # 导入OpenCV库，用于图像处理
import math # 导入数学库
from sensor_msgs.msg import Image # 从sensor_msgs包导入Image消息类型
from cv_bridge import CvBridge # 从cv_bridge包导入CvBridge类，用于ROS图像和OpenCV图像之间的转换
from jsk_recognition_msgs.msg import BoundingBox, BoundingBoxArray # 导入BoundingBox和BoundingBoxArray消息类型
from fusion_prediction.msg import CameraBBox, CameraBBoxes # 导入自定义的摄像机边界框消息类型
# 定义函数将四元数转换为欧拉角
def quaternion2euluer(x, y, z, w):
# ... # 实现四元数到欧拉角的转换
# ...
# 定义函数将局部坐标点转换为全局坐标点，考虑PointPillars的输出
def local2global(local_pt, x_centroid, y_centroid, yaw):
# ... # 实现局部到全局坐标的转换
# ...
# 定义函数计算两个向量之间的角度
def calc_angle(vec1, vec2):
# ... # 使用内积计算两向量之间的角度
# ...
# 定义函数计算两个边界框之间的IOU
def iou(cam_bbox, lidar_bbox):
# ... # 计算两个边界框之间的交并比
# ...
# 定义融合类
class fusion:
def __init__(self):
# 初始化CvBridge
self.bridge = CvBridge()
# 初始化最终融合的对象信息字典
self.fusion_distance_dict = dict()
self.fusion_velocity_dict = dict()
self.fusion_heading_dict = dict()
# 初始化每个传感器检测到的对象边界框列表
self.cam_bbox_list = []
self.lidar_bbox_list = []
# 初始化融合后的最终对象边界框字典
self.final_object_bbox_dict = dict()
# 初始化对象id字典
for i in range(1, 11):
# ...
# ...
# 初始化ROS节点和订阅者
rospy.init_node('fusion_node', anonymous=False)
rospy.Subscriber('yolov5/detections', CameraBBoxes, self.camera_callback)
rospy.Subscriber('yolov5/image_out', Image, self.image_callback)
rospy.Subscriber('pillars/detections', BoundingBoxArray, self.lidar_callback)
self.fusion_pub = rospy.Publisher('display/detections', BoundingBoxArray, queue_size=50)
# YOLO检测结果回调函数
def camera_callback(self, data):
# ...
# ...
def image_callback(self, data):
# ...
# PointPillars检测结果回调函数
def lidar_callback(self, data):
# ...
# ...
# 定义函数将LiDAR bbox转换为全局BEV坐标
def bbox2globalBEV(self, lidar_bbox):
# ...
# 定义函数选择LiDAR bbox的四个角点
def select_corners(self, lidar_bbox, world_pts):
# ...
# 摄像机和激光雷达的校准函数
def calib_sensor(self):
# ... # 实现摄像机和激光雷达之间的校准
# ...
替换数据脚本
#!/usr/bin/env python
import rosbag
import os
from sensor_msgs.msg import PointCloud2, Image
from cv_bridge import CvBridge
import cv2
import glob
def replace_data(input_bag_path, output_bag_path, image_folder, pointcloud_folder):
bridge = CvBridge()
# 加载新的图片数据
image_files = sorted(glob.glob(os.path.join(image_folder, '*.png'))) # 可以根据您的图片格式进行调整
image_iterator = iter(image_files)
# 加载新的点云数据
pointcloud_files = sorted(glob.glob(os.path.join(pointcloud_folder, '*.pcd'))) # 假设为pcd格式
pointcloud_iterator = iter(pointcloud_files)
with rosbag.Bag(output_bag_path, 'w') as outbag:
for topic, msg, t in rosbag.Bag(input_bag_path).read_messages():
# 替换图片
if isinstance(msg, Image) and topic == '/desired_image_topic':
try:
new_image_path = next(image_iterator)
new_image = cv2.imread(new_image_path)
msg = bridge.cv2_to_imgmsg(new_image, "bgr8") # 假设图像为BGR格式
except StopIteration:
print("No more new images to replace. Using original.")
# 替换点云
if isinstance(msg, PointCloud2) and topic == '/desired_pointcloud_topic':
try:
new_pointcloud_path = next(pointcloud_iterator)
# 这里您需要一些代码来读取您的点云文件格式并转换为PointCloud2消息
# 例如，使用pcl库加载.pcd文件，然后使用ros中的pcl转换库将其转换为PointCloud2
except StopIteration:
print("No more new pointclouds to replace. Using original.")
outbag.write(topic, msg, t)
if __name__ == "__main__":
input_bag_path = 'input.bag' # 输入文件路径
output_bag_path = 'output.bag' # 输出文件路径
image_folder = 'images' # 新图片的文件夹路径
pointcloud_folder = 'pointclouds' # 新点云的文件夹路径
replace_data(input_bag_path, output_bag_path, image_folder, pointcloud_folder)
print("Data replacement is completed. New bagfile is saved as {}".format(output_bag_path))
再一份
#!/usr/bin/env python
import rosbag
import os
import glob
from sensor_msgs.msg import PointCloud2, Image
from cv_bridge import CvBridge
import cv2
import pcl
import struct
import numpy as np
def read_pcd(pcd_file):
pcd = pcl.load_XYZRGB(pcd_file)
points = np.zeros((pcd.size, 4), dtype=np.float32)
for i in range(pcd.size):
x, y, z, rgb = pcd[i]
r, g, b = struct.unpack('BBB', struct.pack('I', int(rgb)))
rgb = struct.unpack('I', struct.pack('BBB', b, g, r))[0]
points[i] = [x, y, z, rgb]
return points
def replace_data(input_bag_path, output_bag_path, image_folder, pointcloud_folder):
bridge = CvBridge()
# 加载新的图片数据
image_files = sorted(glob.glob(os.path.join(image_folder, '*.png')))
image_iterator = iter(image_files)
# 加载新的点云数据
pointcloud_files = sorted(glob.glob(os.path.join(pointcloud_folder, '*.pcd')))
pointcloud_iterator = iter(pointcloud_files)
with rosbag.Bag(output_bag_path, 'w') as outbag:
for topic, msg, t in rosbag.Bag(input_bag_path).read_messages():
# 替换图片
if isinstance(msg, Image) and topic == '/desired_image_topic':
try:
new_image_path = next(image_iterator)
new_image = cv2.imread(new_image_path)
msg = bridge.cv2_to_imgmsg(new_image, "bgr8")
except StopIteration:
print("No more new images to replace. Using original.")
# 替换点云
if isinstance(msg, PointCloud2) and topic == '/desired_pointcloud_topic':
try:
new_pointcloud_path = next(pointcloud_iterator)
points = read_pcd(new_pointcloud_path)
# 构造新的PointCloud2消息
msg.header = msg.header # 保留原始header
msg.height = 1
msg.width = points.shape[0]
msg.fields = [
PointField(name='x', offset=0, datatype=PointField.FLOAT32, count=1),
PointField(name='y', offset=4, datatype=PointField.FLOAT32, count=1),
PointField(name='z', offset=8, datatype=PointField.FLOAT32, count=1),
PointField(name='rgb', offset=12, datatype=PointField.UINT32, count=1),
]
msg.is_bigendian = False
msg.point_step = 16
msg.row_step = points.tostring().__len__()
msg.data = points.tostring()
msg.is_dense = False
except StopIteration:
print("No more new pointclouds to replace. Using original.")
outbag.write(topic, msg, t)
if __name__ == "__main__":
input_bag_path = 'input.bag' # 输入文件路径
output_bag_path = 'output.bag' # 输出文件路径
image_folder = 'images' # 新图片的文件夹路径
pointcloud_folder = 'pointclouds' # 新点云的文件夹路径
replace_data(input_bag_path, output_bag_path, image_folder, pointcloud_folder)
print("Data replacement is completed. New bagfile is saved as {}".format(output_bag_path))
c
#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/PointCloud2.h>
#include <opencv2/opencv.hpp>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <cv_bridge/cv_bridge.h>
int main(int argc, char** argv) {
rosbag::Bag bag;
bag.open("input.bag", rosbag::bagmode::Read);
rosbag::Bag outbag;
outbag.open("output.bag", rosbag::bagmode::Write);
std::vector<std::string> image_files; // 图像文件路径列表
std::vector<std::string> pointcloud_files; // 点云文件路径列表
// ... 填充image_files和pointcloud_files
auto image_iterator = image_files.begin();
auto pointcloud_iterator = pointcloud_files.begin();
rosbag::View view(bag);
for (rosbag::MessageInstance const m : view) {
sensor_msgs::Image::ConstPtr image_msg = m.instantiate<sensor_msgs::Image>();
sensor_msgs::PointCloud2::ConstPtr pointcloud_msg = m.instantiate<sensor_msgs::PointCloud2>();
if (image_msg != nullptr && m.getTopic() == "/desired_image_topic" && image_iterator != image_files.end()) {
cv::Mat new_image = cv::imread(*image_iterator);
sensor_msgs::ImagePtr new_image_msg = cv_bridge::CvImage(image_msg->header, "bgr8", new_image).toImageMsg();
outbag.write(m.getTopic(), m.getTime(), new_image_msg);
image_iterator++;
} else if (pointcloud_msg != nullptr && m.getTopic() == "/desired_pointcloud_topic" && pointcloud_iterator != pointcloud_files.end()) {
pcl::PointCloud<pcl::PointXYZRGB>::Ptr new_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::io::loadPCDFile(*pointcloud_iterator, *new_cloud);
sensor_msgs::PointCloud2 new_pointcloud_msg;
pcl::toROSMsg(*new_cloud, new_pointcloud_msg);
new_pointcloud_msg.header = pointcloud_msg->header;
outbag.write(m.getTopic(), m.getTime(), new_pointcloud_msg);
pointcloud_iterator++;
} else {
outbag.write(m.getTopic(), m.getTime(), m);
}
}
bag.close();
outbag.close();
return 0;
}
open3d的python
import rosbag
import cv2
import open3d as o3d
from sensor_msgs.msg import Image, PointCloud2
from cv_bridge import CvBridge
import sensor_msgs.point_cloud2 as pc2
image_files = [] # 图像文件路径列表
pointcloud_files = [] # 点云文件路径列表
# ... 填充image_files和pointcloud_files
in_bag = rosbag.Bag('input.bag', 'r')
out_bag = rosbag.Bag('output.bag', 'w')
bridge = CvBridge()
image_iterator = iter(image_files)
pointcloud_iterator = iter(pointcloud_files)
for topic, msg, t in in_bag.read_messages():
if topic == '/desired_image_topic':
new_image_path = next(image_iterator, None)
if new_image_path:
new_image = cv2.imread(new_image_path)
new_image_msg = bridge.cv2_to_imgmsg(new_image, encoding="bgr8")
new_image_msg.header = msg.header
out_bag.write(topic, new_image_msg, t)
continue
if topic == '/desired_pointcloud_topic':
new_pointcloud_path = next(pointcloud_iterator, None)
if new_pointcloud_path:
new_cloud = o3d.io.read_point_cloud(new_pointcloud_path)
points = [[point.x, point.y, point.z] for point in new_cloud.points]
new_pointcloud_msg = pc2.create_cloud_xyz32(msg.header, points)
out_bag.write(topic, new_pointcloud_msg, t)
continue
out_bag.write(topic, msg, t)
in_bag.close()
out_bag.close()
c
#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/PointCloud2.h>
#include <opencv2/opencv.hpp>
#include <Open3D/Open3D.h>
#include <cv_bridge/cv_bridge.h>
int main(int argc, char** argv) {
rosbag::Bag bag;
bag.open("input.bag", rosbag::bagmode::Read);
rosbag::Bag outbag;
outbag.open("output.bag", rosbag::bagmode::Write);
std::vector<std::string> image_files; // 图像文件路径列表
std::vector<std::string> pointcloud_files; // 点云文件路径列表
// ... 填充image_files和pointcloud_files
auto image_iterator = image_files.begin();
auto pointcloud_iterator = pointcloud_files.begin();
rosbag::View view(bag);
for (rosbag::MessageInstance const m : view) {
sensor_msgs::Image::ConstPtr image_msg = m.instantiate<sensor_msgs::Image>();
sensor_msgs::PointCloud2::ConstPtr pointcloud_msg = m.instantiate<sensor_msgs::PointCloud2>();
if (image_msg != nullptr && m.getTopic() == "/desired_image_topic" && image_iterator != image_files.end()) {
cv::Mat new_image = cv::imread(*image_iterator);
sensor_msgs::ImagePtr new_image_msg = cv_bridge::CvImage(image_msg->header, "bgr8", new_image).toImageMsg();
outbag.write(m.getTopic(), m.getTime(), new_image_msg);
image_iterator++;
} else if (pointcloud_msg != nullptr && m.getTopic() == "/desired_pointcloud_topic" && pointcloud_iterator != pointcloud_files.end()) {
auto new_cloud = std::make_shared<open3d::geometry::PointCloud>();
open3d::io::ReadPointCloud(*pointcloud_iterator, *new_cloud);
sensor_msgs::PointCloud2 new_pointcloud_msg;
pcl_conversions::fromPCL(*new_cloud, new_pointcloud_msg);
new_pointcloud_msg.header = pointcloud_msg->header;
outbag.write(m.getTopic(), m.getTime(), new_pointcloud_msg);
pointcloud_iterator++;
} else {
outbag.write(m.getTopic(), m.getTime(), m);
}
}
bag.close();
outbag.close();
return 0;
}
import pyrosbag
import pcl
import cv2
def replace_pointcloud_and_images(input_bag_path, output_bag_path, new_pointcloud, new_image_path, pointcloud_topic, image_topic):
# 打开输入bag文件
with pyrosbag.Bag(input_bag_path, 'r') as inbag:
# 创建输出bag文件
with pyrosbag.Bag(output_bag_path, 'w') as outbag:
# 遍历输入bag中的所有消息
for topic, msg, t in inbag.read_messages():
# 如果当前话题是点云话题
if topic == pointcloud_topic:
# 创建一个新的点云消息，并用新的点云数据填充它
new_msg = msg.__class__()
new_msg.data = new_pointcloud.tostring()
# 将新消息写入输出bag
outbag.write(topic, new_msg, t)
# 如果当前话题是图像话题
elif topic == image_topic:
# 读取新图像
new_image = cv2.imread(new_image_path)
# 创建一个新的图像消息，并用新图像数据填充它
new_msg = msg.__class__()
new_msg.data = new_image.tostring()
# 将新消息写入输出bag
outbag.write(topic, new_msg, t)
# 对于其他话题，不做更改
else:
outbag.write(topic, msg, t)
# 从文件读取新点云
new_pointcloud = pcl.load('new_pointcloud.pcd')
# 新图像的路径
new_image_path = 'new_image.png'
# 指定你想要替换的点云和图像的话题
pointcloud_topic = '/pointcloud_topic'
image_topic = '/image_topic'
# 输入和输出bag文件的路径
input_bag_path = 'input.bag'
output_bag_path = 'output.bag'
replace_pointcloud_and_images(input_bag_path, output_bag_path, new_pointcloud, new_image_path, pointcloud_topic, image_topic)
扫一扫
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
