lidar_SLAM package

.gitignore

@@ -18,3 +18,7 @@ complex_yolov4_mse_loss.pth
 trace*
 
 rosbag2*
+
+closuresBag*
+
+src/mapping/offline_lidar_slam/save

Dockerfile

@@ -127,6 +127,8 @@ RUN apt-get update && apt update && apt upgrade -y && \
     ros-humble-vision-opencv \
     #
     software-properties-common \
+    #
+    libopen3d-dev\
     # cleanup to make image smaller
     && apt clean && rm -rf /var/lib/apt/lists/*
 
@@ -155,7 +157,7 @@ RUN pip3 install --ignore-installed \
     # Scientific Computing - used widely
     numpy==1.26.4 \
     #
-    open3d==0.18.0 \
+    open3d==0.19.0 \
     #
     opencv-python==4.10.0.84 \
     #
@@ -217,7 +219,6 @@ RUN pip3 install --ignore-installed \
     # six>=1.5 \
     # mmcls \
     easydict==1.13 \
-    kiss-icp==1.0.0 \
     g2o-python==0.0.12 \
     rosbags==0.10.4 \
     ultralytics
@@ -236,7 +237,6 @@ RUN mim install 'mmdet>=3.0.0'
 RUN mim install "mmdet3d>=1.1.0"
 RUN mim install "mmpose>=1.1.0"
 
-
 # install loop closure package "MapClosures"
 # this issue was addressed here: https://github.com/abetlen/llama-cpp-python/issues/707
 # RUN python3 -m pip uninstall -y exceptiongroup
@@ -262,14 +262,29 @@ RUN git clone https://github.com/The-OpenROAD-Project/lemon-graph.git && cd lemo
     && git checkout 62ac753 \
     && mkdir build && cd build && cmake .. && make && make install
 
+RUN pip3 install \
+    readchar==4.2.1 \
+    scikit-build-core==0.11.5 \
+    pyproject-metadata==0.9.1 \
+    pybind11==3.0.0 \
+    ninja==1.13.0 \
+    # this version of cmake is being used to build custom kiss-icp, map-closures, kiss-slam libraries.
+    cmake==4.1.0 
+
 # RUN useradd -ms /bin/bash docker
 RUN usermod -a -G dialout root
 RUN usermod -a -G tty root
 # USER docker
 
 ENV ROS_VERSION 2
+WORKDIR /
+RUN git clone https://github.com/Nova-UTD/kiss-icp.git && cd kiss-icp && make custom
+RUN git clone https://github.com/RainerKuemmerle/g2o.git && cd g2o && mkdir build && cd build && cmake .. && make -j && make install
+RUN git clone https://github.com/Nova-UTD/MapClosures.git && cd MapClosures && make custom
+RUN git clone https://github.com/Nova-UTD/kiss-slam.git && cd kiss-slam && make custom
 
 WORKDIR /navigator
+
 COPY ./docker/entrypoint.sh /opt/entrypoint.sh
 
 ENTRYPOINT [ "/opt/entrypoint.sh" ]

README.md

@@ -1,7 +1,28 @@
-[<img src="docs/assets/res/logo-alt.gif" width="200" />](https://nova-utd.github.io/navigator)
+# This branch aims to solve SLAM and map-based-localization.
 
-Navigator is a simple, modular, open-source autonomous driving stack. It's developed by Nova, an undergraduate research team at UT Dallas.
+## Intended use:
+I recommend using the current setup for localization, with the ```localization_gpsguess``` node, and using ```mapping_node``` and ```map_addition_gpsguess``` for mapping.
 
-For docs, visit [our documentation site](https://nova-utd.github.io/navigator).
+mapping_node relies on ```/gnss_gt/odometry``` (highly accurate GPS) to properly place the map in the world coordinate frame. The other two nodes can make do with very inaccurate GPS readings (+-10 meters).
 
-For info about our group, Nova, visit our [group site](https://nova-utd.github.io).
+localization and mapping nodes may take a little bit of driving to fully register your current location. Continue driving slowly until the pose is determined. If it takes longer than about 10 seconds with gps guess, restart the nodes. It's not possible to estimate how long it might take with global registration considering the infinite size possibilities.
+
+## Alternate configurations:
+```localization_node``` and ```map_addition_node``` perform the same function as ```localization_gpsguess``` and ```map_addition_gpsguess```, respectively, but do not rely on any GPS signal and use only global registration, which can be very time-intensive and possible inaccurate on larger maps. However, on the simulator map, both of these algorithms work relatively well.
+
+In addition, ```map_addition_gpsinit``` uses the same initial pose gathering system as mapping_node, relying on highly accurate GPS readings.
+
+## Setup
+- The dockerfile in this branch should build everything properly. Run ```docker compose build navigator_carla``` Email adipu@utexas.edu if you have issues.
+- This pipeline has been tested with vehicle spawner disabled in carla, and using carla_manual_control. Set these in carla launchfile
+
+## Usage
+- The lidar_SLAM package uses ```combined_map.pcd``` located in ```resource/``` for localization nodes, and writes to the current working directory as well as the build resource directory when mapping nodes are run and saved. To update the localization map, ```combined_map.pcd``` must be replaced with another pointcloud file of the same name and extension. Since the file in the build directory is referenced, you must ```colcon build``` when replacing the ```combined_map.pcd``` file.
+  - Essentially, after running a mapping node, copy the pointcloud file from the directory where the node was run to the ```src/lidar_SLAM/resource``` directory
+
+- The localization node is automatically run with navigator when ```launch launches/launch.carla.py``` is executed, but it can also be run by executing ```run lidar_SLAM localization_gpsguess``` (or ```localization_node``` for global registration initial estimate)
+  - Rviz should be set up on this branch to show the gnss_gt vector alongside the vector returned by the localization node, but if not:
+    - In the left toolbar, click add, then under topics add the odometry topics under ```/gnss_gt/odometry``` and ```/localized_pose```.
+
+## src/mapping/lidar_slam_testing
+- This folder contains offline python scripts that allow for the testing and viewing of trajectories generated by kiss-slam, as well as by the same localization pipeline that runs in the localization_node