From 1ad47b01e1eeb1c3a5e226537db0c2bd26254d14 Mon Sep 17 00:00:00 2001 From: mice85 Date: Mon, 23 Sep 2024 17:08:09 +0100 Subject: [PATCH 01/17] script created to run fast-lio2 --- scripts/localisation_benchmark/fast_lio2.py | 39 +++++++++++++++++++++ 1 file changed, 39 insertions(+) create mode 100644 scripts/localisation_benchmark/fast_lio2.py diff --git a/scripts/localisation_benchmark/fast_lio2.py b/scripts/localisation_benchmark/fast_lio2.py new file mode 100644 index 0000000..41aa430 --- /dev/null +++ b/scripts/localisation_benchmark/fast_lio2.py @@ -0,0 +1,39 @@ +import libtmux + +if __name__ == "__main__": + + path = '/media/mice85/blackdrive/oxford_spires_dataset/data/' + sequence = '2024-03-14_blenhein_1' + seq_path = path + sequence + + server = libtmux.Server() + server.cmd('new-session', '-d', '-P', '-F#{session_id}') + session = server.sessions[0] + window_1 = session.new_window(attach=False, window_name="roscore") + window_2 = session.new_window(attach=False, window_name="rosbag") + pane_1 = window_1.split() + pane_2 = window_2.split() + + pane_1.send_keys('cd ~/oxford-lab/oxford_ws/') + pane_1.send_keys('source devel/setup.bash') + pane_1.send_keys('roslaunch fast_lio {}.launch'.format(sequence)) + + pane_2.send_keys('cd {}/rosbag/'.format(seq_path)) + pane_2.send_keys('rosbag play *.bag --clock') + + # Check when the rosbag is "Done." + while (1): + pane_2.clear() + cap_curr = pane_2.capture_pane() + for line in cap_curr: + if "[RUNNING]" in line: + print(line) + if 'Done.' in cap_curr: + break + + # TODO: Copy to output folder + + # TODO: Transform to TUM format + + + server.kill() \ No newline at end of file From c9cb187504e9e34bd35919fcba3817cee32f6114 Mon Sep 17 00:00:00 2001 From: mice85 Date: Mon, 23 Sep 2024 17:17:47 +0100 Subject: [PATCH 02/17] folders and example for roslaunch --- .../fast_lio2/2024-03-14_blenhein_1.launch | 27 +++++++++++++++++++ 1 file changed, 27 insertions(+) create mode 100644 spires_ros/fast_lio2/2024-03-14_blenhein_1.launch diff --git a/spires_ros/fast_lio2/2024-03-14_blenhein_1.launch b/spires_ros/fast_lio2/2024-03-14_blenhein_1.launch new file mode 100644 index 0000000..f9c6101 --- /dev/null +++ b/spires_ros/fast_lio2/2024-03-14_blenhein_1.launch @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + + + + + + + + From 65e10143528cd7461860aea8d3225d0664b03710 Mon Sep 17 00:00:00 2001 From: mice85 Date: Mon, 23 Sep 2024 17:25:20 +0100 Subject: [PATCH 03/17] include config for fast_lio --- spires_ros/fast_lio2/hesai.yaml | 38 +++++++++++++++++++++++++++++++++ 1 file changed, 38 insertions(+) create mode 100644 spires_ros/fast_lio2/hesai.yaml diff --git a/spires_ros/fast_lio2/hesai.yaml b/spires_ros/fast_lio2/hesai.yaml new file mode 100644 index 0000000..4582c31 --- /dev/null +++ b/spires_ros/fast_lio2/hesai.yaml @@ -0,0 +1,38 @@ +common: + #lid_topic: "/os_cloud_node/points" + #imu_topic: "/os_cloud_node/imu" + lid_topic: "/hesai/pandar/" + imu_topic: "/alphasense_driver_ros/imu" + time_sync_en: false # ONLY turn on when external time synchronization is really not possible + time_offset_lidar_to_imu: 0.0 # Time offset between lidar and IMU calibrated by other algorithms, e.g. LI-Init (can be found in README). + # This param will take effect no matter what time_sync_en is. So if the time offset is not known exactly, please set as 0.0 + +preprocess: + lidar_type: 5 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, + scan_line: 64 + timestamp_unit: 3 # 0-second, 1-milisecond, 2-microsecond, 3-nanosecond. + blind: 4 + +mapping: + acc_cov: 0.1 + gyr_cov: 0.1 + b_acc_cov: 0.0001 + b_gyr_cov: 0.0001 + fov_degree: 180 + det_range: 150.0 + extrinsic_est_en: false # true: enable the online estimation of IMU-LiDAR extrinsic + extrinsic_T: [ -0.00459163, -0.00200303, -0.0547992 ] + extrinsic_R: [ -0.0106465, -0.999911, 0.00806769, + 0.999941, -0.0106296, 0.00171369, + -0.00166849, 0.00808546, 0.999966] + +publish: + path_en: false + scan_publish_en: true # false: close all the point cloud output + dense_publish_en: true # false: low down the points number in a global-frame point clouds scan. + scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame + +pcd_save: + pcd_save_en: false + interval: 10 # how many LiDAR frames saved in each pcd file; + # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. From 54de3ab64ed257f3055283d9c649dedecbfef6e9 Mon Sep 17 00:00:00 2001 From: mice85 Date: Tue, 24 Sep 2024 11:17:14 +0100 Subject: [PATCH 04/17] new structure for spires_ros --- .../fast_lio2/{ => launch_osd}/2024-03-14_blenhein_1.launch | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename spires_ros/fast_lio2/{ => launch_osd}/2024-03-14_blenhein_1.launch (100%) diff --git a/spires_ros/fast_lio2/2024-03-14_blenhein_1.launch b/spires_ros/fast_lio2/launch_osd/2024-03-14_blenhein_1.launch similarity index 100% rename from spires_ros/fast_lio2/2024-03-14_blenhein_1.launch rename to spires_ros/fast_lio2/launch_osd/2024-03-14_blenhein_1.launch From efe1a0a8c898640e733a4674519207921eb5b6c7 Mon Sep 17 00:00:00 2001 From: mice85 Date: Tue, 24 Sep 2024 16:08:57 +0100 Subject: [PATCH 05/17] New docker for trajectory evaluation --- .docker_loc/Dockerfile | 81 ++++++++++++++++++++++++++++++++++ .docker_loc/docker-compose.yml | 20 +++++++++ 2 files changed, 101 insertions(+) create mode 100644 .docker_loc/Dockerfile create mode 100644 .docker_loc/docker-compose.yml diff --git a/.docker_loc/Dockerfile b/.docker_loc/Dockerfile new file mode 100644 index 0000000..acd57ea --- /dev/null +++ b/.docker_loc/Dockerfile @@ -0,0 +1,81 @@ +############## +# Base image # +############## +ARG UBUNTU_VERSION=20.04 +ARG NVIDIA_CUDA_VERSION=11.8.0 + +FROM nvidia/cuda:${NVIDIA_CUDA_VERSION}-devel-ubuntu${UBUNTU_VERSION} + +# Install ROS +ARG ROS_DISTRO=noetic +# Prevent stop building ubuntu at time zone selection. +ENV DEBIAN_FRONTEND=noninteractive +RUN apt-get update && \ + apt-get install -y --no-install-recommends \ + lsb-release \ + curl && \ + sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' && \ + curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | apt-key add - && \ + apt-get update && \ + apt install -y \ + ros-noetic-ros-base \ + python3-rosdep \ + python3-catkin-tools \ + ros-noetic-catkin &&\ + rm -rf /var/lib/apt/lists/* && \ + echo "source /opt/ros/noetic/setup.bash" >> ~/.bashrc && \ + rosdep init && \ + rosdep update + +# Install tools +RUN apt-get update && \ + apt-get install -y --no-install-recommends \ + build-essential \ + cmake \ + curl \ + git \ + python-is-python3 \ + python3.8-dev \ + python3-pip \ + vim \ + wget && \ + rm -rf /var/lib/apt/lists/* + +RUN python -m pip install --no-cache-dir --upgrade pip setuptools pathtools promise pybind11 + +# create catkin workspace +ARG CATKIN_WS_DIR=/home/catkin_ws +RUN . /opt/ros/noetic/setup.sh && mkdir -p ${CATKIN_WS_DIR}/src && cd ${CATKIN_WS_DIR} && catkin build && \ + echo "source ${CATKIN_WS_DIR}/devel/setup.bash" >> ~/.bashrc +# create requered packages +RUN git clone https://github.com/ori-drs/FAST_LIO.git ${CATKIN_WS_DIR}/src/FAST_LIO +RUN cd ${CATKIN_WS_DIR} && catkin build + +# Trajectory evaluation dependences +RUN pip install --user libtmux +RUN pip install evo + +# Final setup +ARG GID +ARG UID +ENV UNAME=docker_dev +RUN addgroup --gid $GID $UNAME +RUN adduser --disabled-password --gecos '' --uid $UID --gid $GID $UNAME + +ARG SPIRES_DIR=/home/oxford_spires_dataset +WORKDIR ${SPIRES_DIR} + +COPY ./oxford_spires_utils/ ${SPIRES_DIR}/oxford_spires_utils/ +COPY ./spires_ros/fast_lio2/launch_osd ${CATKIN_WS_DIR}/src/FAST_LIO/launch +COPY ./spires_ros/fast_lio2/hesai.yaml ${CATKIN_WS_DIR}/src/FAST_LIO/config +COPY ./pyproject.toml ${SPIRES_DIR}/pyproject.toml +RUN pip install -e . + +# Make the outputs of the container match the host + +RUN chown -R ${UID}:${GID} ${SPIRES_DIR} +USER ${UNAME} +RUN echo "PS1='${debian_chroot:+($debian_chroot)}\[\033[01;33m\]\u@docker-\h\[\033[00m\]:\[\033[01;34m\]\w\[\033[00m\]\$ '" >> ~/.bashrc +RUN echo "alias recon_benchmark=\"python ${SPIRES_DIR}/scripts/reconstruction_benchmark/main.py\"" >> ~/.bashrc + +CMD ["/bin/bash"] \ No newline at end of file diff --git a/.docker_loc/docker-compose.yml b/.docker_loc/docker-compose.yml new file mode 100644 index 0000000..3e0f459 --- /dev/null +++ b/.docker_loc/docker-compose.yml @@ -0,0 +1,20 @@ +services: + spires: + container_name: spires_container + build: + context: .. + dockerfile: .docker_loc/Dockerfile + args: + - USERNAME=${USER:-user} + - UID=${UID} + - GID=${GID} + environment: + - NVIDIA_VISIBLE_DEVICES=all + - NVIDIA_DRIVER_CAPABILITIES=all + runtime: nvidia + network_mode: "host" + tty: true + volumes: + - ../scripts:${SPIRES_DIR}/scripts + - ../tests:${SPIRES_DIR}/tests + - ${HOST_SPIRES_DATA_DIR}:${SPIRES_DATA_DIR} From a58eafc5ec2454b5e15e42f33271496cdcd65958 Mon Sep 17 00:00:00 2001 From: mice85 Date: Tue, 24 Sep 2024 16:16:29 +0100 Subject: [PATCH 06/17] paths adapted to the docker --- scripts/localisation_benchmark/fast_lio2.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/scripts/localisation_benchmark/fast_lio2.py b/scripts/localisation_benchmark/fast_lio2.py index 41aa430..cad3cfb 100644 --- a/scripts/localisation_benchmark/fast_lio2.py +++ b/scripts/localisation_benchmark/fast_lio2.py @@ -2,26 +2,26 @@ if __name__ == "__main__": - path = '/media/mice85/blackdrive/oxford_spires_dataset/data/' + path = '/home/oxford_spires_dataset/data/' sequence = '2024-03-14_blenhein_1' seq_path = path + sequence server = libtmux.Server() server.cmd('new-session', '-d', '-P', '-F#{session_id}') session = server.sessions[0] - window_1 = session.new_window(attach=False, window_name="roscore") + window_1 = session.new_window(attach=False, window_name="launch fast_lio2") window_2 = session.new_window(attach=False, window_name="rosbag") pane_1 = window_1.split() pane_2 = window_2.split() - pane_1.send_keys('cd ~/oxford-lab/oxford_ws/') + pane_1.send_keys('cd /home/catkin_ws/') pane_1.send_keys('source devel/setup.bash') pane_1.send_keys('roslaunch fast_lio {}.launch'.format(sequence)) pane_2.send_keys('cd {}/rosbag/'.format(seq_path)) pane_2.send_keys('rosbag play *.bag --clock') - # Check when the rosbag is "Done." + # Check if the rosbag is "Done." while (1): pane_2.clear() cap_curr = pane_2.capture_pane() From 6526d4112a976abf7aebc60cf36cb7020d95721f Mon Sep 17 00:00:00 2001 From: mice85 Date: Tue, 24 Sep 2024 17:17:47 +0100 Subject: [PATCH 07/17] tmux installation added to the docker file --- .docker_loc/Dockerfile | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/.docker_loc/Dockerfile b/.docker_loc/Dockerfile index acd57ea..fce0c6b 100644 --- a/.docker_loc/Dockerfile +++ b/.docker_loc/Dockerfile @@ -52,7 +52,9 @@ RUN git clone https://github.com/ori-drs/FAST_LIO.git ${CATKIN_WS_DIR}/src/FAST_ RUN cd ${CATKIN_WS_DIR} && catkin build # Trajectory evaluation dependences -RUN pip install --user libtmux +RUN apt-get update +RUN apt-get install tmux -y +RUN pip install libtmux RUN pip install evo # Final setup From eb8bd84314e6e613002724b35537ea4d1785f796 Mon Sep 17 00:00:00 2001 From: mice85 Date: Wed, 25 Sep 2024 10:50:21 +0100 Subject: [PATCH 08/17] lio_sam ros configuration added, and new output for fast_lio --- .../launch_osd/2024-03-14_blenhein_1.launch | 4 +- spires_ros/lio_sam/frontier_hesai_19.yaml | 184 ++++++++++++++++++ .../launch_lsam/2024-03-14_blenhein_1.yaml | 184 ++++++++++++++++++ 3 files changed, 370 insertions(+), 2 deletions(-) create mode 100644 spires_ros/lio_sam/frontier_hesai_19.yaml create mode 100644 spires_ros/lio_sam/launch_lsam/2024-03-14_blenhein_1.yaml diff --git a/spires_ros/fast_lio2/launch_osd/2024-03-14_blenhein_1.launch b/spires_ros/fast_lio2/launch_osd/2024-03-14_blenhein_1.launch index f9c6101..288f8ca 100644 --- a/spires_ros/fast_lio2/launch_osd/2024-03-14_blenhein_1.launch +++ b/spires_ros/fast_lio2/launch_osd/2024-03-14_blenhein_1.launch @@ -15,9 +15,9 @@ - + - + + + + + + + + + + + + + From 05d021d6fe6383375ca0936291dd037108684617 Mon Sep 17 00:00:00 2001 From: mice85 Date: Wed, 25 Sep 2024 16:55:10 +0100 Subject: [PATCH 14/17] rviz commented --- spires_ros/lio_sam/launch_lsam/2024-03-14_blenhein_1.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/spires_ros/lio_sam/launch_lsam/2024-03-14_blenhein_1.launch b/spires_ros/lio_sam/launch_lsam/2024-03-14_blenhein_1.launch index f870b5e..f9108e1 100644 --- a/spires_ros/lio_sam/launch_lsam/2024-03-14_blenhein_1.launch +++ b/spires_ros/lio_sam/launch_lsam/2024-03-14_blenhein_1.launch @@ -12,6 +12,6 @@ - + From 2842ff4e0557c1ae40602f8e59d1e63c8bcb09e1 Mon Sep 17 00:00:00 2001 From: mice85 Date: Wed, 25 Sep 2024 17:07:48 +0100 Subject: [PATCH 15/17] ruff error fixed --- scripts/localisation_benchmark/fast_lio2.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/scripts/localisation_benchmark/fast_lio2.py b/scripts/localisation_benchmark/fast_lio2.py index e574c6d..cf05abc 100644 --- a/scripts/localisation_benchmark/fast_lio2.py +++ b/scripts/localisation_benchmark/fast_lio2.py @@ -1,6 +1,7 @@ -import libtmux -import time import sys +import time + +import libtmux if __name__ == "__main__": From 6501e12361207ae1968587722c044f536a487edd Mon Sep 17 00:00:00 2001 From: mice85 Date: Wed, 25 Sep 2024 18:48:31 +0100 Subject: [PATCH 16/17] lio_sam installation include in the docker --- .docker_loc/Dockerfile | 35 ++++++++++++++++++++++++++++++++++- 1 file changed, 34 insertions(+), 1 deletion(-) diff --git a/.docker_loc/Dockerfile b/.docker_loc/Dockerfile index 7ef1d6f..6520446 100644 --- a/.docker_loc/Dockerfile +++ b/.docker_loc/Dockerfile @@ -46,14 +46,46 @@ RUN git clone https://github.com/ori-drs/livox_ros_driver2.git ${CATKIN_WS_DIR}/ RUN git clone https://github.com/Livox-SDK/Livox-SDK2.git ${CATKIN_WS_DIR}/src/Livox-SDK2 RUN cd ${CATKIN_WS_DIR}/src/Livox-SDK2/ && mkdir build && cd build && cmake .. && make -j && make install +# Install lio_sam dependences +RUN apt-get update +RUN apt-get install libboost-all-dev +RUN apt-get install software-properties-common -y +RUN add-apt-repository ppa:borglab/gtsam-release-4.0 +RUN apt-get update +RUN apt-get install libgtsam-dev libgtsam-unstable-dev -y + # Install ros packages RUN git clone https://github.com/catkin/catkin_simple.git ${CATKIN_WS_DIR}/src/catkin_simple RUN git clone https://github.com/ori-drs/FAST_LIO.git -b ros1 ${CATKIN_WS_DIR}/src/FAST_LIO RUN cd ${CATKIN_WS_DIR}/src/FAST_LIO && git submodule update --init +RUN git clone https://github.com/ori-drs/LIO_SAM_6AXIS.git -b dev/frontier_hesai_ho ${CATKIN_WS_DIR}/src/LIO_SAM_6AXIS + +# Install C++ Dependencies +RUN apt-get update && apt-get install --no-install-recommends -y \ + libblosc-dev \ + libboost-iostreams-dev \ + libboost-numpy-dev \ + libboost-python-dev \ + libboost-system-dev \ + libeigen3-dev \ + libtbb-dev \ + libgflags-dev \ + libgl1-mesa-glx \ + libgoogle-glog-dev \ + protobuf-compiler \ + python3-catkin-tools \ + && rm -rf /var/lib/apt/lists/* RUN cd ${CATKIN_WS_DIR} && catkin build livox_ros_driver2 RUN cd ${CATKIN_WS_DIR} && catkin build fast_lio +RUN apt-get update +RUN apt-get install wget +RUN cd ${CATKIN_WS_DIR}/src && wget https://sourceforge.net/projects/geographiclib/files/distrib-C++/GeographicLib-2.4.tar.gz && tar xfpz GeographicLib-2.4.tar.gz +RUN cd ${CATKIN_WS_DIR}/src/GeographicLib-2.4 && mkdir BUILD && cd BUILD && cmake .. && make && make install + +RUN cd ${CATKIN_WS_DIR} && catkin build lio_sam_6axis + # Trajectory evaluation dependences RUN apt-get update RUN apt-get install tmux -y @@ -74,11 +106,12 @@ COPY ./oxford_spires_utils/ ${SPIRES_DIR}/oxford_spires_utils/ COPY ./spires_ros/fast_lio2/launch_osd ${CATKIN_WS_DIR}/src/FAST_LIO/launch RUN cd ${CATKIN_WS_DIR}/src/FAST_LIO && mkdir config COPY ./spires_ros/fast_lio2/hesai.yaml ${CATKIN_WS_DIR}/src/FAST_LIO/config +COPY ./spires_ros/lio_sam/launch_lsam ${CATKIN_WS_DIR}/src/LIO_SAM_6AXIS/LIO-SAM-6AXIS/launch +COPY ./spires_ros/lio_sam/config_lsam ${CATKIN_WS_DIR}/src/LIO_SAM_6AXIS/LIO-SAM-6AXIS/config COPY ./pyproject.toml ${SPIRES_DIR}/pyproject.toml # RUN pip install -e . # Make the outputs of the container match the host - RUN chown -R ${UID}:${GID} ${SPIRES_DIR} USER ${UNAME} RUN echo "PS1='${debian_chroot:+($debian_chroot)}\[\033[01;33m\]\u@docker-\h\[\033[00m\]:\[\033[01;34m\]\w\[\033[00m\]\$ '" >> ~/.bashrc From 7a7c45fd3b01450e8682e80275b036226899d7b8 Mon Sep 17 00:00:00 2001 From: mice85 Date: Wed, 25 Sep 2024 19:40:43 +0100 Subject: [PATCH 17/17] lio_sam script included --- scripts/localisation_benchmark/lio_sam.py | 58 +++++++++++++++++++++++ 1 file changed, 58 insertions(+) create mode 100644 scripts/localisation_benchmark/lio_sam.py diff --git a/scripts/localisation_benchmark/lio_sam.py b/scripts/localisation_benchmark/lio_sam.py new file mode 100644 index 0000000..7ceac16 --- /dev/null +++ b/scripts/localisation_benchmark/lio_sam.py @@ -0,0 +1,58 @@ +import sys +import time + +import libtmux + +if __name__ == "__main__": + + # TODO: put it in def an get path from arg. + + path = '/home/oxford_spires_dataset/data/' + sequence = '2024-03-14_blenhein_1' + seq_path = path + sequence + + server = libtmux.Server() + server.cmd('new-session', '-d', '-P', '-F#{session_id}') + session = server.sessions[0] + window_1 = session.new_window(attach=False, window_name="launch lio_sam") + window_2 = session.new_window(attach=False, window_name="rosbag") + window_3 = session.new_window(attach=False, window_name="rosservice") + pane_1 = window_1.panes.get() + pane_2 = window_2.panes.get() + pane_3 = window_3.panes.get() + + pane_1.send_keys('cd /home/catkin_ws/') + pane_1.send_keys('source devel/setup.bash') + pane_1.send_keys('roslaunch lio_sam_6axis {}.launch'.format(sequence)) + + pane_2.send_keys('cd {}/rosbag/'.format(seq_path)) + pane_2.send_keys('rosbag play *.bag --clock') + + # Check if the rosbag is "Done." + print("LIO-SAM launched!! - {}.launch".format(sequence)) + print("Starting rosbag files in {}/rosbag/ ...".format(seq_path)) + while (1): + pane_2.clear() + time.sleep(1) + cap_curr = pane_2.capture_pane() + for line in cap_curr: + if "[RUNNING]" in line: + print(line) + sys.stdout.write("\033[F") + if 'Done.' in cap_curr: + sys.stdout.write("\033[K") + print('Done.') + break + print("Saving output ...") + pane_3.send_keys('rosservice call /lio_sam_6axis/save_map') + time.sleep(10) + + print("PROCESS FINISHED!!") + print("Output in: {}/output_loc/map/optimized_odom_tum.txt".format(sequence)) + + # TODO: Copy to output folder + + # TODO: Transform to TUM format + + + server.kill() \ No newline at end of file