激光雷达点云可视化
更新时间：
本实验介绍可视化场景中激光雷达点云消息，键盘控制车辆观察车辆点云移动
场景简介
本实验介绍：可视化场景中激光雷达点云消息，键盘控制车辆观察车辆点云移动
实验室资源方式简介
进入实操前，请确保阿里云账号满足以下条件：
个人账号资源
使用您个人的云资源进行操作，资源归属于个人。
平台仅提供手册参考，不会对资源做任何操作。
确保已完成云工开物300元代金券领取。
已通过实名认证且账户余额≥100元。
本次实验将在您的账号下开通实操所需计算型实例规格族c7a，费用约为：3元/时（具体金额取决于实验完成的时间），需要您通过阿里云云工开物学生专属300元抵扣金兑换本次实操的云资源。
如果您调整了资源规格、使用时长，或执行了本方案以外的操作，可能导致费用发生变化，请以控制台显示的实际价格和最终账单为准。
领取专属权益及创建实验资源
在开始实验之前，请先点击右侧屏幕的“进入实操”再进行后续操作
本次实验需要您通过领取阿里云云工开物学生专属300元抵扣券兑换本次实操的云资源，如未领取请先点击领取。（若已领取请跳过）
重要
实验产生的费用优先使用优惠券，优惠券使用完毕后需您自行承担。
实验步骤
1、服务部署
点击链接，进入部署页面
按下图设置【服务实例名称】、【地域】、【实例密码】
服务实例名称：test（可自定义命名）
地域：华东2（上海）
实例密码：Sjtu@520
说明输入实例密码时请注意大小写，请记住您设置的实例名称及对应密码，后续实验过程会用到。
完成填写后点击【下一步：确认订单】
核对实例信息及价格预览，无误请点击【立即创建】
重要领取300元优惠券后，资源应为0元/小时，且会提示【您当前账户的余额充足】！若提示余额不足等，请检查是否正确领取优惠券
创建成功，点击【去列表查看】
查看实例，并等待部署
点击左侧的图标展开目录，选择目录中的【云服务器ECS】
云服务器ECS—实例—远程连接
下拉展开更多登录方式，选择【通过VNC远程连接】
点击ecs-user，并输入之前设置的实例密码：Sjtu@520，回车
进入镜像后，点击左侧第2个图标，打开文件夹后点击catkin_ws文件夹
进入catkin_ws文件夹，在空白处点击鼠标右键，选择Open in Terminal
2、代码部分
生成gazebo仿真环境需要调用spawn.launch
<launch> <!-- 定义启动文件中的参数 --> <arg name="paused" default="false"/> <!-- 是否暂停仿真，默认为false --> <arg name="use_sim_time" default="true"/> <!-- 是否使用仿真时间，默认为true --> <arg name="gui" default="true"/> <!-- 是否启动GUI，默认为true --> <arg name="headless" default="false"/> <!-- 是否无头模式，默认为false --> <arg name="debug" default="false"/> <!-- 是否调试模式，默认为false --> <arg name="verbose" default="false"/> <!-- 是否详细输出，默认为false --> <arg name="model" value="$(find site_model)/urdf/car.urdf"/> <!-- 机器人的URDF模型路径 --> <!-- Start gazebo and load the world --> <include file="$(find gazebo_ros)/launch/empty_world.launch" > <arg name="paused" value="$(arg paused)"/> <arg name="use_sim_time" value="$(arg use_sim_time)"/> <arg name="gui" value="$(arg gui)"/> <arg name="headless" value="$(arg headless)"/> <arg name="debug" value="$(arg debug)"/> <arg name="verbose" value="$(arg verbose)"/> </include> <!-- Spawn the site --> <node name="tf_footprint_base" pkg="tf" type="static_transform_publisher" args="0 0 0 0 0 0 base_link base_footprint 40" /> <arg name="gpu" default="false"/> <arg name="organize_cloud" default="false"/> <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find site_model)/urdf/site_model.urdf.xacro' organize_cloud:=$(arg organize_cloud) gpu:=$(arg gpu)" /> <node pkg="gazebo_ros" type="spawn_model" name="spawn_model" args="-urdf -param /robot_description -model site_model"/> <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher"> <param name="publish_frequency" type="double" value="30.0" /> </node> <node name="fake_joint_calibration" pkg="rostopic" type="rostopic" args="pub /calibrated std_msgs/Bool true"/> <!-- 从yaml文件加载联合控制器的参数 --> <rosparam file="$(find pkg)/config/ctrl.yaml" command="load"/> <!-- 加载机器人模型描述参数 --> <group ns="car1"> <param name="robot_description" textfile="$(find pkg)/urdf/car1.urdf"/> <node name= "robot_state_publisher" pkg= "robot_state_publisher" type= "robot_state_publisher"> <param name="publish_frequency" type="double" value="20.0" /> </node> <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen" args="-urdf -model car1 -param robot_description -x -1.5 -y -3 -z 0.05"/> <node name="controller_manager" pkg="controller_manager" type="spawner" respawn="true" output="screen" ns="/car1" args="left_rear_wheel_velocity_controller right_rear_wheel_velocity_controller left_front_wheel_velocity_controller right_front_wheel_velocity_controller left_steering_hinge_position_controller right_steering_hinge_position_controller joint_state_controller"/> </group> <group ns="car2"> <param name="robot_description" textfile="$(find pkg)/urdf/car2.urdf"/> <node name= "robot_state_publisher" pkg= "robot_state_publisher" type= "robot_state_publisher"> <param name="publish_frequency" type="double" value="20.0" /> </node> <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen" args="-urdf -model car2 -param robot_description -x -1.5 -y -4 -z 0.05"/> <node name="controller_manager" pkg="controller_manager" type="spawner" respawn="true" output="screen" ns="/car2" args="left_rear_wheel_velocity_controller right_rear_wheel_velocity_controller left_front_wheel_velocity_controller right_front_wheel_velocity_controller left_steering_hinge_position_controller right_steering_hinge_position_controller joint_state_controller"/> </group> <node pkg="site_model" type="ServoCommands.py" name="ServoCommands" output="screen"/> </launch>
随后调用pointcloud_listener.py激活激光雷达。
#!/usr/bin/env python3 # 指定脚本使用系统环境中的 Python 3 解释器执行 ############################################################# # This py file subscribe the topic of lidars and output a # # list of them. # ############################################################# # 脚本功能描述：订阅激光雷达话题并输出它们的列表 import rospy # 导入 ROS Python 客户端库，用于与 ROS 系统交互 import std_msgs.msg # 导入 ROS 的标准消息类型，用于消息头部的定义 import message_filters # 导入 message_filters 库，用于同步多个话题的消息 from sensor_msgs.msg import PointCloud2 # 导入 ROS 的 PointCloud2 消息类型，用于接收激光雷达数据 # self-defined msg from msgs.msg._ListPointCloud import * # 导入自定义的 ListPointCloud 消息类型 def pointcloud_listener(cloud11, cloud12, cloud2): """ 回调函数，用于处理接收到的点云数据，并将其集成到一个消息中。 :param cloud11: 第一组点云数据 :param cloud12: 第二组点云数据 :param cloud2: 第三组点云数据 """ msgcloud = ListPointCloud() # 创建一个 ListPointCloud 消息对象，用于整合所有激光雷达数据 msgcloud.pointcloud.append(cloud11) # 将第一个激光雷达的点云消息添加到 ListPointCloud 消息对象的 pointcloud 列表中 msgcloud.pointcloud.append(cloud12) # 将第二个激光雷达的点云消息添加到 ListPointCloud 消息对象的 pointcloud 列表中 msgcloud.pointcloud.append(cloud2) # 将第三个激光雷达的点云消息添加到 ListPointCloud 消息对象的 pointcloud 列表中 # Add time stamp msgcloud.header = std_msgs.msg.Header() # 创建一个标准消息头部对象 msgcloud.header.stamp = rospy.Time.now() # 设置消息的时间戳为当前时间 # Publish pub = rospy.Publisher("/pointcloud_list", ListPointCloud, queue_size=1) # 创建一个发布者，用于发布整合后的激光雷达消息，话题名为 "/pointcloud_list" pub.publish(msgcloud) # 发布整合后的激光雷达消息 if __name__ == '__main__': # 主程序入口，确保脚本直接运行时执行以下代码 rospy.init_node('lidar_listener', anonymous=True) # 初始化 ROS 节点，节点名为 'lidar_listener'，anonymous=True 表示节点名唯一 sub_cloudpts_11 = message_filters.Subscriber('/velodyne1_points', PointCloud2) # 创建一个订阅者，订阅话题 '/velodyne1_points'，消息类型为 PointCloud2 sub_cloudpts_12 = message_filters.Subscriber('/velodyne2_points', PointCloud2) # 创建一个订阅者，订阅话题 '/velodyne2_points'，消息类型为 PointCloud2 sub_cloudpts_2 = message_filters.Subscriber('/velodyne3_points', PointCloud2) # 创建一个订阅者，订阅话题 '/velodyne3_points'，消息类型为 PointCloud2 sync = message_filters.ApproximateTimeSynchronizer([sub_cloudpts_11, sub_cloudpts_12, sub_cloudpts_2], 10, 1) # 创建一个时间同步器，用于同步来自多个激光雷达的消息，队列大小为 10，时间差阈值为 1 秒 sync.registerCallback(pointcloud_listener) # 注册回调函数 pointcloud_listener，当同步器接收到所有激光雷达的消息时调用该函数 print("PointCloud List Begin.") # 打印提示信息，表示激光雷达监听器已启动 rospy.spin() # 进入 ROS 事件循环，保持节点运行，直到节点被关闭随后调用pointcloud_combiner合并所有点云并固定其坐标。
/*########################################################### # This py file subscribe the topic of lidars and publish # # the integrated information. # ###########################################################*/ #include <ros/ros.h> // 包含 ROS 核心库 #include <sensor_msgs/PointCloud2.h> // 包含 ROS 的 PointCloud2 消息类型 #include <pcl_conversions/pcl_conversions.h> // 包含 PCL 与 ROS 消息转换的库 #include <pcl/io/pcd_io.h> // 包含 PCL 的 I/O 操作库 #include <pcl/point_cloud.h> // 包含 PCL 点云库 #include <pcl/filters/statistical_outlier_removal.h> // 包含 PCL 的统计离群值去除库 #include <pcl/filters/extract_indices.h> // 包含 PCL 的索引提取库 #include <pcl/sample_consensus/method_types.h> // 包含 PCL 的样本一致性方法类型库 #include <pcl/sample_consensus/model_types.h> // 包含 PCL 的样本一致性模型类型库 #include <pcl/segmentation/sac_segmentation.h> // 包含 PCL 的样本一致性分割库 #include <pcl/ModelCoefficients.h> // 包含 PCL 的模型系数库 #include <pcl/point_types.h> // 包含 PCL 的点类型库 #include <msgs/ListPointCloud.h> // 包含自定义的 ListPointCloud 消息类型 using namespace std; // 使用标准命名空间 typedef pcl::PointXYZI PointT; // 定义点云格式为 XYZI typedef pcl::PointCloud<PointT> PointCloudT; // 定义点云类型为 PointCloudT #define lidar11_x -0.55740706 // 定义第一个激光雷达的 x 偏移量 #define lidar11_y 1.25432157 // 定义第一个激光雷达的 y 偏移量 #define lidar11_z 0.006 // 定义第一个激光雷达的 z 偏移量 #define lidar12_x 0.43254243 // 定义第二个激光雷达的 x 偏移量 #define lidar12_y 0.26437207 // 定义第二个激光雷达的 y 偏移量 #define lidar12_z 0.006 // 定义第二个激光雷达的 z 偏移量 #define lidar2_x 9.74248361e-04 // 定义第三个激光雷达的 x 偏移量 #define lidar2_y -9.89292350e-01 // 定义第三个激光雷达的 y 偏移量 #define lidar2_z 1.91780397e-01 // 定义第三个激光雷达的 z 偏移量 class pointcloud_combiner // 定义点云合并类 { private: ros::NodeHandle nh; // ROS 节点句柄 ros::Publisher pub; // ROS 发布者 ros::Subscriber sub; // ROS 订阅者 public: pointcloud_combiner() // 构造函数 { pub = nh.advertise<sensor_msgs::PointCloud2>("/point_cloud_combined", 1); // 初始化发布者，发布话题为 "/point_cloud_combined" sub = nh.subscribe("/pointcloud_list", 1, &pointcloud_combiner::callback, this); // 初始化订阅者，订阅话题为 "/pointcloud_list" } void callback(const msgs::ListPointCloud& pointcloud_list) // 回调函数，处理接收到的点云消息 { // define pointcloud type: sensor_msgs::PointCloud2 sensor_msgs::PointCloud2 cloud11, cloud12, cloud2; // 定义三个点云消息对象 cloud11 = pointcloud_list.pointcloud[0]; // 获取第一个激光雷达的点云消息 cloud12 = pointcloud_list.pointcloud[1]; // 获取第二个激光雷达的点云消息 cloud2 = pointcloud_list.pointcloud[2]; // 获取第三个激光雷达的点云消息 // Transform cloud from type sensor_msgs::PointCloud2 to pcl::PointCloud PointCloudT cloud11_pcl, cloud12_pcl, cloud2_pcl; // 定义三个 PCL 点云对象 pcl::fromROSMsg(cloud11, cloud11_pcl); // 将第一个点云消息转换为 PCL 点云 pcl::fromROSMsg(cloud12, cloud12_pcl); // 将第二个点云消息转换为 PCL 点云 pcl::fromROSMsg(cloud2, cloud2_pcl); // 将第三个点云消息转换为 PCL 点云 // 调整点云位置 for(int i=0;i<cloud2_pcl.points.size();i++) // 遍历第三个激光雷达的点云 { // cloud11 if(i<cloud11_pcl.points.size()) // 如果当前索引在第一个激光雷达点云范围内 { cloud11_pcl.points[i].x += lidar11_x; // 调整第一个激光雷达点云的 x 坐标 cloud11_pcl.points[i].y += lidar11_y; // 调整第一个激光雷达点云的 y 坐标 cloud11_pcl.points[i].z += lidar11_z; // 调整第一个激光雷达点云的 z 坐标 } // cloud12 if(i<cloud12_pcl.points.size()) // 如果当前索引在第二个激光雷达点云范围内 { cloud12_pcl.points[i].x += lidar12_x; // 调整第二个激光雷达点云的 x 坐标 cloud12_pcl.points[i].y += lidar12_y; // 调整第二个激光雷达点云的 y 坐标 cloud12_pcl.points[i].z += lidar12_z; // 调整第二个激光雷达点云的 z 坐标 } cloud2_pcl.points[i].x += lidar2_x; // 调整第三个激光雷达点云的 x 坐标 cloud2_pcl.points[i].y += lidar2_y; // 调整第三个激光雷达点云的 y 坐标 cloud2_pcl.points[i].z += lidar2_z; // 调整第三个激光雷达点云的 z 坐标 } // 合并点云并去除地面 PointCloudT cloud = cloud11_pcl + cloud12_pcl + cloud2_pcl; // 合并三个激光雷达的点云 PointCloudT cloud_filtered, cloud_segmented; // 定义过滤后的点云和分割后的点云 //剔除离群值 pcl::StatisticalOutlierRemoval<pcl::PointXYZI> statFilter; // 创建统计离群值去除对象 statFilter.setInputCloud(cloud.makeShared()); // 设置输入点云 statFilter.setMeanK(10); // 设置均值 K 值 statFilter.setStddevMulThresh(0.2); // 设置标准差乘数阈值 statFilter.filter(cloud_filtered); // 执行滤波，输出过滤后的点云 //RANSAC算法 分割 pcl::ModelCoefficients coefficients;//初始化模型系数 pcl::PointIndices::Ptr inliers(new pcl::PointIndices());//初始化索引参数 pcl::SACSegmentation<pcl::PointXYZI> segmentation;//创建算法 segmentation.setModelType(pcl::SACMODEL_PLANE);//设置分割模型为平面模型 segmentation.setMethodType(pcl::SAC_RANSAC);//设置迭代算法 segmentation.setMaxIterations(1000);//设置最大迭代次数 segmentation.setDistanceThreshold(0.01);//设置最大距离阈值 segmentation.setInputCloud(cloud_filtered.makeShared());//输入点云(已经过滤波和降采样) segmentation.segment(*inliers, coefficients);//输出点云结果 ×inliers是结果点云的索引，coe是模型系数 //发布模型系数 pcl_msgs::ModelCoefficients ros_coefficients; // 定义 ROS 模型系数消息 pcl_conversions::fromPCL(coefficients, ros_coefficients);//pcl->msg //发布抽样的内点索引 pcl_msgs::PointIndices ros_inliers; // 定义 ROS 内点索引消息 pcl_conversions::fromPCL(*inliers, ros_inliers); // 将 PCL 内点索引转换为 ROS 消息 //创建分割点云，从点云中提取内点 pcl::ExtractIndices<pcl::PointXYZI> extract; // 创建索引提取对象 extract.setInputCloud(cloud_filtered.makeShared()); // 设置输入点云 extract.setIndices(inliers); // 设置索引 extract.setNegative(true); // 提取外点 extract.filter(cloud_segmented); // 执行提取，输出分割后的点云 cout << "Combined cloud size: " << cloud_segmented.points.size() << endl; // 打印合并后的点云大小 // 发布合并后的点云 ros::Publisher pub = nh.advertise<sensor_msgs::PointCloud2>("/point_cloud_combined", 1); // 初始化发布者 sensor_msgs::PointCloud2 output; // 定义输出点云消息 pcl::toROSMsg(cloud_segmented, output); // 将 PCL 点云转换为 ROS 消息 output.header.frame_id = "point_cloud"; // 设置消息的坐标系 output.header.stamp = ros::Time::now(); // 设置消息的时间戳 pub.publish(output); // 发布合并后的点云 消息 } }; int main (int argc, char **argv) // 主函数 { ros::init (argc, argv, "pointcloud_combiner"); // 初始化 ROS 节点，节点名为 "pointcloud_combiner" pointcloud_combiner ptc; // 创建点云合并对象 cout << "Pointcloud Combine Begin." << endl; // 打印提示信息，表示点云合并开始 ros::spin(); // 进入 ROS 事件循环 return 0; // 返回 0，表示程序正常退出 }分别调用ServoCommands.py和KeyControl.py控制小车行驶。
ServoCommand.py如下：
#!/usr/bin/python3 # 指定脚本使用 Python 3 解释器执行 import rospy # 导入 ROS Python 客户端库，用于与 ROS 系统交互 from std_msgs.msg import Float64 # 导入 ROS 标准消息类型 Float64，用于发布浮点数数据 from ackermann_msgs.msg import AckermannDriveStamped # 导入 AckermannDriveStamped 消息类型，用于接收速度和转向命令 def set_throttle_steer(data): # 定义回调函数，用于处理接收到的 AckermannDriveStamped 消息 # 创建发布者对象，用于向各个车轮速度控制器发布速度命令 pub_vel_left_rear_wheel = rospy.Publisher("/car1/left_rear_wheel_velocity_controller/command", Float64, queue_size=10) pub_vel_right_rear_wheel = rospy.Publisher("/car1/right_rear_wheel_velocity_controller/command", Float64, queue_size=10) pub_vel_left_front_wheel = rospy.Publisher("/car1/left_front_wheel_velocity_controller/command", Float64, queue_size=10) pub_vel_right_front_wheel = rospy.Publisher("/car1/right_front_wheel_velocity_controller/command", Float64, queue_size=10) # 创建发布者对象，用于向转向控制器发布转向角度命令 pub_pos_left_steering_hinge = rospy.Publisher("/car1/left_steering_hinge_position_controller/command", Float64, queue_size=10) pub_pos_right_steering_hinge = rospy.Publisher("/car1/right_steering_hinge_position_controller/command", Float64, queue_size=10) # 从接收到的消息中提取速度值，并乘以 28 进行缩放 throttle = data.drive.speed * 28 # 从接收到的消息中提取转向角度值 steer = data.drive.steering_angle # 向四个车轮的速度控制器发布速度命令 pub_vel_left_rear_wheel.publish(throttle) pub_vel_right_rear_wheel.publish(throttle) pub_vel_left_front_wheel.publish(throttle) pub_vel_right_front_wheel.publish(throttle) # 向两个转向控制器发布转向角度命令 pub_pos_left_steering_hinge.publish(steer) pub_pos_right_steering_hinge.publish(steer) def servo_commands(): # 定义主函数，用于初始化 ROS 节点并订阅消息 # 初始化 ROS 节点，节点名称为 'servo_commands1'，anonymous=True 表示节点名称会自动添加唯一后缀 rospy.init_node('servo_commands1', anonymous=True) # 订阅 "/car1/ackermann_cmd_mux/output" 主题，接收 AckermannDriveStamped 消息，并调用 set_throttle_steer 回调函数处理 rospy.Subscriber("/car1/ackermann_cmd_mux/output", AckermannDriveStamped, set_throttle_steer) # 保持节点运行，等待消息并调用回调函数 rospy.spin() if __name__=='__main__': # 主程序入口，确保脚本直接运行时执行以下代码 try: # 调用 servo_commands 函数，启动 ROS 节点 servo_commands() except rospy.ROSInternalException: # 捕获 ROS 内部异常并忽略，防止程序崩溃 passKeyControl.py如下：
#! /usr/bin/env python # 指定脚本使用系统环境中的 Python 解释器执行 import rospy # 导入 ROS Python 客户端库，用于与 ROS 系统交互 import sys, select, termios, tty # 导入系统、终端设置和终端输入输出相关的库 from ackermann_msgs.msg import AckermannDriveStamped # 导入 AckermannDriveStamped 消息类型，用于发布速度和转向命令 def getKey(): # 定义函数，用于从终端获取单个按键输入 tty.setraw(sys.stdin.fileno()) # 设置终端为原始模式，禁用行缓冲和特殊字符处理 select.select([sys.stdin], [], [], 0) # 监听标准输入，等待用户按键 key = sys.stdin.read(1) # 读取用户按下的单个字符 termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings) # 恢复终端原始设置 return key # 返回用户按下的字符 settings = termios.tcgetattr(sys.stdin) # 获取当前终端设置并保存到变量 settings 中 def pub_cmd(): # 定义主函数，用于发布速度和转向命令 index = 1 # 定义车辆索引，默认为 1 rospy.init_node("pub_cmd") # 初始化 ROS 节点，节点名称为 "pub_cmd" # rospy.set_param("Car Index", 1) # 注释掉的代码：设置 ROS 参数 "Car Index" 为 1 pub = rospy.Publisher("/car"+str(index)+"/ackermann_cmd_mux/output", AckermannDriveStamped, queue_size=10) # 创建发布者对象，用于向指定主题发布 AckermannDriveStamped 消息 akm = AckermannDriveStamped() # 创建 AckermannDriveStamped 消息对象 while 1: # 进入无限循环，持续监听键盘输入 x=0 # 初始化速度变量，默认为 0 a=0 # 初始化转向角度变量，默认为 0 key = getKey() # 调用 getKey 函数获取用户按下的按键 rospy.loginfo("键盘已录入：%s", key) # 记录日志，显示用户按下的按键 if key == 'w': # 如果按下 'w' 键，设置前进速度 x=0.3 a=0 elif key == 's': # 如果按下 's' 键，设置后退速度 x=-0.3 a=0 elif key == 'a': # 如果按下 'a' 键，设置左转 x=0.3 a=0.7 elif key == 'd': # 如果按下 'd' 键，设置右转 x=0.3 a=-0.7 elif key == 'x': # 如果按下 'x' 键，停止车辆 x=0 a=0 elif key == 'o': # 如果按下 'o' 键，退出循环 break else: # 如果按下其他键，跳过当前循环 continue # akm.drive.speed = x*6.97674 # 注释掉的代码：将速度值乘以 6.97674 进行缩放 akm.drive.speed = x # 设置消息中的速度值 akm.drive.steering_angle = a*0.7 # 设置消息中的转向角度值，并乘以 0.7 进行缩放 # akm.drive.jerk = 2 # 注释掉的代码：设置消息中的 jerk 值 pub.publish(akm) # 发布消息到指定主题 if __name__=="__main__": # 主程序入口，确保脚本直接运行时执行以下代码 pub_cmd() # 调用 pub_cmd 函数，启动 ROS 节点 pass # 占位符，无实际作用调用visualize.py运行激光雷达点云可视化程序。
#!/usr/bin/env python3 """ 基于ROS和Open3D的持续视角点云可视化器 功能特性： - 支持动态点云更新与视角保持 - 集成统计离群点滤波与体素下采样 - 独立渲染线程保障可视化流畅性 """ import rospy import ros_numpy # ROS与Numpy数据转换接口 import open3d as o3d import numpy as np from sensor_msgs.msg import PointCloud2 from threading import Lock, Thread class PersistentViewVisualizer: def __init__(self): """初始化可视化窗口、ROS订阅及渲染线程""" # Open3D可视化窗口配置 self.vis = o3d.visualization.Visualizer() self.vis.create_window( window_name="PointCloud", width=1280, # 高清分辨率保障细节呈现 height=720 ) # 点云预处理配置 self.pcd = o3d.geometry.PointCloud() self.pcd = self.pcd.voxel_down_sample(voxel_size=0.5) # 体素下采样降噪 # 视角控制初始化 self.view_ctl = self.vis.get_view_control() self.current_view = None # 存储用户调整后的视角参数 # ROS通信配置 rospy.Subscriber( "/point_cloud_combined", PointCloud2, self.pcl_callback, queue_size=1, buff_size=2**24 # 64MB缓冲区应对大点云 ) rospy.loginfo("启动点云查看器") # 多线程安全机制 self.lock = Lock() # 防止渲染与数据更新冲突 self.render_thread = Thread(target=self._render_loop) self.render_thread.start() def _render_loop(self): """独立渲染线程保障可视化流畅性""" while not rospy.is_shutdown(): with self.lock: # 线程安全同步 self.vis.poll_events() self.vis.update_renderer() rospy.sleep(0.01) # 约60Hz刷新率平衡性能与流畅度 self.vis.destroy_window() def pcl_callback(self, msg): """点云数据处理回调函数 Args: msg (PointCloud2): ROS点云消息 """ try: # 离群点滤波处理 cl, ind = self.pcd.remove_statistical_outlier( nb_neighbors=20, # 邻域点数 std_ratio=2.0 # 标准差阈值 ) filtered_pcd = self.pcd.select_by_index(ind) self.pcd.points = filtered_pcd.points # 复用对象减少内存分配 # ROS转Open3D数据格式 pc_array = ros_numpy.point_cloud2.pointcloud2_to_array(msg) xyz = ros_numpy.point_cloud2.get_xyz_points(pc_array).reshape(-1,3) # 颜色通道处理 if 'rgb' in pc_array.dtype.names: rgb = ros_numpy.point_cloud2.split_rgb_field(pc_array)['rgb'] self.pcd.colors = o3d.utility.Vector3dVector(rgb.reshape(-1,3)/255.0) # 更新点云数据 with self.lock: self.pcd.points = o3d.utility.Vector3dVector(xyz) # 首次添加几何体 if not hasattr(self, '_geometry_added'): self.vis.add_geometry(self.pcd) self._geometry_added = True else: self.vis.update_geometry(self.pcd) # 视角记忆与恢复 if self.current_view: self.view_ctl.convert_from_pinhole_camera_parameters(self.current_view) else: # 初始化视角存储 self.current_view = self.view_ctl.convert_to_pinhole_camera_parameters() except Exception as e: rospy.logerr(f"点云处理异常: {str(e)}") # TODO: 添加异常类型细分处理 if __name__ == "__main__": rospy.init_node("persistent_view_pcl") visualizer = PersistentViewVisualizer() rospy.spin() # 保持节点运行3、实验操作
在~/catkin_ws文件夹中新建终端，输出以下命令开启仿真环境。
source devel/setup.bash roslaunch site_model spawn.launch
在~/catkin_ws文件夹中新建终端，输出以下命令激活激光雷达。
source devel/setup.bash python3 src/site_model/src/LidCamFusion/pointcloud_listener.py
在~/catkin_ws文件夹中新建终端，输出以下命令合并所有点云并固定其坐标。
source devel/setup.bash rosrun site_model pointcloud_combiner
在~/catkin_ws文件夹中新建两个终端，分别输出以下命令开启小车控制。
source devel/setup.bash python3 src/site_model/src/scripts_wx/ServoCommands.pysource devel/setup.bash python3 src/site_model/src/scripts_wx/KeyControl.py
在~/catkin_ws文件夹中新建终端，输出以下命令运行激光雷达点云可视化程序。
sudo -s source devel/setup.bash rosrun site_model visualize.py
点云可视化后可通过鼠标拖动、缩放以及旋转，控制小车行驶同时能看到点云的移动。
清理资源
计算巢—服务实例—复制服务实例ID，点击【删除】
在弹窗粘贴服务实例ID，并进行勾选，点击【确定删除】
完成安全验证后，即可成功释放实例。
回到云服务器ECS——实例，检查是否成功释放资源
关闭实验
在完成实验后，点击 结束实操
点击 取消 回到实验页面，点击 确定 跳转实验评分
请为本次实验评分，并给出您的建议，点击 确认，结束本次实验
该文章对您有帮助吗？