8.17 Radar
How to Replace Radar Head
If you need to replace the radar head, you need to modify the configuration parameters in the main body software 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 to false here
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
How to Start Radar - ROS2
Voice & Vision: Orin (192.168.41.2)
Embodied Intelligence: Orin2 (192.168.41.3)
cd livox_ws
source install/setup.bash
ros2 launch livox_ros_driver2 msg_MID360_launch.py
1. Radar Data
-
Description: Get radar raw data
-
Control method:
topic -
Topic name:
/livox/lidar -
Type:
sensor_msgs::msg::PointCloud2 -
Data 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 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 -
Example code:
# The point cloud data doesn't look very intuitive, you can use the following code to subscribe to the topic and save the point cloud data for viewing
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, ready to save to directory: {self.save_dir}')
def pointcloud_callback(self, msg):
now = time.time()
if now - self.last_save_time < 0.2: # Save once every 0.2 seconds
return
self.last_save_time = now
if self.image_count >= self.max_images:
self.get_logger().info('Saved 100 images, exiting program.')
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 this save.')
return
# Extract X and Z, use Z as color map
x = points[:, 0]
z = points[:, 2]
c = z # Z value for color mapping
# Draw X-Z plot
plt.figure(figsize=(8, 6))
scatter = plt.scatter(x, z, c=c, cmap='viridis', s=0.5) # viridis is blue-green-yellow gradient
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. Radar IMU Data
-
Description: Get radar IMU raw data
-
Control method:
topic -
Topic name:
/livox/imu -
Type:
sensor_msgs::msg::Imu -
Data format:
# 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