Height Mapping Repository

This repository performs height mapping on a Livox MID360 Lidar. It contains a mild fork of FAST-LIO, which includes publishing a frame transform such that the odom frame is 'z-up' -- useful for height mapping. Pointclouds transformed into the odom frame from FAST-LIO are then minimized, with some outlier detection and filling of unobserved squares.

A large heightmap is maintained on one thread; this heightmap remains axis aligned with the odom frame, and ingests points from lidar as fast as possible. On a separate thread, the larger heightmap is queried from an arbitary SE(2) pose. Maintaining the larger map helps avoid frequent map shifts, as well as having to deal with orientation changes during map shifting.

Installation

The docker branch of this repository maintains a docker image which can be used to run the heightmapping software in ROS2 humble.

Heightmap only

To run only the heightmap, this code can be cloned (be sure to run git submodule update --init --recursive to pull the submoduled FAST-LIO fork); create an environment variable $HEIGHT_MAPPING_ROOT=<path-to-repo>. If using the 'nice' G1 in Amber lab, the configuration for the lidar is found in /src/fastlio_vel/config/livox_MID360_config.json. If using a different lidar, the IP addresses of the host and the lidar need to be set appropriately in this file. Then, build docker image. To install terminal aliases, run source scripts/setup_aliases.bash. Using the installed terminal aliases, run livox, which performs:

source /opt/ros/humble/setup.bash && source ~/ws_livox/install/setup.bash

Then, in ~/repos/height_mapping run colcon build --symlink-install to build both the FAST-LIO fork and height_mapping. Finally, run source install/setup.bash from $HEIGHT_MAPPING_ROOT to source the install, or use the alias height_mapping.

Integration with downstream project

To integrate with a downstream project, simply submodule the main repository (this has not been tested yet...). Note dependencies on the Livox SDK2 and livox_ros_driver2. Additionally, note that the file /src/fastlio_vel/config/livox_MID360_config.json may need to be updated with the proper IP addresses for the Lidar (unless the robot is the Amber lab 'nice' Unitree G1).

Heightmap spoofing

To build with the option to spoof a heightmap by querying a mujoco file, compile the workspace using

colcon build --symlink-install --cmake-args -DBUILD_MUJOCO_SPOOF_NODE=ON

Then to run the heightmap spoofing, run lidar, fast_lio_vel (for pose estimates) and the spoofed heightmap node.

Running

Currently, to run the heightmap, the Lidar must be launched first, then the height mapping launch file (these will be combined soon). To launch the lidar, run

ros2 launch fast_lio_vel livox.launch.py

And then to run the height mapping code:

ros2 launch height_mapping height_mapping.launch.py --use_sim_time:=<bool> --rviz:=<bool>

Where use_sim_time should be included, as true, if running the height mapping on a pre-recorded bag file (which should be played with --clock), and --rviz:=<true should be included to launch an rviz session to visualize the output of the height mapping.

The launch file launches a height mapping node with parameters in $HEIGHT_MAPPING_ROOT/src/height_mapping/config/height_mapping.yaml, and FAST-LIO gets launch with parameters in $HEIGHT_MAPPING_ROOT/src/fastlio_vel/config/mid360_g1.yaml.

Similarly, to run the heightmap spoofing code, run:

ros2 launch height_mapping height_spoof.launch.py --use_sim_time:=<bool> --rviz:=<bool>

This will load the .xml file in rsc folder into Mujoco to use for raycasting.

Odom via Motion Capture

To use motion capture as the source of odometry, ensure the following

• Networking Setup
  • The host computer (running Motive) connects to the Motive Ethernet box via Ethernet (likely over the IP address 169.245.255). This network should not be used for NatNet streaming.
  • Connect the host computer and client computer (running ROS2) to a different local network, i.e. 192.168.1.1 for the client, and 192.168.1.2 for the host (configure the IPV4 settings manually, with network mask 255.255.255.0).
  • Ensure both computers can ping each other over this local network. If not, check firewall settings on the host computer.
• The motive streaming client has the following settings:
  • NatNet: enabled
  • Local Interface: 192.168.1.2 (or appropriate IP for the host computer, running motive)
  • Transmission Type: Multicast
  • Rigid Bodies: enabled
  • Up Axis: Z-Axis
  • VRPN: disabled
• Ensure the NatNet ROS2 client is installed from here, and launch the natnet client with ros2 launch natnet_ros2 gui_natnet_ros2.launch.py In the resulting GUI, set:
  • ROS Domain ID: 2 (or appropriate ROS domain ID)
  • Server IP: 192.168.1.2 (or appropriate IP for the host computer, running motive)
  • Client IP: 192.168.1.1 (or appropriate IP for the client machine)
  • publish rigid body: checked (true)
  • World frame: odom

Upon clicking 'Start', the NatNet client should begin streaming motion capture data to the ROS2 network. It will publish to the topic /g1/pose as a geometry_msgs/msg/PoseStamped message (assuming the rigid body in Motive is named 'g1').