融合的pointpillars.detect代码注释

#!/usr/bin/env python3 # 指定Python解释器的路径

# 导入必要的库和模块
import rospy                            # 导入ROS的Python库
from sensor_msgs.msg import PointCloud2  # 导入ROS的PointCloud2消息类型
import sensor_msgs.point_cloud2 as pc2   # 导入PointCloud2相关函数库
from std_msgs.msg import Header          # 导入ROS的Header消息类型
from pathlib import Path                 # 导入Path库，用于处理文件路径
from jsk_recognition_msgs.msg import BoundingBox, BoundingBoxArray # 导入BoundingBox消息类型

from time import time                    # 导入时间函数
import numpy as np                       # 导入NumPy库
from pyquaternion import Quaternion      # 导入四元数库
import scipy.linalg as linalg            # 导入SciPy的线性代数模块
import sys                               # 导入系统模块
import os                                # 导入操作系统模块

# 将自定义的路径加入系统路径
sys.path.append('/home/heven/pred_ws/src/pred_fusion/PointPillars')

# 获取当前文件的路径，并添加到系统路径中
FILE = Path(__file__).resolve()
ROOT = FILE.parents[0].parents[0].parents[0] / "PointPillars"
if str(ROOT) not in sys.path:
    sys.path.append(str(ROOT))          # 将根目录添加到PATH
ROOT = Path(os.path.relpath(ROOT, Path.cwd()))  # 获得相对路径

# 从自定义模块中导入模型
from model import PointPillars

# 再次将自定义的路径加入系统路径
sys.path.append('/home/heven/pred_ws/src/pred_fusion/OpenPCDet')

# 获取当前文件的路径，并添加到系统路径中
FILE = Path(__file__).resolve()
ROOT = FILE.parents[0].parents[0].parents[0] / "OpenPCDet"
if str(ROOT) not in sys.path:
    sys.path.append(str(ROOT))          # 将根目录添加到PATH
ROOT = Path(os.path.relpath(ROOT, Path.cwd()))  # 获得相对路径

# 从自定义模块中导入配置和工具
from pcdet.config import cfg, cfg_from_yaml_file
from pcdet.datasets import DatasetTemplate
from pcdet.utils import common_utils

# 定义一个用于筛选点云范围的函数
def point_range_filter(pts, point_range=[0, -39.68, -3, 69.12, 39.68, 1]):
    '''
    data_dict: dict(pts, gt_bboxes_3d, gt_labels, gt_names, difficulty)
    point_range: [x1, y1, z1, x2, y2, z2]
    '''
    # 各个方向上的条件筛选
    flag_x_low = pts[:, 0] > point_range[0]
    flag_y_low = pts[:, 1] > point_range[1]
    flag_z_low = pts[:, 2] > point_range[2]
    flag_x_high = pts[:, 0] < point_range[3]
    flag_y_high = pts[:, 1] < point_range[4]
    flag_z_high = pts[:, 2] < point_range[5]
    keep_mask = flag_x_low & flag_y_low & flag_z_low & flag_x_high & flag_y_high & flag_z_high
    pts = pts[keep_mask]
    return pts 

# 定义一个用于从四元数转换到欧拉角的函数
def quaternion2euluer(x, y, z, w):
    """
        Convert a quaternion into euler angles (roll, pitch, yaw)
        roll is rotation around x in radians (counterclockwise)
        pitch is rotation around y in radians (counterclockwise)
        yaw is rotation around z in radians (counterclockwise)
    """
    import math
    t0 = +2.0 * (w * x + y * z)
    t1 = +1.0 - 2.0 * (x * x + y * y)
    roll_x = math.atan2(t0, t1)
    t2 = +2.0 * (w * y - z * x)
    t2 = +1.0 if t2 > +1.0 else t2
    t2 = -1.0 if t2 < -1.0 else t2
    pitch_y = math.asin(t2)
    t3 = +2.0 * (w * z + x * y)
    t4 = +1.0 - 2.0 * (y * y + z * z)
    yaw_z = math.atan2(t3, t4)
    return math.degrees(yaw_z) # 返回以度为单位的偏航角

