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