From df3e88f4b58dbdd6f13c431801337270b5f24dd5 Mon Sep 17 00:00:00 2001 From: bboyack Date: Tue, 31 Mar 2026 14:24:28 -0600 Subject: [PATCH] Added docker functionality. Added removal of NaNs for Ouster Lidar. Updated README for each of these. --- README.md | 47 +++++++++++++++++++++++++- docker/compose.yaml | 23 +++++++++++++ docker/dockerfile | 40 +++++++++++++++++++++++ docker/run_docker.sh | 17 ++++++++++ include/pointcloud_preprocess.h | 4 +-- src/pointcloud_preprocess.cc | 58 +++++++++++++++++++++++++++++++++ 6 files changed, 186 insertions(+), 3 deletions(-) create mode 100644 docker/compose.yaml create mode 100644 docker/dockerfile create mode 100755 docker/run_docker.sh diff --git a/README.md b/README.md index f9dbf6c..5841e50 100644 --- a/README.md +++ b/README.md @@ -27,7 +27,9 @@ All experiments, including both simulation and real-world experiments in paper a - PCL ## Build -``` + +### Local +``` bash mkdir -p /workspace/src cd /workspace/src git clone https://github.com/M-Evanovic/LL-Localizer.git @@ -36,6 +38,32 @@ catkin build source devel/setup.bash ``` +### Docker +
+ Docker details (click to expand) + + > Docker setup was tested on Ubuntu 24.04. + + Need to give docker permission to use gui. Run the following on your host machine. + + ``` bash + xhost +local:docker + ``` + + Clone repo, build docker image, and start container. + ``` bash + git clone https://github.com/M-Evanovic/LL-Localizer.git + cd LL-Localizer + ./docker/run_docker.sh + ``` + + In a new terminal, enter the container so you can launch ll_localizer in one and play a rosbag in another. + > Make sure to change the volume path for your bags in the `docker/compose.yaml` file to match the path to your data. + ```bash + docker exec -it ll_localizer bash + ``` +
+ ## Run SLAM Mode - Set the path ```/localization_param/map_file_name``` to ```" "``` in ```/config/config.yaml```. When no prior map is available, the system automatically switches to SLAM mode. - Derectly launch ll_localizer: @@ -59,6 +87,23 @@ roslaunch ll_localizer localizer_velodyne.launch rosbag play yourbag.bag ``` +## Debug +
+ Debug Ouster + + - If you get an error when running an ouster saying it can't find the 'ring' field, it's likely because you have a newer lidar. The old lidar's ring fild was a `uint8_t` but the newer ones are `uint16_t`. This change will need to be made in `pointcloud_preprocess.h` on lines 34 and 50. + +
+ +
+ Immediate Crash + + - If ll_localizer crashes right after you start playing your rosbag, your lidar points might contain NaN values. Remove NaN values from data before processing. This was implemented for the ouster lidar on lines 144-146 of `pointcloud_preprocess.cc`. Similar should be possible with the other lidar types. + +
+ + + ## Datasets - [NCLT](https://robots.engin.umich.edu/nclt/): The University of Michigan North Campus Long-Term Vision and LIDAR Dataset - [M2DGR](https://github.com/SJTU-ViSYS/M2DGR): A Multi-modal and Multi-scenario SLAM Dataset for Ground Robots diff --git a/docker/compose.yaml b/docker/compose.yaml new file mode 100644 index 0000000..54baa43 --- /dev/null +++ b/docker/compose.yaml @@ -0,0 +1,23 @@ +services: + ll_localizer: + build: + context: .. + dockerfile: docker/dockerfile + image: ll_localizer + container_name: ll_localizer + volumes: + - ../:/root/catkin_ws/src/LL-Localizer # Volumes in entire LL-Localizer folder to allow customization + - ${HOME}/data_byu/bags/longterm_mapping/ros1_bags:/bags # Change to match location of bag data + - /tmp/.X11-unix:/tmp/.X11-unix # Needed to display RViz within the container + environment: + DISPLAY: "${DISPLAY}" + DISABLE_ROS1_EOL_WARNINGS: 1 + NVIDIA_VISIBLE_DEVICES: all + NVIDIA_DRIVER_CAPABILITIES: all + runtime: nvidia + working_dir: /root/catkin_ws + network_mode: host + stdin_open: true + tty: true + # command: ["/ros_entrypoint.sh", "roscore"] + \ No newline at end of file diff --git a/docker/dockerfile b/docker/dockerfile new file mode 100644 index 0000000..2ddc206 --- /dev/null +++ b/docker/dockerfile @@ -0,0 +1,40 @@ +# Start from a ROS Noetic base image. +FROM ros:noetic + +# Install common build tools and necessary dependencies +RUN apt-get update && apt-get install -y --no-install-recommends \ + git \ + libgoogle-glog-dev \ + libeigen3-dev \ + libpcl-dev \ + libtbb-dev \ + python3-catkin-tools \ + && rm -rf /var/lib/apt/lists/* + +# --- Install ROS Dependencies --- +RUN apt-get update && apt-get install -y --no-install-recommends \ + ros-noetic-catkin \ + ros-noetic-rviz \ + ros-noetic-pcl-ros \ + ros-noetic-eigen-conversions \ + pcl-tools \ + && rm -rf /var/lib/apt/lists/* + +# --- Copy in LL-Localizer Source Code --- +# Set the working directory +WORKDIR /root/catkin_ws/src/ + +# Copy your project source code into the Docker image +COPY . ./LL-Localizer + +WORKDIR /root/catkin_ws +# Build the ROS workspace +RUN /bin/bash -c "source /opt/ros/noetic/setup.bash && \ + catkin_make -j6 --pkg ll_localizer" + + +RUN echo "source /opt/ros/noetic/setup.bash" >> ~/.bashrc && \ + echo "if [ -f ~/catkin_ws/devel/setup.bash ]; then" >> ~/.bashrc && \ + echo " source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc && \ + echo "fi" >> ~/.bashrc + diff --git a/docker/run_docker.sh b/docker/run_docker.sh new file mode 100755 index 0000000..60ea76d --- /dev/null +++ b/docker/run_docker.sh @@ -0,0 +1,17 @@ +#!/bin/bash + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" + +# Allow local docker access to X server +xhost +local:docker + +# Ensure access is revoked on script exit +trap 'xhost -local:docker' EXIT + +# Run script in the directory of the script +cd "$SCRIPT_DIR" + +docker compose -f compose.yaml -p ll_localizer up -d + +docker exec -it ll_localizer /bin/bash + diff --git a/include/pointcloud_preprocess.h b/include/pointcloud_preprocess.h index bde7b19..b1f7749 100644 --- a/include/pointcloud_preprocess.h +++ b/include/pointcloud_preprocess.h @@ -31,7 +31,7 @@ struct EIGEN_ALIGN16 Point { float intensity; uint32_t t; uint16_t reflectivity; - uint8_t ring; + uint16_t ring; uint16_t ambient; uint32_t range; EIGEN_MAKE_ALIGNED_OPERATOR_NEW @@ -47,7 +47,7 @@ POINT_CLOUD_REGISTER_POINT_STRUCT(ouster_ros::Point, // use std::uint32_t to avoid conflicting with pcl::uint32_t (std::uint32_t, t, t) (std::uint16_t, reflectivity, reflectivity) - (std::uint8_t, ring, ring) + (std::uint16_t, ring, ring) (std::uint16_t, ambient, ambient) (std::uint32_t, range, range) ) diff --git a/src/pointcloud_preprocess.cc b/src/pointcloud_preprocess.cc index 0b3d756..7d254c1 100644 --- a/src/pointcloud_preprocess.cc +++ b/src/pointcloud_preprocess.cc @@ -82,11 +82,69 @@ void PointCloudPreprocess::AviaHandler(const livox_ros_driver::CustomMsg::ConstP } } +// Copied directly from newer version of PCL +template void +removeNaNFromPointCloud (const pcl::PointCloud &cloud_in, + pcl::PointCloud &cloud_out, + pcl::Indices &index) +{ + // If the clouds are not the same, prepare the output + if (&cloud_in != &cloud_out) + { + cloud_out.header = cloud_in.header; + cloud_out.resize (cloud_in.size ()); + cloud_out.sensor_origin_ = cloud_in.sensor_origin_; + cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_; + } + // Reserve enough space for the indices + index.resize (cloud_in.size ()); + + // If the data is dense, we don't need to check for NaN + if (cloud_in.is_dense) + { + // Simply copy the data + cloud_out = cloud_in; + for (std::size_t j = 0; j < cloud_out.size (); ++j) + index[j] = j; + } + else + { + std::size_t j = 0; + for (std::size_t i = 0; i < cloud_in.size (); ++i) + { + if (!std::isfinite (cloud_in[i].x) || + !std::isfinite (cloud_in[i].y) || + !std::isfinite (cloud_in[i].z)) + continue; + cloud_out[j] = cloud_in[i]; + index[j] = i; + j++; + } + if (j != cloud_in.size ()) + { + // Resize to the correct size + cloud_out.resize (j); + index.resize (j); + } + + cloud_out.height = 1; + cloud_out.width = static_cast(j); + + // Removing bad points => dense (note: 'dense' doesn't mean 'organized') + cloud_out.is_dense = true; + } +} + void PointCloudPreprocess::Oust64Handler(const sensor_msgs::PointCloud2::ConstPtr &msg) { cloud_out.clear(); cloud_full.clear(); pcl::PointCloud pl_orig; pcl::fromROSMsg(*msg, pl_orig); + + // Remove NaNs + std::vector indices; + removeNaNFromPointCloud(pl_orig, pl_orig, indices); + int plsize = pl_orig.size(); cloud_out.reserve(plsize);