版本：V2.0.5.x

8.17 雷达

更换雷达头方法

如果是雷达头或需要更换成雷达头，需要本体软件修改配置参数，更改方式如下：

需要更改的配置文件： /home/ubuntu/ros2ws/install/body_control/share/body_control/param/config_ethercat.yaml

auto_init: true
enable_check_ready: true
net_card_name: "enp4s0"
enable_arms: true
enable_legs: true
enable_imu: true
enable_hands: true
enable_6dof: true
enable_head: false                 #  这里设置成 false
enable_waist: true
enable_waist_home: true
enable_head_home: false
enable_power: false
selfMotor_idle_current: 0.3
# 0 - standard, 1 - extended, 2 - time_sharing_mixed 
slave_mode:
- 0     # left_leg
- 0     # right_leg
- 0     # xsens_imu

启动雷达方法-ROS2

天工行者·无界： Orin（192.168.41.2）

天工行者·无疆： Orin2（192.168.41.3）

cd livox_ws
source install/setup.bash
ros2 launch livox_ros_driver2 msg_MID360_launch.py

1. 雷达数据

说明：获取雷达原始数据
控制方式：topic
话题名称：/livox/lidar
类型：sensor_msgs::msg::PointCloud2

数据格式：

# This message holds a collection of N-dimensional points, which may
# contain additional information such as normals, intensity, etc. The
# point data is stored as a binary blob, its layout described by the
# contents of t he "fields" array.
#
# The point cloud data may be organized 2d (image-like) or 1d (unordered).
# Point clouds organized as 2d images may be produced by camera depth
sensors
# such as stereo or time-of-flight.
# Time of sensor data acquisition, and the coordinate frame ID (for 3d points). 
std_msgs/Header header
    builtin_interfaces/Time stamp
        int32 sec
        uint32 nanosec
    string frame_id
# 2D structure of the point cloud. If the cloud is unordered, height is
# 1 and width is the length of the point cloud.
uint32 height 
uint32 width
# Describes the channels and their layout in the binary data blob. 
PointField[] fields
    uint8 INT8       = 1
    uint8 UINT8     = 2
    uint8 INT16     = 3
    uint8 UINT16    = 4
    uint8 INT32     = 5
    uint8 UINT32    = 6
    uint8 FLOAT32 = 7
    uint8 FLOAT64 = 8
    string name          #
    uint32 offset       #
    uint8 datatype   #
    uint32 count        #

bool is_bigendian # Is this data bigendian?
uint32 point_step     # Length of a point in bytes 
uint32 row_step        # Length of a row in bytes
uint8[] data                # Actual point data, size is (row_step*height)
bool is_dense              # True if there are no invalid points

示例代码：

# 点云数据看起来不太直观，可用如下代码订阅话题，将点云数据保存起来，以便查看
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import PointCloud2
import sensor_msgs_py.point_cloud2 as pc2

import numpy as np
import os
import time
import glob
import matplotlib.pyplot as plt

class PointCloudXZPlotter(Node):
    def __init__(self):
        super().__init__('livox_pointcloud_xz_plotter')

        self.save_dir = 'pointcloud_imgs'
        self.prepare_directory()

        self.subscription = self.create_subscription(
            PointCloud2,
            '/livox/lidar',
            self.pointcloud_callback,
            10
        )

        self.image_count = 0
        self.max_images = 100
        self.last_save_time = time.time()

    def prepare_directory(self):
        if not os.path.exists(self.save_dir):
            os.makedirs(self.save_dir)
        else:
            png_files = glob.glob(os.path.join(self.save_dir, '*.png'))
            for f in png_files:
                os.remove(f)
            self.get_logger().info(f'清空旧图片，准备保存至目录：{self.save_dir}')

    def pointcloud_callback(self, msg):
        now = time.time()
        if now - self.last_save_time < 0.2:  # 每 0.2 秒保存一次
            return
        self.last_save_time = now

        if self.image_count >= self.max_images:
            self.get_logger().info('已保存 100 张图片，退出程序。')
            rclpy.shutdown()
            return

        points = np.array([
            [x, y, z]
            for x, y, z in pc2.read_points(msg, field_names=("x", "y", "z"), skip_nans=True)
        ])

        if len(points) == 0:
            self.get_logger().warn('收到的点云为空，跳过本次保存。')
            return

        # 提取 X 和 Z，用 Z 作为颜色映射
        x = points[:, 0]
        z = points[:, 2]
        c = z  # Z 值用于颜色映射

        # 绘制 X-Z 图
        plt.figure(figsize=(8, 6))
        scatter = plt.scatter(x, z, c=c, cmap='viridis', s=0.5)  # viridis 是蓝-绿-黄渐变
        plt.colorbar(scatter, label='Z 高度 [m]')
        plt.title(f'PointCloud X-Z View #{self.image_count}')
        plt.xlabel('X [m]')
        plt.ylabel('Z [m]')
        plt.axis('equal')
        plt.grid(True)

        # 保存图像
        image_path = os.path.join(self.save_dir, f'pointcloud_{self.image_count}.png')
        plt.savefig(image_path, dpi=150)
        plt.close()
        self.get_logger().info(f'保存图像: {image_path}')
        self.image_count += 1

def main(args=None):
    rclpy.init(args=args)
    node = PointCloudXZPlotter()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

2. 雷达 IMU 数据

说明：获取雷达的 IMU 原始数据
控制方式：topic
话题名称：/livox/imu
类型：sensor_msgs::msg::Imu

数据格式：

# This is a message to hold data from an IMU (Inertial Measurement Unit) #
# Accelerations should be in m/s^2 (not in g's), and rotational velocity should be in rad/sec
#
# If the covariance of the measurement is known, it should be filled in (if all you know is the
# variance of each measurement, e.g. from the datasheet, just put those along the diagonal)
# A covariance matrix of all zeros will be interpreted as "covariance unknown", and to use the
# data a covariance will have to be assumed or gotten from some other source #
# If you have no estimate for one of the data elements (e.g. your IMU doesn't produce an
# orientation estimate), please set element 0 of the associated covariance matrix to -1
# If you are interpreting this message, please check for a value of -1 in the first element of each
# covariance matrix, and disregard the associated estimate.
std_msgs/Header header
    builtin_interfaces/Time stamp
        int32 sec
        uint32 nanosec
    string frame_id
    
geometry_msgs/Quaternion orientation
    float64 x 0
    float64 y 0
    float64 z 0
    float64 w 1
float64[9] orientation_covariance # Row major about x, y, z axes
geometry_msgs/Vector3 angular_velocity
    float64 x
    float64 y
    float64 z
float64[9] angular_velocity_covariance # Row major about x, y, z axes

geometry_msgs/Vector3 linear_acceleration
    float64 x
    float64 y
    float64 z
float64[9] linear_acceleration_covariance # Row major x, y z

更换雷达头方法​

启动雷达方法-ROS2​

1. 雷达数据​

2. 雷达 IMU 数据​

更换雷达头方法

启动雷达方法-ROS2

1. 雷达数据

2. 雷达 IMU 数据