8.17 LiDAR
Replacing the LiDAR Head
If the LiDAR head is replaced or needs to be configured as a LiDAR head, the configuration parameters in the main software must be updated as follows:
Configuration file to modify:
/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 # Set this to 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
Starting the LiDAR – ROS 2
- Walker Tienkung · Voice & Vision: Orin (
192.168.41.2) - Walker Tienkung · Embodied Intelligence: Orin2 (
192.168.41.3)
cd livox_ws
source install/setup.bash
ros2 launch livox_ros_driver2 msg_MID360_launch.py
1. LiDAR Point Cloud Data
-
Description: Obtain raw LiDAR data.
-
Communication type: topic
-
Topic name:
/livox/lidar -
Message type:
sensor_msgs::msg::PointCloud2 -
Message format:
# 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 the "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 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 big‑endian?
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 -
Example code:
# Point cloud data is not intuitive to inspect directly.
# The following example subscribes to the topic and saves X‑Z projection
# images of the point cloud for visualization.
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'Cleared old images, saving to: {self.save_dir}')
def pointcloud_callback(self, msg):
now = time.time()
if now - self.last_save_time < 0.2: # Save once every 0.2 s
return
self.last_save_time = now
if self.image_count >= self.max_images:
self.get_logger().info('Saved 100 images, exiting.')
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('Received empty point cloud, skipping.')
return
# Extract X and Z, and use Z for color mapping
x = points[:, 0]
z = points[:, 2]
c = z # Z value used for color mapping
# Plot X‑Z view
plt.figure(figsize=(8, 6))
scatter = plt.scatter(x, z, c=c, cmap='viridis', s=0.5)
plt.colorbar(scatter, label='Z height [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)
# Save image
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'Saved image: {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. LiDAR IMU Data
-
Description: Obtain raw IMU data from the LiDAR.
-
Communication type: topic
-
Topic name:
/livox/imu -
Message type:
sensor_msgs::msg::Imu -
Message format:
# This message holds data from an IMU (Inertial Measurement Unit).
# Accelerations are in m/s^2 and rotational velocities in rad/s.
#
# If the covariance of a measurement is known, it should be filled in.
# A covariance matrix of all zeros will be interpreted as "covariance unknown".
# If there is no estimate for one of the elements, set the first element
# of the corresponding covariance matrix to -1.
std_msgs/Header header
builtin_interfaces/Time stamp
int32 sec
uint32 nanosec
string frame_id
geometry_msgs/Quaternion orientation
float64 x
float64 y
float64 z
float64 w
float64[9] orientation_covariance
geometry_msgs/Vector3 angular_velocity
float64 x
float64 y
float64 z
float64[9] angular_velocity_covariance
geometry_msgs/Vector3 linear_acceleration
float64 x
float64 y
float64 z
float64[9] linear_acceleration_covariance