# 定义Demo数据集类，继承DatasetTemplate
class DemoDataset(DatasetTemplate):
    def __init__(self, dataset_cfg, class_names, training=True, root_path=None, logger=None, ext='.bin'):
        super().__init__(
            dataset_cfg=dataset_cfg, class_names=class_names, training=training, root_path=root_path, logger=logger
        )

# 定义Pointpillars_ROS类，封装了ROS与PointPillars模型的交互当然，以下是给出的代码的每一行的中文注释：

```python
#!/usr/bin/env python3 # 使用环境变量中的python3解释器执行此脚本

import rospy # 导入ROS的Python库
from sensor_msgs.msg import PointCloud2 # 从sensor_msgs包中导入PointCloud2消息类型
import sensor_msgs.point_cloud2 as pc2 # 导入sensor_msgs包中的point_cloud2模块
from std_msgs.msg import Header # 从std_msgs包中导入Header消息类型
from pathlib import Path # 导入Path库，用于处理文件和目录路径
from jsk_recognition_msgs.msg import BoundingBox, BoundingBoxArray # 从jsk_recognition_msgs包中导入BoundingBox和BoundingBoxArray消息类型

from time import time # 导入time模块的time函数，用于获取当前时间戳
import numpy as np # 导入NumPy库，并命名为np
from pyquaternion import Quaternion # 导入Quaternion库

import numpy as np # 再次导入NumPy库，这行代码是冗余的
import torch # 导入PyTorch库
import scipy.linalg as linalg # 导入SciPy库的线性代数模块，并命名为linalg
import sys # 导入系统库
import os # 导入操作系统库

sys.path.append('/home/heven/pred_ws/src/pred_fusion/PointPillars') # 将指定路径添加到系统路径中，使Python能够找到该目录下的模块

FILE = Path(__file__).resolve() # 获取当前文件的绝对路径
ROOT = FILE.parents[0].parents[0].parents[0] / "PointPillars" # 获取PointPillars的根路径
if str(ROOT) not in sys.path:
    sys.path.append(str(ROOT))  # 如果ROOT不在系统路径中，则将其添加
ROOT = Path(os.path.relpath(ROOT, Path.cwd()))  # 获取相对于当前工作目录的相对路径

from model import PointPillars # 从model模块中导入PointPillars类

sys.path.append('/home/heven/pred_ws/src/pred_fusion/OpenPCDet') # 将OpenPCDet的路径添加到系统路径中

FILE = Path(__file__).resolve() # 再次获取当前文件的绝对路径
ROOT = FILE.parents[0].parents[0].parents[0] / "OpenPCDet" # 获取OpenPCDet的根路径
if str(ROOT) not in sys.path:
    sys.path.append(str(ROOT))  # 如果ROOT不在系统路径中，则将其添加
ROOT = Path(os.path.relpath(ROOT, Path.cwd()))  # 获取相对于当前工作目录的相对路径

from pcdet.config import cfg, cfg_from_yaml_file # 从pcdet.config模块中导入cfg和cfg_from_yaml_file
from pcdet.datasets import DatasetTemplate # 从pcdet.datasets模块中导入DatasetTemplate类
from pcdet.utils import common_utils # 从pcdet.utils模块中导入common_utils

# 定义一个函数，用于过滤掉不在指定范围内的点
def point_range_filter(pts, point_range=[0, -39.68, -3, 69.12, 39.68, 1]):
    # ... 省略具体实现 ...

# 定义一个函数，用于将四元数转换为欧拉角
def quaternion2euluer(x, y, z, w):
    # ... 省略具体实现 ...

# 定义一个名为DemoDataset的类，继承自DatasetTemplate
class DemoDataset(DatasetTemplate):
    # ... 省略具体实现 ...

# 定义一个名为Pointpillars_ROS的类
class Pointpillars_ROS:
    # ... 省略具体实现 ...

# 如果此脚本作为主程序运行
if __name__ == '__main__':
    sec = Pointpillars_ROS() # 创建Pointpillars_ROS的实例
    
    try:
        rospy.spin() # 进入ROS的事件循环
    except KeyboardInterrupt: # 如果接收到键盘中断信号
        del sec # 删除sec实例
        print("Shutting down") # 打印“关闭”消息

轨迹预测

# 导入相关的库和依赖
import rospy
import torch
import sys
from pathlib import Path
import os

from time import time

# 导入消息类型
from jsk_recognition_msgs.msg import BoundingBoxArray
from visualization_msgs.msg import Marker, MarkerArray
from nav_msgs.msg import Odometry
from collections import deque

FILE = Path(__file__).resolve()  # 获取当前文件的绝对路径
ROOT = FILE.parents[0]
if str(ROOT) not in sys.path:
    sys.path.append(str(ROOT))  # 将ROOT添加到路径中
ROOT = Path(os.path.relpath(ROOT, Path.cwd()))  # 获取相对路径

import rviz_visualizer as visual  # 导入RViz可视化库

# 重新设置文件和根路径
FILE = Path(__file__).resolve()
ROOT = FILE.parents[0].parents[0].parents[0] / "crat-pred"
if str(ROOT) not in sys.path:
    sys.path.append(str(ROOT))  # 添加ROOT到路径
ROOT = Path(os.path.relpath(ROOT, Path.cwd()))  # 获取相对路径

from model.crat_pred import CratPred  # 从模型中导入CRAT-Pred

class Predictor():
    def __init__(self):

        # 初始化相关变量
        self.id_list = []
        self.global_trajectory = dict()
        self.global_trajectory[0] = []
        self.init_flag = 0
        self.ref_odom_x = 0
        self.centers_list = dict()
        self.past_trajectory = dict()

        # 初始化ROS节点
        rospy.init_node('prediction_node', anonymous=False)
        self.sensor_direction = rospy.get_param("~sensor_direction")
        self.crat_pred_path = rospy.get_param("~crat_pred_path")

        # 订阅传感器和轨迹数据
        rospy.Subscriber('fusion/tracks', BoundingBoxArray, self.tracking_callback)
        rospy.Subscriber('/Odometry', Odometry, self.odom_callback)
        self.traj_pub = rospy.Publisher('/trajectory', MarkerArray, queue_size=1)

    def odom_callback(self, msg):

        # 初始化起始位置，并保存当前位置
        if self.init_flag == 0:
            self.ref_odom_x = msg.pose.pose.position.x
            self.init_flag = 1

        self.odom_x = msg.pose.pose.position.x - self.ref_odom_x
        self.odom_y = msg.pose.pose.position.y

        self.cur_time = msg.header.stamp.secs + msg.header.stamp.nsecs * 1e-9

    def tracking_callback(self, data):

        # 存储物体边界框并预测
        self.object_boxes = data.boxes
        self.prediction()

    def prediction(self):

        # 根据时间间隔进行预测
        if (self.cur_time % 0.1) > 0.09 or (self.cur_time % 0.1) < 0.01:

            # 这一段代码处理车辆和物体的位置和方向，并将其转换为相对位移
            # 进一步处理将用于多代理轨迹预测的数据结构

            # 利用队列进行轨迹预测，并使用CRAT-Pred模型进行推理
            # 将预测结果转换为局部坐标，并通过RViz进行可视化

    def position_2_displ(self, global_traj):

        # 将全局坐标转换为位移
        displ = []
        for index in range(len(global_traj)-1):
            displ.append([global_traj[index+1][0] - global_traj[index][0], global_traj[index+1][1] - global_traj[index][1], 1.0000])

        return displ

if __name__ == "__main__":

    # 如果ROS未关闭，则启动预测器并开始监听
    if not rospy.is_shutdown():
        Predictor()
        rospy.spin()

launch