diff --git a/.gitmodules b/.gitmodules deleted file mode 100644 index 07acdcd..0000000 --- a/.gitmodules +++ /dev/null @@ -1,3 +0,0 @@ -[submodule "dependencies/OpenPlaceRecognition"] - path = dependencies/OpenPlaceRecognition - url = https://github.com/OPR-Project/OpenPlaceRecognition.git diff --git a/ERRORS.md b/ERRORS.md new file mode 100644 index 0000000..f18f303 --- /dev/null +++ b/ERRORS.md @@ -0,0 +1,38 @@ +[place_recognition_node.py-1] 2025-03-31 14:33:42.986 | WARNING | opr.models.place_recognition.pointmamba::16 - The 'pointmamba' package is not installed. Please install it manually if neccessary. +[place_recognition_node.py-1] [INFO] [1743431623.286569389] [multimodal_multicamera_lidar_place_recognition]: Initialized PlaceRecognitionNode node. +[place_recognition_node.py-1] [INFO] [1743431696.078611049] [multimodal_multicamera_lidar_place_recognition]: Received synchronized messages. +[place_recognition_node.py-1] Traceback (most recent call last): +[place_recognition_node.py-1] File "/home/docker_opr_ros2/ros2_ws/install/open_place_recognition/lib/open_place_recognition/place_recognition_node.py", line 377, in +[place_recognition_node.py-1] main() +[place_recognition_node.py-1] File "/home/docker_opr_ros2/ros2_ws/install/open_place_recognition/lib/open_place_recognition/place_recognition_node.py", line 371, in main +[place_recognition_node.py-1] rclpy.spin(pr_node) +[place_recognition_node.py-1] File "/opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/__init__.py", line 226, in spin +[place_recognition_node.py-1] executor.spin_once() +[place_recognition_node.py-1] File "/opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/executors.py", line 739, in spin_once +[place_recognition_node.py-1] self._spin_once_impl(timeout_sec) +[place_recognition_node.py-1] File "/opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/executors.py", line 736, in _spin_once_impl +[place_recognition_node.py-1] raise handler.exception() +[place_recognition_node.py-1] File "/opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/task.py", line 239, in __call__ +[place_recognition_node.py-1] self._handler.send(None) +[place_recognition_node.py-1] File "/opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/executors.py", line 437, in handler +[place_recognition_node.py-1] await call_coroutine(entity, arg) +[place_recognition_node.py-1] File "/opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/executors.py", line 362, in _execute_subscription +[place_recognition_node.py-1] await await_or_execute(sub.callback, msg) +[place_recognition_node.py-1] File "/opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/executors.py", line 107, in await_or_execute +[place_recognition_node.py-1] return callback(*args) +[place_recognition_node.py-1] File "/opt/ros/humble/local/lib/python3.10/dist-packages/message_filters/__init__.py", line 83, in callback +[place_recognition_node.py-1] self.signalMessage(msg) +[place_recognition_node.py-1] File "/opt/ros/humble/local/lib/python3.10/dist-packages/message_filters/__init__.py", line 64, in signalMessage +[place_recognition_node.py-1] cb(*(msg + args)) +[place_recognition_node.py-1] File "/opt/ros/humble/local/lib/python3.10/dist-packages/message_filters/__init__.py", line 313, in add +[place_recognition_node.py-1] self.signalMessage(*msgs) +[place_recognition_node.py-1] File "/opt/ros/humble/local/lib/python3.10/dist-packages/message_filters/__init__.py", line 64, in signalMessage +[place_recognition_node.py-1] cb(*(msg + args)) +[place_recognition_node.py-1] File "/home/docker_opr_ros2/ros2_ws/install/open_place_recognition/lib/open_place_recognition/place_recognition_node.py", line 354, in listener_callback +[place_recognition_node.py-1] output = self.pipeline.infer(input_data) +[place_recognition_node.py-1] File "/home/docker_opr_ros2/OpenPlaceRecognition/src/opr/pipelines/place_recognition/base.py", line 124, in infer +[place_recognition_node.py-1] _, pred_i = self.database_index.search(descriptor, 1) +[place_recognition_node.py-1] File "/usr/local/lib/python3.10/dist-packages/faiss-1.7.4-py3.10.egg/faiss/class_wrappers.py", line 329, in replacement_search +[place_recognition_node.py-1] assert d == self.d +[place_recognition_node.py-1] AssertionError +[ERROR] [place_recognition_node.py-1]: process has died [pid 2597, exit code 1, cmd '/home/docker_opr_ros2/ros2_ws/install/open_place_recognition/lib/open_place_recognition/place_recognition_node.py --ros-args -r __node:=multimodal_multicamera_lidar_place_recognition --params-file /tmp/launch_params_wdk5sjdl']. \ No newline at end of file diff --git a/README.md b/README.md index 9171edc..ccabbfc 100644 --- a/README.md +++ b/README.md @@ -1,19 +1,12 @@ # OpenPlaceRecognition-ROS2 -The ROS2 wrapper is intended for: +The OpenPlaceRecognition ROS2 wrapper is intended for: - 🔌 **Easy integration** of multimodal localization into robots and autonomous systems based on ROS2. - 🚘 **Application in existing robotic platforms and autopilots** running ROS2. - ⚙️ **Rapid deployment** of neural network-based global localization methods into real-world ROS2 projects. -> Check out out [OpenPlaceRecognition library](https://github.com/OPR-Project/OpenPlaceRecognition). -> It is a library for place recognition and localization, which includes a collection of datasets, including ITLP-Campus. -> The library provides a unified API for loading datasets, training and evaluating models, and performing place recognition tasks. -> The library is designed to be easy to use and extensible, allowing researchers and developers to quickly experiment with different models and datasets. - -## Installation - -### Requirements +# Requirements #### Hardware @@ -34,75 +27,127 @@ The ROS2 wrapper is intended for: - CUDA Toolkit >= 11.1 - cuDNN >= 7.5 - [OpenPlaceRecognition](https://github.com/OPR-Project/OpenPlaceRecognition) - - ROS2 Humble + - [ROS2 Humble](https://docs.ros.org/en/humble/Installation.html) + - Docker + +# Installation + +Firstly retreave all dependencies for this project + +```bash +# Step 1: Clone the main repository with submodules +cd ~ +git clone --recursive https://github.com/OPR-Project/OpenPlaceRecognition -If you want to install all dependencies manually, please refer to the -[Advanced Installation section of the OpenPlaceRecognition documentation](https://openplacerecognition.readthedocs.io/en/latest/#installation) -for detailed instructions. +# Step 2: Clone the ITLP-Campus-Outdoor dataset +git clone https://huggingface.co/datasets/OPR-Project/ITLP-Campus-Outdoor ~/Datasets/ITLP-Campus-Outdoor -The only additional dependency is ROS2. Please refer to the -[ROS2 Humble installation guide](https://docs.ros.org/en/humble/Installation.html) -for instructions on how to install it. +# Step 3: Create required directories for pretrained weights +mkdir -p ~/OpenPlaceRecognition/weights/place_recognition +mkdir -p ~/OpenPlaceRecognition/weights/registration -### Quick start +# Step 4: Download pretrained weights +wget https://huggingface.co/OPR-Project/PlaceRecognition-NCLT/resolve/main/minkloc3d_nclt.pth \ + -O ~/OpenPlaceRecognition/weights/place_recognition/minkloc3d_nclt.pth -It is highly recommended to use the provided Dockerfile to build the environment. -The scripts to build, run and enter the container are provided in the [docker/](./docker) directory. -You can use the following commands: +wget https://huggingface.co/OPR-Project/Registration-KITTI/resolve/main/geotransformer_kitti.pth \ + -O ~/OpenPlaceRecognition/weights/registration/geotransformer_kitti.pth + +# Step 5: Export environment variables +export OPR_PATH=$HOME/OpenPlaceRecognition +export DATASETS_DIR=$HOME/Datasets +export DISPLAY=:0 +``` + +### Quick Start with Docker + +It is highly recommended to use the provided Dockerfile to build the complete environment. The Docker scripts are located in the `docker/` directory. For now, we only support x86_64 architecture. The aarch64 version will be released soon. ```bash -# 0. clone the repository and init submodules +# 0. Clone the repository git clone https://github.com/OPR-Project/OpenPlaceRecognition-ROS2.git cd OpenPlaceRecognition-ROS2 -git submodule update --init --recursive -# 1. build the image -bash docker/build.sh +# 1. Build the Docker image +bash docker/build_x86_64.sh -# 2. start the container and mount the data directory -bash docker/start.sh [DATA_DIR] +# 2. Start the container with the data directory mounted +bash docker/start_x86_64.sh -# 3. enter the container +# 3. Enter the container bash docker/into.sh ``` -To use [OpenPlaceRecognition](https://github.com/OPR-Project/OpenPlaceRecognition) library in the container, you need to install it first. -Run the following command inside the container: +Inside the container, install the additional Python dependencies: ```bash -pip install -e ~/ros2_ws/dependencies/OpenPlaceRecognition +pip install -e ~/OpenPlaceRecognition +pip install -e ~/OpenPlaceRecognition/third_party/GeoTransformer +pip install -e ~/OpenPlaceRecognition/third_party/HRegNet +pip install -e ~/OpenPlaceRecognition/third_party/PointMamba ``` -## Running nodes - -### Build the workspace - -Inside `ros2_ws/` directory, run the following command: +### Build the ROS2 Workspace ```bash -colcon build --packages-select open_place_recognition opr_interfaces +cd ~/ros2_ws/ +colcon build --symlink-install +source ~/ros2_ws/install/setup.bash ``` -### Run the nodes -Open the new terminal and run: +## Launch Files and Usage -```bash -source ros2_ws/install/setup.bash -``` +The project provides a variety of launch files to support testing, visualization, and deployment. Each launch file is configurable via command-line arguments to adapt to different sensor setups and dataset paths. -Run the place recognition node using launch file: +- **Dataset Conversion from Bag Files:** + Convert ROS2 bag files into a standardized OPR dataset format for further processing. + ```bash + ros2 launch open_place_recognition dataset_from_bag.launch.py + ``` -```bash -ros2 launch open_place_recognition place_recognition_launch.py -``` +- **Dataset Conversion from RTAB-Map:** + Convert RTAB-Map datasets to the compatible format required by the OPR pipeline. + ```bash + ros2 launch open_place_recognition dataset_from_rtabmap.launch.py + ``` + +- **Dataset Publisher:** + Publish sensor streams (cameras, LiDAR, etc.) for dataset creation or real-time monitoring. + ```bash + ros2 launch open_place_recognition dataset_publisher.launch.py + ``` + +- **Dataset Indexing:** + Index your dataset features to prepare for efficient retrieval and subsequent training. + ```bash + ros2 launch open_place_recognition dataset_indexing.launch.py + ``` -Run the visualizer node using launch file: +- **Place Recognition:** + This node performs a simple database search using the current sensor inputs (e.g. images, LiDAR) to identify the closest matching location in the pre-built database. + ```bash + ros2 launch open_place_recognition place_recognition.launch.py + ``` + +- **Localization:** + This pipeline extends place recognition by adding position matching algorithms, improving accuracy and robustness for real-world deployment. + ```bash + ros2 launch open_place_recognition localization.launch.py + ``` + + +## Additional Tools + +For further analysis and visualization, you can run Jupyter Lab from within the container: ```bash -ros2 launch open_place_recognition visualizer_launch.py +export PATH=$PATH:$HOME/.local/bin +cd ~/OpenPlaceRecognition/notebooks +jupyter lab --ip=0.0.0.0 --port=8888 --allow-root ``` + ## License -[Apache 2.0 license](./LICENSE) +This project is licensed under the [Apache 2.0 License](./LICENSE). \ No newline at end of file diff --git a/TODO.md b/TODO.md new file mode 100644 index 0000000..def4ec0 --- /dev/null +++ b/TODO.md @@ -0,0 +1,5 @@ +- Fix place_recognition node +- Fix localization node +- Add gps/barometer for better global localization +- Test rtabmap and rosbag convertions +- Add docker supports for aarch64(Jetson based PC) \ No newline at end of file diff --git a/dependencies/OpenPlaceRecognition b/dependencies/OpenPlaceRecognition deleted file mode 160000 index 68eb76f..0000000 --- a/dependencies/OpenPlaceRecognition +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 68eb76f8697940a2928189405e0326e83a3c459e diff --git a/docker/Dockerfile.jetson b/docker/Dockerfile.jetson new file mode 100644 index 0000000..420c73c --- /dev/null +++ b/docker/Dockerfile.jetson @@ -0,0 +1,111 @@ +FROM open-place-recognition-jetson:r35.4.1-cu114-cp310 +ARG MAX_JOBS=4 + +ENV DEBIAN_FRONTEND noninteractive + +RUN add-apt-repository -y ppa:ubuntu-toolchain-r/test && \ + apt-get update && apt-get upgrade -y && apt-get install -y \ + openssh-server \ + g++-11 \ + unzip \ + zip \ + default-jdk \ + && rm -rf /var/lib/apt/lists/* + +RUN ln -s /usr/local/bin/python3 /usr/local/bin/python + +# upgrade pip +ARG PIP_VERSION=23.3.2 +ARG SETUPTOOLS_VERSION=69.0.3 +RUN pip install pip==${PIP_VERSION} setuptools==${SETUPTOOLS_VERSION} + +# install tensorrt bindings for python3.10 +# COPY docker/TensorRT /TensorRT +RUN git clone --branch=release/8.5 https://github.com/NVIDIA/TensorRT.git && \ + git submodule update --init --recursive +ENV TRT_OSSPATH=/TensorRT +ENV EXT_PATH=/external +RUN mkdir -p ${EXT_PATH} && cd ${EXT_PATH} && \ + git clone https://github.com/pybind/pybind11.git +RUN wget https://www.python.org/ftp/python/3.10.14/Python-3.10.14.tar.xz && \ + tar -xf Python-3.10.14.tar.xz && \ + mkdir ${EXT_PATH}/python3.10 && \ + cp -r Python-3.10.14/Include ${EXT_PATH}/python3.10/include && \ + cp /usr/include/aarch64-linux-gnu/python3.10/pyconfig.h ${EXT_PATH}/python3.10/include/. && \ + rm -rf Python-3.10.14.tar.xz Python-3.10.14 +RUN cd ${TRT_OSSPATH}/python && \ + PYTHON_MAJOR_VERSION=3 PYTHON_MINOR_VERSION=10 TARGET_ARCHITECTURE=aarch64 bash build.sh +RUN cd ${TRT_OSSPATH}/python/build/dist && \ + pip install tensorrt-*.whl + + +### install MinkowskiEngine +ARG NINJA_VERSION=1.11.1.1 +RUN pip install ninja==${NINJA_VERSION} +RUN git clone --recursive "https://github.com/alexmelekhin/MinkowskiEngine.git" && \ + cd MinkowskiEngine && \ + git checkout 4b628a7 && \ + python3 setup.py install --force_cuda --blas=openblas && \ + cd .. && \ + rm -rf MinkowskiEngine + +RUN pip install \ + numpy'<2.0.0' \ + opencv-python==4.10.0.84 + +### Install Torch-TensorRT. See: https://pytorch.org/TensorRT/getting_started/installation.html#compiling-from-source. + +WORKDIR / +RUN rm -rf /TensorRT +RUN git clone -b v2.2.0 https://github.com/pytorch/TensorRT.git + +# Install Bazel. +WORKDIR /TensorRT +RUN export BAZEL_VERSION=$(cat .bazelversion); \ + mkdir bazel; \ + cd bazel; \ + curl -fSsL -O https://github.com/bazelbuild/bazel/releases/download/${BAZEL_VERSION}/bazel-${BAZEL_VERSION}-dist.zip; \ + unzip bazel-${BAZEL_VERSION}-dist.zip +WORKDIR /TensorRT/bazel +RUN bash ./compile.sh +RUN cp output/bazel /usr/local/bin/ + +# Install the torch_tensorrt package. +RUN pip install pyyaml +WORKDIR /TensorRT +# See: https://github.com/pytorch/TensorRT/issues/2623. +COPY docker/torch_tensorrt_workspace/WORKSPACE.jp51 /TensorRT/WORKSPACE +RUN python3 setup.py install --use-cxx11-abi + +# install onnx-runtime +RUN wget https://nvidia.box.com/shared/static/ndh4omnbyx9icnbwsizmivngnjo5kp1b.whl -O onnxruntime_gpu-1.18.0-cp310-cp310-linux_aarch64.whl && \ + pip install onnxruntime_gpu-1.18.0-cp310-cp310-linux_aarch64.whl && \ + rm onnxruntime_gpu-1.18.0-cp310-cp310-linux_aarch64.whl + +# COPY docker/Open3D /Open3D +RUN git clone https://github.com/isl-org/Open3D.git && cd Open3D && git checkout c8856fc +WORKDIR /Open3D +RUN bash util/install_deps_ubuntu.sh assume-yes && \ + rm -rf /var/lib/apt/lists/* +RUN mkdir build && cd build && \ + cmake \ + -DCMAKE_BUILD_TYPE=Release \ + -DBUILD_SHARED_LIBS=ON \ + -DGLIBCXX_USE_CXX11_ABI=ON \ + -DBUILD_CUDA_MODULE=ON \ + -DBUILD_PYTORCH_OPS=ON \ + -DBUILD_TENSORFLOW_OPS=OFF \ + -DPYTHON_EXECUTABLE=$(which python) \ + -DCMAKE_INSTALL_PREFIX=/usr/local \ + .. && \ + make -j1 && \ + make install -j1 +RUN pip install yapf==0.43.0 +ENV LD_PRELOAD=${LD_PRELOAD}:/usr/local/lib/libOpen3D.so +RUN cd build && make install-pip-package -j1 + +# install polygraphy +RUN pip install colored polygraphy --extra-index-url https://pypi.ngc.nvidia.com + +# install pytorch3d +RUN FORCE_CUDA=1 pip install "git+https://github.com/facebookresearch/pytorch3d.git@stable" diff --git a/docker/Dockerfile.devel b/docker/Dockerfile.x86_64 similarity index 54% rename from docker/Dockerfile.devel rename to docker/Dockerfile.x86_64 index 44c3f74..e7a16ef 100644 --- a/docker/Dockerfile.devel +++ b/docker/Dockerfile.x86_64 @@ -1,9 +1,15 @@ FROM alexmelekhin/open-place-recognition:base -# to install "dvc[gdrive]" we need to install "distro" package first +# Install distro package as before. ARG DISTRO_VERSION=1.9.0 RUN pip install distro==${DISTRO_VERSION} +# OPR_PATH is provided as a relative path within the build context. +ARG OPR_PATH= + +# Copy the external OpenPlaceRecognition folder into /OpenPlaceRecognition inside the container. +COPY ${OPR_PATH} /OpenPlaceRecognition + # Set the locale RUN apt-get update && apt-get install -y locales && \ locale-gen en_US en_US.UTF-8 && \ @@ -15,40 +21,53 @@ RUN apt-get install -y software-properties-common && add-apt-repository universe curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg && \ echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" | tee /etc/apt/sources.list.d/ros2.list > /dev/null -# Install ROS2 +# Install ROS2 and common tools RUN apt-get update && apt-get upgrade -y && apt-get install -y \ + sudo \ + wget \ + git \ + nano \ + curl \ ros-humble-desktop \ ros-dev-tools \ ros-humble-image-transport-plugins && \ rosdep init && rosdep update -COPY dependencies/OpenPlaceRecognition OpenPlaceRecognition +# Copy local ROS2 packages into the workspace source directory. +RUN mkdir -p /ros2_ws/src + +# Run additional setup steps from the external OPR folder. WORKDIR /OpenPlaceRecognition -RUN cd third_party/GeoTransformer && \ - bash setup.sh +RUN cd third_party/GeoTransformer && bash setup.sh WORKDIR / -RUN rm -rf OpenPlaceRecognition +RUN rm -rf /OpenPlaceRecognition -# add user and his password +# Add user and set password. ENV USER=docker_opr_ros2 ARG UID=1000 ARG GID=1000 -# default password ARG PW=user +RUN useradd -m ${USER} --uid=${UID} && echo "${USER}:${PW}" | chpasswd && \ + usermod -aG sudo ${USER} && echo "${USER} ALL=(ALL) NOPASSWD:ALL" >> /etc/sudoers -RUN useradd -m ${USER} --uid=${UID} && echo "${USER}:${PW}" | chpasswd && adduser ${USER} sudo WORKDIR /home/${USER} -# create some directories for mounting volumes -RUN mkdir ros2_ws && chown -R ${UID}:${GID} /home/${USER} -RUN mkdir Datasets && chown -R ${UID}:${GID} /home/${USER} +# Create directories for mounting volumes. +RUN mkdir -p ros2_ws && chown -R ${UID}:${GID} /home/${USER} +RUN mkdir OpenPlaceRecognition && chown -R ${UID}:${GID} /home/${USER} USER ${UID}:${GID} +# Source ROS2 on startup RUN echo "source /opt/ros/humble/setup.bash" >> /home/${USER}/.bashrc -COPY dependencies/OpenPlaceRecognition/requirements.txt requirements.txt + +# Install pip requirements from the external folder. +COPY ${OPR_PATH}/requirements.txt requirements.txt RUN pip install --user -r requirements.txt && rm requirements.txt -COPY dependencies/OpenPlaceRecognition/requirements-dev.txt requirements-dev.txt +COPY ${OPR_PATH}/requirements-dev.txt requirements-dev.txt RUN pip install --user -r requirements-dev.txt && rm requirements-dev.txt -COPY dependencies/OpenPlaceRecognition/requirements-notebook.txt requirements-notebook.txt +COPY ${OPR_PATH}/requirements-notebook.txt requirements-notebook.txt RUN pip install --user -r requirements-notebook.txt && rm requirements-notebook.txt + +# Install Paddle packages via pip. +RUN pip install --user paddleocr paddlepaddle-gpu diff --git a/docker/build.sh b/docker/build_jetson.sh similarity index 50% rename from docker/build.sh rename to docker/build_jetson.sh index fbc6dd9..8fe7290 100644 --- a/docker/build.sh +++ b/docker/build_jetson.sh @@ -3,29 +3,26 @@ orange=`tput setaf 3` reset_color=`tput sgr0` + +echo "This architecture is still in development" +exit 1 + + ARCH=`uname -m` -if [ $ARCH != "x86_64" ]; then +if [ $ARCH != "aarch64" ]; then echo "${orange}${ARCH}${reset_color} architecture is not supported" exit 1 fi -if command -v nvidia-smi &> /dev/null; then - echo "Detected ${orange}CUDA${reset_color} hardware" - DOCKERFILE=Dockerfile.devel - DEVICE=cuda -else - echo "${orange}CPU-only${reset_color} build is not supported yet" - exit 1 -fi - -echo "Building for ${orange}${ARCH}${reset_color} with ${orange}${DEVICE}${reset_color}" +echo "Building for ${orange}${ARCH}${reset_color}" PROJECT_ROOT_DIR=$(cd ./"`dirname $0`"/.. || exit; pwd) +DOCKERFILE=Dockerfile.jetson docker build $PROJECT_ROOT_DIR \ --build-arg MAX_JOBS=4 \ --build-arg UID=$(id -u) \ --build-arg GID=$(id -g) \ -f $PROJECT_ROOT_DIR/docker/$DOCKERFILE \ - -t open-place-recognition-ros2:devel \ + -t open-place-recognition-jetson:base-r35.4.1-cu114-cp310 \ --network=host diff --git a/docker/build_x86_64.sh b/docker/build_x86_64.sh new file mode 100644 index 0000000..ec2146c --- /dev/null +++ b/docker/build_x86_64.sh @@ -0,0 +1,72 @@ +#!/bin/bash + +orange=$(tput setaf 3) +reset_color=$(tput sgr0) + +ARCH=$(uname -m) +if [ "$ARCH" != "x86_64" ]; then + echo "${orange}${ARCH}${reset_color} architecture is not supported" + exit 1 +fi + +if command -v nvidia-smi &> /dev/null; then + echo "Detected ${orange}CUDA${reset_color} hardware" + SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" + DOCKERFILE_PATH="$SCRIPT_DIR/Dockerfile.x86_64" + DEVICE=cuda +else + echo "${orange}CPU-only${reset_color} build is not supported yet" + exit 1 +fi + +echo "Building for ${orange}${ARCH}${reset_color} with ${orange}${DEVICE}${reset_color}" + +# OPR_PATH must be provided as an absolute path. +if [ -z "$OPR_PATH" ]; then + echo "Error: Please set the OPR_PATH environment variable to your external OpenPlaceRecognition directory." + exit 1 +fi + +# Check if OPR_PATH exists. +if [ ! -d "$OPR_PATH" ]; then + echo "Error: OPR_PATH ($OPR_PATH) does not exist." + exit 1 +fi +echo "Using OpenPlaceRecognition path: ${orange}${OPR_PATH}${reset_color}" + +# Create a temporary build context directory (outside of your repository) +TMP_BUILD_CONTEXT=$(mktemp -d) +echo "Using temporary build context: ${orange}${TMP_BUILD_CONTEXT}${reset_color}" + +# Copy the Dockerfile into the temporary build context. +cp "$DOCKERFILE_PATH" "$TMP_BUILD_CONTEXT/" + +# Copy the external OPR directory into the temporary build context under a dedicated folder. +# This prevents polluting your repository. +mkdir -p "$TMP_BUILD_CONTEXT/OpenPlaceRecognition_external" +cp -r "$OPR_PATH" "$TMP_BUILD_CONTEXT/OpenPlaceRecognition_external" + +# The build argument OPR_PATH will be set to point to the external folder inside the temporary build context. +# For example, if OPR_PATH=/home/rover2/Downloads/OpenPlaceRecognition/OpenPlaceRecognition, +# then $(basename "$OPR_PATH") will be "OpenPlaceRecognition", and the full relative path becomes: +# "OpenPlaceRecognition_external/OpenPlaceRecognition" +BUILD_OPR_PATH="OpenPlaceRecognition_external/$(basename "$OPR_PATH")" + +# Allow external specification of the base image. +if [ -z "$BASE_IMAGE" ]; then + BASE_IMAGE=alexmelekhin/open-place_recognition:base +fi +echo "Using base image: ${orange}${BASE_IMAGE}${reset_color}" + +docker build "$TMP_BUILD_CONTEXT" \ + --build-arg MAX_JOBS=4 \ + --build-arg UID=$(id -u) \ + --build-arg GID=$(id -g) \ + --build-arg OPR_PATH="${BUILD_OPR_PATH}" \ + --build-arg BASE_IMAGE="${BASE_IMAGE}" \ + -f "$TMP_BUILD_CONTEXT/$(basename "$DOCKERFILE_PATH")" \ + -t open-place-recognition-ros2:devel \ + --network=host + +# Clean up the temporary build context. +rm -rf "$TMP_BUILD_CONTEXT" diff --git a/docker/start.sh b/docker/start_jetson.sh similarity index 100% rename from docker/start.sh rename to docker/start_jetson.sh diff --git a/docker/start_x86_64.sh b/docker/start_x86_64.sh new file mode 100644 index 0000000..5974e94 --- /dev/null +++ b/docker/start_x86_64.sh @@ -0,0 +1,64 @@ +#!/bin/bash + +orange=`tput setaf 3` +reset_color=`tput sgr0` + +# get_real_path(){ +# if [ "${1:0:1}" == "/" ]; then +# echo "$1" +# else +# realpath -m "$PWD"/"$1" +# fi +# } + +ARCH=`uname -m` +if [ $ARCH == "x86_64" ]; then + if command -v nvidia-smi &> /dev/null; then + DEVICE=cuda + ARGS="--ipc host --gpus all" + else + echo "${orange}CPU-only${reset_color} build is not supported yet" + exit 1 + fi +else + echo "${orange}${ARCH}${reset_color} architecture is not supported" + exit 1 +fi + + +if [ -z "$DATASETS_DIR" ]; then + echo "Error: DATASETS_DIR environment variable is not set. Please set it to your Datasets directory." + exit 1 +fi + +if [ ! -d $DATASETS_DIR ]; then + echo "Error: DATASETS_DIR=$DATASETS_DIR is not an existing directory." + exit 1 +fi + +if [ -z "$OPR_PATH" ]; then + echo "Error: OPR_PATH environment variable is not set. Please set it to your OpenPlaceRecognition directory." + exit 1 +fi + +PROJECT_ROOT_DIR=$(cd ./"`dirname $0`"/.. || exit; pwd) + +echo "Running on ${orange}${ARCH}${reset_color} with ${orange}${DEVICE}${reset_color}" + +xhost + + docker run -it -d --rm \ + $ARGS \ + --env="DISPLAY=$DISPLAY" \ + --env="QT_X11_NO_MITSHM=1" \ + --privileged \ + --name ${USER}_opr_ros2 \ + --net host \ + -v /tmp/.X11-unix:/tmp/.X11-unix:rw \ + -v $PROJECT_ROOT_DIR:/home/docker_opr_ros2/ros2_ws/src:rw \ + -v $DATASETS_DIR:/home/docker_opr_ros2/Datasets:rw \ + -v $OPR_PATH:/home/docker_opr_ros2/OpenPlaceRecognition:rw \ + open-place-recognition-ros2:devel +xhost - + +docker exec --user root \ + ${USER}_opr_ros2 bash -c "/etc/init.d/ssh start" diff --git a/src/open_place_recognition/open_place_recognition/__init__.py b/docs/SETUP_OPR.md similarity index 100% rename from src/open_place_recognition/open_place_recognition/__init__.py rename to docs/SETUP_OPR.md diff --git a/open_place_recognition/CMakeLists.txt b/open_place_recognition/CMakeLists.txt new file mode 100644 index 0000000..c2d8c8c --- /dev/null +++ b/open_place_recognition/CMakeLists.txt @@ -0,0 +1,34 @@ +cmake_minimum_required(VERSION 3.5) +project(open_place_recognition) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + + +# Find ament and any required dependencies (e.g. rclpy if used) +find_package(ament_cmake REQUIRED) +find_package(rclpy REQUIRED) + +# Install launch files (matches glob pattern in setup.py) +install( + DIRECTORY + configs + launch + rviz + DESTINATION share/${PROJECT_NAME} +) + +install(PROGRAMS + src/dataset_from_rosbag_node.py + src/dataset_from_rtabmap_node.py + src/dataset_publisher_node.py + src/dataset_indexing_node.py + src/localization_node.py + src/place_recognition_node.py +# src/test_depth_estimation_node.py +# src/test_visualizer_node.py + DESTINATION lib/${PROJECT_NAME} +) + +ament_package() diff --git a/src/open_place_recognition/configs/anno/ade20k.yaml b/open_place_recognition/configs/anno/ade20k.yaml similarity index 100% rename from src/open_place_recognition/configs/anno/ade20k.yaml rename to open_place_recognition/configs/anno/ade20k.yaml diff --git a/src/open_place_recognition/configs/anno/oneformer.yaml b/open_place_recognition/configs/anno/oneformer.yaml similarity index 100% rename from src/open_place_recognition/configs/anno/oneformer.yaml rename to open_place_recognition/configs/anno/oneformer.yaml diff --git a/src/open_place_recognition/configs/pipelines/aruco_localization_pipeline.yaml b/open_place_recognition/configs/pipelines/aruco_localization_pipeline.yaml similarity index 100% rename from src/open_place_recognition/configs/pipelines/aruco_localization_pipeline.yaml rename to open_place_recognition/configs/pipelines/aruco_localization_pipeline.yaml diff --git a/src/open_place_recognition/configs/pipelines/localization_pipeline.yaml b/open_place_recognition/configs/pipelines/localization_pipeline.yaml similarity index 100% rename from src/open_place_recognition/configs/pipelines/localization_pipeline.yaml rename to open_place_recognition/configs/pipelines/localization_pipeline.yaml diff --git a/src/open_place_recognition/configs/pipelines/place_recognition/multimodal_pr.yaml b/open_place_recognition/configs/pipelines/place_recognition/multimodal_pr.yaml similarity index 76% rename from src/open_place_recognition/configs/pipelines/place_recognition/multimodal_pr.yaml rename to open_place_recognition/configs/pipelines/place_recognition/multimodal_pr.yaml index 29efcfb..d39ff44 100644 --- a/src/open_place_recognition/configs/pipelines/place_recognition/multimodal_pr.yaml +++ b/open_place_recognition/configs/pipelines/place_recognition/multimodal_pr.yaml @@ -28,7 +28,6 @@ model: fusion_module: _target_: opr.modules.Concat -database_dir: /home/docker_opr_ros2/Datasets/itlpcampus_nature_exps/databases/indoor_floor_5 -model_weights_path: /home/docker_opr_ros2/ros2_ws/src/open_place_recognition/weights/multi-image_lidar_late-fusion_nclt.pth +model_weights_path: weights/place_recognition/multi-image_lidar_late-fusion_nclt.pth device: cuda -pointcloud_quantization_size: 0.5 +pointcloud_quantization_size: 0.5 \ No newline at end of file diff --git a/src/open_place_recognition/configs/pipelines/place_recognition/multimodal_semantic_pr.yaml b/open_place_recognition/configs/pipelines/place_recognition/multimodal_semantic_pr.yaml similarity index 84% rename from src/open_place_recognition/configs/pipelines/place_recognition/multimodal_semantic_pr.yaml rename to open_place_recognition/configs/pipelines/place_recognition/multimodal_semantic_pr.yaml index 2d54d3b..cd848be 100644 --- a/src/open_place_recognition/configs/pipelines/place_recognition/multimodal_semantic_pr.yaml +++ b/open_place_recognition/configs/pipelines/place_recognition/multimodal_semantic_pr.yaml @@ -41,7 +41,7 @@ model: fusion_module: _target_: opr.modules.Concat -database_dir: /home/docker_opr_ros2/Datasets/itlpcampus_nature_exps/databases/indoor_floor_5 -model_weights_path: /home/docker_opr_ros2/ros2_ws/src/open_place_recognition/weights/multi-image_multi-semantic_lidar_late-fusion_nclt.pth +# database_dir: /home/docker_opr_ros2/Datasets/itlpcampus_nature_exps/databases/indoor_floor_5 +model_weights_path: weights/place_recognition/multi-image_multi-semantic_lidar_late-fusion_nclt.pth device: cuda pointcloud_quantization_size: 0.5 diff --git a/src/open_place_recognition/configs/pipelines/place_recognition/multimodal_semantic_with_soc_outdoor_pr.yaml b/open_place_recognition/configs/pipelines/place_recognition/multimodal_semantic_with_soc_outdoor_pr.yaml similarity index 85% rename from src/open_place_recognition/configs/pipelines/place_recognition/multimodal_semantic_with_soc_outdoor_pr.yaml rename to open_place_recognition/configs/pipelines/place_recognition/multimodal_semantic_with_soc_outdoor_pr.yaml index 39f7a10..cf7389f 100644 --- a/src/open_place_recognition/configs/pipelines/place_recognition/multimodal_semantic_with_soc_outdoor_pr.yaml +++ b/open_place_recognition/configs/pipelines/place_recognition/multimodal_semantic_with_soc_outdoor_pr.yaml @@ -49,7 +49,7 @@ model: fusion_module: _target_: opr.modules.Concat -database_dir: /home/docker_opr_ros2/Datasets/itlpcampus_nature_exps/databases/outdoor_2023-04-11-day -model_weights_path: /home/docker_opr_ros2/ros2_ws/src/open_place_recognition/weights/multimodal_semantic_with_soc_outdoor_nclt.pth +# database_dir: /home/docker_opr_ros2/Datasets/itlpcampus_nature_exps/databases/outdoor_2023-04-11-day +model_weights_path: weights/place_recognition/multimodal_semantic_with_soc_outdoor_nclt.pth device: cuda pointcloud_quantization_size: 0.5 diff --git a/src/open_place_recognition/configs/pipelines/place_recognition/multimodal_with_soc_outdoor_pr.yaml b/open_place_recognition/configs/pipelines/place_recognition/multimodal_with_soc_outdoor_pr.yaml similarity index 82% rename from src/open_place_recognition/configs/pipelines/place_recognition/multimodal_with_soc_outdoor_pr.yaml rename to open_place_recognition/configs/pipelines/place_recognition/multimodal_with_soc_outdoor_pr.yaml index 43a6335..1818053 100644 --- a/src/open_place_recognition/configs/pipelines/place_recognition/multimodal_with_soc_outdoor_pr.yaml +++ b/open_place_recognition/configs/pipelines/place_recognition/multimodal_with_soc_outdoor_pr.yaml @@ -36,7 +36,7 @@ model: fusion_module: _target_: opr.modules.Concat -database_dir: /home/docker_opr_ros2/Datasets/itlpcampus_nature_exps/databases/outdoor_2023-04-11-day -model_weights_path: /home/docker_opr_ros2/ros2_ws/src/open_place_recognition/weights/multimodal_with_soc_outdoor_nclt.pth +# database_dir: /home/docker_opr_ros2/Datasets/itlpcampus_nature_exps/databases/outdoor_2023-04-11-day +model_weights_path: weights/place_recognition/multimodal_with_soc_outdoor_nclt.pth device: cuda pointcloud_quantization_size: 0.5 diff --git a/src/open_place_recognition/configs/pipelines/place_recognition/text_labels_pr.yaml b/open_place_recognition/configs/pipelines/place_recognition/text_labels_pr.yaml similarity index 80% rename from src/open_place_recognition/configs/pipelines/place_recognition/text_labels_pr.yaml rename to open_place_recognition/configs/pipelines/place_recognition/text_labels_pr.yaml index 82047c0..bcfd564 100644 --- a/src/open_place_recognition/configs/pipelines/place_recognition/text_labels_pr.yaml +++ b/open_place_recognition/configs/pipelines/place_recognition/text_labels_pr.yaml @@ -30,7 +30,7 @@ model: fusion_module: _target_: opr.modules.Concat -database_dir: /home/docker_opr_ros2/Datasets/itlpcampus_nature_exps/databases/indoor_floor_5 -model_weights_path: /home/docker_opr_ros2/ros2_ws/src/open_place_recognition/weights/multi-image_lidar_late-fusion_nclt.pth +# database_dir: /home/docker_opr_ros2/Datasets/itlpcampus_nature_exps/databases/indoor_floor_5 +model_weights_path: weights/place_recognition/multi-image_lidar_late-fusion_nclt.pth device: cuda pointcloud_quantization_size: 0.5 diff --git a/src/open_place_recognition/configs/pipelines/topological_localization_pipeline.yaml b/open_place_recognition/configs/pipelines/topological_localization_pipeline.yaml similarity index 100% rename from src/open_place_recognition/configs/pipelines/topological_localization_pipeline.yaml rename to open_place_recognition/configs/pipelines/topological_localization_pipeline.yaml diff --git a/src/open_place_recognition/configs/pipelines/updated_pipelines/aruco_localization_pipeline.yaml b/open_place_recognition/configs/pipelines/updated_pipelines/aruco_localization_pipeline.yaml similarity index 100% rename from src/open_place_recognition/configs/pipelines/updated_pipelines/aruco_localization_pipeline.yaml rename to open_place_recognition/configs/pipelines/updated_pipelines/aruco_localization_pipeline.yaml diff --git a/src/open_place_recognition/configs/pipelines/updated_pipelines/localization_pipeline.yaml b/open_place_recognition/configs/pipelines/updated_pipelines/localization_pipeline.yaml similarity index 100% rename from src/open_place_recognition/configs/pipelines/updated_pipelines/localization_pipeline.yaml rename to open_place_recognition/configs/pipelines/updated_pipelines/localization_pipeline.yaml diff --git a/src/open_place_recognition/configs/pipelines/updated_pipelines/multimodal_pr.yaml b/open_place_recognition/configs/pipelines/updated_pipelines/multimodal_pr.yaml similarity index 100% rename from src/open_place_recognition/configs/pipelines/updated_pipelines/multimodal_pr.yaml rename to open_place_recognition/configs/pipelines/updated_pipelines/multimodal_pr.yaml diff --git a/src/open_place_recognition/configs/pipelines/updated_pipelines/multimodal_semantic_pr.yaml b/open_place_recognition/configs/pipelines/updated_pipelines/multimodal_semantic_pr.yaml similarity index 100% rename from src/open_place_recognition/configs/pipelines/updated_pipelines/multimodal_semantic_pr.yaml rename to open_place_recognition/configs/pipelines/updated_pipelines/multimodal_semantic_pr.yaml diff --git a/src/open_place_recognition/configs/pipelines/updated_pipelines/multimodal_semantic_with_soc_outdoor_pr.yaml b/open_place_recognition/configs/pipelines/updated_pipelines/multimodal_semantic_with_soc_outdoor_pr.yaml similarity index 100% rename from src/open_place_recognition/configs/pipelines/updated_pipelines/multimodal_semantic_with_soc_outdoor_pr.yaml rename to open_place_recognition/configs/pipelines/updated_pipelines/multimodal_semantic_with_soc_outdoor_pr.yaml diff --git a/src/open_place_recognition/configs/pipelines/updated_pipelines/multimodal_with_soc_outdoor_pr.yaml b/open_place_recognition/configs/pipelines/updated_pipelines/multimodal_with_soc_outdoor_pr.yaml similarity index 100% rename from src/open_place_recognition/configs/pipelines/updated_pipelines/multimodal_with_soc_outdoor_pr.yaml rename to open_place_recognition/configs/pipelines/updated_pipelines/multimodal_with_soc_outdoor_pr.yaml diff --git a/src/open_place_recognition/configs/pipelines/updated_pipelines/text_labels_pr.yaml b/open_place_recognition/configs/pipelines/updated_pipelines/text_labels_pr.yaml similarity index 100% rename from src/open_place_recognition/configs/pipelines/updated_pipelines/text_labels_pr.yaml rename to open_place_recognition/configs/pipelines/updated_pipelines/text_labels_pr.yaml diff --git a/src/open_place_recognition/configs/sensors/husky.yaml b/open_place_recognition/configs/sensors/husky.yaml similarity index 100% rename from src/open_place_recognition/configs/sensors/husky.yaml rename to open_place_recognition/configs/sensors/husky.yaml diff --git a/open_place_recognition/launch/dataset_from_bag.launch.py b/open_place_recognition/launch/dataset_from_bag.launch.py new file mode 100644 index 0000000..e9b8611 --- /dev/null +++ b/open_place_recognition/launch/dataset_from_bag.launch.py @@ -0,0 +1,26 @@ +# launch/convert.launch.py +import os +import launch +import launch_ros.actions + +home_dir = os.path.expanduser("~") + +def generate_launch_description(): + return launch.LaunchDescription([ + launch_ros.actions.Node( + package='your_package_name', + executable='bag_converter_node', + name='bag_converter_node', + parameters=[ + {"input_dir": os.path.join(home_dir, 'ros2_bags')}, + {"trajectory_file": os.path.join(home_dir, 'ros2_bags/trajectory.db3')}, + {"out_dir": os.path.expanduser('~/.ros/opr_dataset')}, + {"distance_threshold": 5.0}, + {"max_diff": 60000000}, + {"front_cam_topic": "/front/depth_camera/image_raw"}, + {"back_cam_topic": "/back/depth_camera/image_raw"}, + {"lidar_topic": "/lidar/points2_raw"}, + {"trajectory_topic": "/global_trajectory_0"}, + ], + ) + ]) diff --git a/open_place_recognition/launch/dataset_from_rtabmap.launch.py b/open_place_recognition/launch/dataset_from_rtabmap.launch.py new file mode 100644 index 0000000..e960683 --- /dev/null +++ b/open_place_recognition/launch/dataset_from_rtabmap.launch.py @@ -0,0 +1,43 @@ +#!/usr/bin/env python3 +import os +import sys +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import OpaqueFunction, DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + +def generate_launch_description(): + map_name = LaunchConfiguration('map_name') + input_path = LaunchConfiguration('input_path') + output_path = LaunchConfiguration('output_path') + + return LaunchDescription([ + DeclareLaunchArgument( + 'input_path', + default_value='~/Sync/map', + description='Path to the dataset directory' + ), + DeclareLaunchArgument( + 'map_name', + default_value='my_map', + description='Map name for dataset creation' + ), + DeclareLaunchArgument( + 'output_path', + default_value='~/.ros/opr_dataset', + description='Map name for dataset creation' + ), + Node( + package='orca_opr', + executable='dataset_from_rtabmap_node.py', + name='dataset_from_rtabmap', + output='screen', + parameters=[{ + 'input_path': input_path, + 'map_name': map_name, + 'output_path': output_path, + }], + ) + ]) diff --git a/open_place_recognition/launch/dataset_indexing.launch.py b/open_place_recognition/launch/dataset_indexing.launch.py new file mode 100644 index 0000000..aa142fe --- /dev/null +++ b/open_place_recognition/launch/dataset_indexing.launch.py @@ -0,0 +1,71 @@ +#!/usr/bin/env python3 +import os +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + +def generate_launch_description(): + + dataset_path_arg = DeclareLaunchArgument( + 'dataset_path', + default_value=f'{os.path.expanduser("~")}/Datasets/itlp_campus_outdoor/01_2023-02-21', + description='Root directory of the database track (3D data).' + ) + output_path_arg = DeclareLaunchArgument( + 'output_path', + default_value=f'{os.path.expanduser("~")}/Datasets/itlp_campus_outdoor/01_2023-02-21', + description='Where to save the index.faiss and any outputs.' + ) + batch_size_arg = DeclareLaunchArgument( + 'batch_size', + default_value='64', + description='Batch size for the DataLoader.' + ) + num_workers_arg = DeclareLaunchArgument( + 'num_workers', + default_value='4', + description='Number of CPU workers for DataLoader.' + ) + device_arg = DeclareLaunchArgument( + 'device', + default_value='cuda', + description='Device to run the model on (cuda or cpu).' + ) + model_config_path_arg = DeclareLaunchArgument( + 'model_config_path', + default_value=f'{os.path.expanduser("~")}/OpenPlaceRecognition/configs/model/place_recognition/minkloc3d.yaml', + description='Path to MinkLoc3D Hydra config.' + ) + weights_path_arg = DeclareLaunchArgument( + 'weights_path', + default_value=f'{os.path.expanduser("~")}/OpenPlaceRecognition/weights/place_recognition/minkloc3d_nclt.pth', + description='Path to the MinkLoc3D pre-trained weights.' + ) + + return LaunchDescription([ + dataset_path_arg, + output_path_arg, + batch_size_arg, + num_workers_arg, + device_arg, + model_config_path_arg, + weights_path_arg, + + Node( + package='open_place_recognition', + executable='dataset_indexing_node.py', + name='dataset_indexing_node', + output='screen', + parameters=[{ + 'dataset_path': LaunchConfiguration('dataset_path'), + 'output_path': LaunchConfiguration('output_path'), + 'dataset_path': LaunchConfiguration('dataset_path'), + 'batch_size': LaunchConfiguration('batch_size'), + 'num_workers': LaunchConfiguration('num_workers'), + 'device': LaunchConfiguration('device'), + 'model_config_path': LaunchConfiguration('model_config_path'), + 'weights_path': LaunchConfiguration('weights_path'), + }], + ) + ]) diff --git a/open_place_recognition/launch/dataset_publisher.launch.py b/open_place_recognition/launch/dataset_publisher.launch.py new file mode 100644 index 0000000..cfbd344 --- /dev/null +++ b/open_place_recognition/launch/dataset_publisher.launch.py @@ -0,0 +1,189 @@ +#!/usr/bin/env python3 + +import os +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration + +def generate_launch_description(): + # Declare launch arguments with default values + dataset_dir_arg = DeclareLaunchArgument( + 'dataset_dir', + default_value=os.path.join(os.path.expanduser("~"), "Datasets", "06_2023-08-18-night"), + description='Path to the dataset directory (database path)' + ) + enable_front_camera_arg = DeclareLaunchArgument( + 'enable_front_camera', + default_value='true', + description='Enable front camera' + ) + enable_back_camera_arg = DeclareLaunchArgument( + 'enable_back_camera', + default_value='true', + description='Enable back camera' + ) + enable_lidar_arg = DeclareLaunchArgument( + 'enable_lidar', + default_value='true', + description='Enable lidar' + ) + enable_global_ref_arg = DeclareLaunchArgument( + 'enable_global_ref', + default_value='false', + description='Enable global reference' + ) + global_ref_topic_arg = DeclareLaunchArgument( + 'global_ref_topic', + default_value='/global_ref', + description='Global reference topic' + ) + reserve_arg = DeclareLaunchArgument( + 'reserve', + default_value='false', + description='Reserve parameter' + ) + + # Topic names + front_cam_topic_arg = DeclareLaunchArgument( + 'front_cam_topic', + default_value='/zed_node/left/image_rect_color/compressed', + description='Front camera topic' + ) + front_cam_mask_topic_arg = DeclareLaunchArgument( + 'front_cam_mask_topic', + default_value='/zed_node/left/semantic_segmentation', + description='Front camera mask topic' + ) + front_cam_info_topic_arg = DeclareLaunchArgument( + 'front_cam_info_topic', + default_value='/zed_node/left/image_rect_color/camera_info', + description='Front camera info topic' + ) + back_cam_topic_arg = DeclareLaunchArgument( + 'back_cam_topic', + default_value='/realsense_back/color/image_raw/compressed', + description='Back camera topic' + ) + back_cam_mask_topic_arg = DeclareLaunchArgument( + 'back_cam_mask_topic', + default_value='/realsense_back/semantic_segmentation', + description='Back camera mask topic' + ) + back_cam_info_topic_arg = DeclareLaunchArgument( + 'back_cam_info_topic', + default_value='/realsense_back/color/image_raw/camera_info', + description='Back camera info topic' + ) + lidar_topic_arg = DeclareLaunchArgument( + 'lidar_topic', + default_value='/velodyne_points', + description='Lidar topic' + ) + # TF frames + tf_parent_frame_arg = DeclareLaunchArgument( + 'tf_parent_frame', + default_value='base_link', + description='TF parent frame' + ) + front_cam_frame_arg = DeclareLaunchArgument( + 'front_cam_frame', + default_value='zed_left', + description='Front camera TF frame' + ) + back_cam_frame_arg = DeclareLaunchArgument( + 'back_cam_frame', + default_value='realsense_back', + description='Back camera TF frame' + ) + lidar_frame_arg = DeclareLaunchArgument( + 'lidar_frame', + default_value='velodyne', + description='Lidar TF frame' + ) + + # New QoS arguments (separate for front camera, back camera, lidar, global ref) + qos_front_cam_arg = DeclareLaunchArgument( + 'qos_front_cam', + default_value='2', + description='QoS for front camera (0=SystemDefault,1=BestEffort,2=Reliable)' + ) + qos_back_cam_arg = DeclareLaunchArgument( + 'qos_back_cam', + default_value='2', + description='QoS for back camera (0=SystemDefault,1=BestEffort,2=Reliable)' + ) + qos_lidar_arg = DeclareLaunchArgument( + 'qos_lidar', + default_value='2', + description='QoS for lidar (0=SystemDefault,1=BestEffort,2=Reliable)' + ) + qos_global_ref_arg = DeclareLaunchArgument( + 'qos_global_ref', + default_value='2', + description='QoS for global ref subscription (0=SystemDefault,1=BestEffort,2=Reliable)' + ) + + # Use LaunchConfiguration substitutions to pass these values as parameters + params = { + "dataset_dir": LaunchConfiguration('dataset_dir'), + "enable_front_camera": LaunchConfiguration('enable_front_camera'), + "enable_back_camera": LaunchConfiguration('enable_back_camera'), + "enable_lidar": LaunchConfiguration('enable_lidar'), + "enable_global_ref": LaunchConfiguration('enable_global_ref'), + "global_ref_topic": LaunchConfiguration('global_ref_topic'), + "reserve": LaunchConfiguration('reserve'), + # Topic names + "front_cam_topic": LaunchConfiguration('front_cam_topic'), + "front_cam_mask_topic": LaunchConfiguration('front_cam_mask_topic'), + "front_cam_info_topic": LaunchConfiguration('front_cam_info_topic'), + "back_cam_topic": LaunchConfiguration('back_cam_topic'), + "back_cam_mask_topic": LaunchConfiguration('back_cam_mask_topic'), + "back_cam_info_topic": LaunchConfiguration('back_cam_info_topic'), + "lidar_topic": LaunchConfiguration('lidar_topic'), + # TF frames + "tf_parent_frame": LaunchConfiguration('tf_parent_frame'), + "front_cam_frame": LaunchConfiguration('front_cam_frame'), + "back_cam_frame": LaunchConfiguration('back_cam_frame'), + "lidar_frame": LaunchConfiguration('lidar_frame'), + # QoS + "qos_front_cam": LaunchConfiguration('qos_front_cam'), + "qos_back_cam": LaunchConfiguration('qos_back_cam'), + "qos_lidar": LaunchConfiguration('qos_lidar'), + "qos_global_ref": LaunchConfiguration('qos_global_ref'), + } + + return LaunchDescription([ + # Declare all arguments + dataset_dir_arg, + enable_front_camera_arg, + enable_back_camera_arg, + enable_lidar_arg, + enable_global_ref_arg, + global_ref_topic_arg, + reserve_arg, + front_cam_topic_arg, + front_cam_mask_topic_arg, + front_cam_info_topic_arg, + back_cam_topic_arg, + back_cam_mask_topic_arg, + back_cam_info_topic_arg, + lidar_topic_arg, + tf_parent_frame_arg, + front_cam_frame_arg, + back_cam_frame_arg, + lidar_frame_arg, + qos_front_cam_arg, + qos_back_cam_arg, + qos_lidar_arg, + qos_global_ref_arg, + # Launch the node with the parameters + Node( + package='open_place_recognition', + executable='dataset_publisher_node.py', + name='opr_dataset_publisher', + output='screen', + emulate_tty=True, + parameters=[params] + ) + ]) diff --git a/open_place_recognition/launch/localization.launch.py b/open_place_recognition/launch/localization.launch.py new file mode 100644 index 0000000..2958a6f --- /dev/null +++ b/open_place_recognition/launch/localization.launch.py @@ -0,0 +1,157 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +import os + +def generate_launch_description(): + # Get the path to the share directory of the package + config_dir = os.path.join( + get_package_share_directory('open_place_recognition'), + 'configs/pipelines' + ) + qos_front_camera_arg = DeclareLaunchArgument( + 'qos_front_camera', + default_value='2', + description='QoS for front camera (0=SystemDefault,1=BestEffort,2=Reliable)' + ) + qos_back_camera_arg = DeclareLaunchArgument( + 'qos_back_camera', + default_value='2', + description='QoS for back camera (0=SystemDefault,1=BestEffort,2=Reliable)' + ) + qos_lidar_arg = DeclareLaunchArgument( + 'qos_lidar', + default_value='2', + description='QoS for lidar (0=SystemDefault,1=BestEffort,2=Reliable)' + ) + qos_global_ref_arg = DeclareLaunchArgument( + 'qos_global_ref', + default_value='2', + description='QoS for global reference subscription (0=SystemDefault,1=BestEffort,2=Reliable)' + ) + + # Declare launch arguments for configurable parameters + launch_args = [ + qos_front_camera_arg, + qos_back_camera_arg, + qos_lidar_arg, + qos_global_ref_arg, + # Topics for images and masks + DeclareLaunchArgument( + 'image_front_topic', + default_value='/zed_node/left/image_rect_color/compressed', + description='Front camera image topic.' + ), + DeclareLaunchArgument( + 'image_back_topic', + default_value='/realsense_back/color/image_raw/compressed', + description='Back camera image topic.' + ), + DeclareLaunchArgument( + 'mask_front_topic', + default_value='/zed_node/left/semantic_segmentation', + description='Front semantic segmentation mask topic.' + ), + DeclareLaunchArgument( + 'mask_back_topic', + default_value='/realsense_back/semantic_segmentation', + description='Back semantic segmentation mask topic.' + ), + # Lidar topic + DeclareLaunchArgument( + 'lidar_topic', + default_value='/velodyne_points', + description='Lidar pointcloud topic.' + ), + # Global reference system (e.g. GPS, barometer) parameters + DeclareLaunchArgument( + 'enable_global_ref', + default_value='true', + description='Enable subscription to global reference system (e.g. GPS, barometer).' + ), + DeclareLaunchArgument( + 'global_ref_topic', + default_value='/global_ref', + description='Global reference system topic (WGS84).' + ), + DeclareLaunchArgument( + 'dataset_dir', + default_value=os.path.join(os.path.expanduser("~"), "Datasets/itlp_campus_outdoor/01_2023-02-21"), + description='Path to the dataset directory (database path)' + ), + DeclareLaunchArgument( + 'pipeline_cfg', + default_value=os.path.join(config_dir, 'localization_pipeline.yaml'), + description='Path to the pipeline configuration file.' + ), + DeclareLaunchArgument( + 'image_resize', + default_value='[320, 192]', + description='Image resize dimensions.' + ), + DeclareLaunchArgument( + 'exclude_dynamic_classes', + default_value='false', + description='Exclude dynamic objects from the input data.' + ), + # New sensor enable/disable flags + DeclareLaunchArgument( + 'enable_front_camera', + default_value='true', + description='Enable the front camera.' + ), + DeclareLaunchArgument( + 'enable_back_camera', + default_value='true', + description='Enable the back camera.' + ), + DeclareLaunchArgument( + 'enable_lidar', + default_value='true', + description='Enable the lidar sensor.' + ), + # Reserve variable for future use + DeclareLaunchArgument( + 'reserve', + default_value='false', + description='Reserve variable for future use.' + ) + ] + + params = { + "qos_front_camera": LaunchConfiguration("qos_front_camera"), + "qos_back_camera": LaunchConfiguration("qos_back_camera"), + "qos_lidar": LaunchConfiguration("qos_lidar"), + "qos_global_ref": LaunchConfiguration("qos_global_ref"), + "image_front_topic": LaunchConfiguration("image_front_topic"), + "image_back_topic": LaunchConfiguration("image_back_topic"), + "mask_front_topic": LaunchConfiguration("mask_front_topic"), + "mask_back_topic": LaunchConfiguration("mask_back_topic"), + "lidar_topic": LaunchConfiguration("lidar_topic"), + "dataset_dir": LaunchConfiguration("dataset_dir"), + "pipeline_cfg": LaunchConfiguration("pipeline_cfg"), + "image_resize": LaunchConfiguration("image_resize"), + "exclude_dynamic_classes": LaunchConfiguration("exclude_dynamic_classes"), + "enable_front_camera": LaunchConfiguration("enable_front_camera"), + "enable_back_camera": LaunchConfiguration("enable_back_camera"), + "enable_lidar": LaunchConfiguration("enable_lidar"), + "enable_global_ref": LaunchConfiguration("enable_global_ref"), + "global_ref_topic": LaunchConfiguration("global_ref_topic"), + "reserve": LaunchConfiguration("reserve"), + } + + localization_node = Node( + package='open_place_recognition', + executable='localization_node.py', + name='hierarchical_localization', + output='screen', + emulate_tty=True, + parameters=[params] + ) + + return LaunchDescription(launch_args + [localization_node]) + +if __name__ == '__main__': + generate_launch_description() diff --git a/open_place_recognition/launch/place_recognition.launch.py b/open_place_recognition/launch/place_recognition.launch.py new file mode 100644 index 0000000..09f99f4 --- /dev/null +++ b/open_place_recognition/launch/place_recognition.launch.py @@ -0,0 +1,158 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +import os + +def generate_launch_description(): + # Get the path to the share directory of the package + config_dir = os.path.join( + get_package_share_directory('open_place_recognition'), + 'configs/pipelines/place_recognition' + ) + + # ---------------------------------------------------------------------- + # New QoS arguments for front camera, back camera, lidar, global ref: + # ---------------------------------------------------------------------- + qos_front_camera_arg = DeclareLaunchArgument( + 'qos_front_camera', + default_value='2', + description='QoS for front camera (0=SystemDefault,1=BestEffort,2=Reliable)' + ) + qos_back_camera_arg = DeclareLaunchArgument( + 'qos_back_camera', + default_value='2', + description='QoS for back camera (0=SystemDefault,1=BestEffort,2=Reliable)' + ) + qos_lidar_arg = DeclareLaunchArgument( + 'qos_lidar', + default_value='2', + description='QoS for lidar (0=SystemDefault,1=BestEffort,2=Reliable)' + ) + qos_global_ref_arg = DeclareLaunchArgument( + 'qos_global_ref', + default_value='2', + description='QoS for global reference subscription (0=SystemDefault,1=BestEffort,2=Reliable)' + ) + + # ---------------------------------------------------------------------- + # Declare launch arguments for configurable parameters (original code): + # ---------------------------------------------------------------------- + launch_arguments = [ + qos_front_camera_arg, + qos_back_camera_arg, + qos_lidar_arg, + qos_global_ref_arg, + DeclareLaunchArgument( + 'image_front_topic', + default_value='/zed_node/left/image_rect_color/compressed', + description='Front camera image topic.' + ), + DeclareLaunchArgument( + 'image_back_topic', + default_value='/realsense_back/color/image_raw/compressed', + description='Back camera image topic.' + ), + DeclareLaunchArgument( + 'mask_front_topic', + default_value='/zed_node/left/semantic_segmentation', + description='Front semantic segmentation mask topic.' + ), + DeclareLaunchArgument( + 'mask_back_topic', + default_value='/realsense_back/semantic_segmentation', + description='Back semantic segmentation mask topic.' + ), + DeclareLaunchArgument( + 'lidar_topic', + default_value='/velodyne_points', + description='Lidar pointcloud topic.' + ), + DeclareLaunchArgument( + 'dataset_dir', + default_value=os.path.join(os.path.expanduser("~"), "Datasets/itlp_campus_outdoor/01_2023-02-21"), + description='Path to the dataset directory (database path)' + ), + DeclareLaunchArgument( + 'pipeline_cfg', + default_value=os.path.join(config_dir, 'multimodal_pr.yaml'), + description='Path to the pipeline configuration file.' + ), + DeclareLaunchArgument( + 'image_resize', + default_value='[320, 192]', + description='Image resize dimensions.' + ), + # New arguments for sensor enable/disable and global reference system + DeclareLaunchArgument( + 'enable_front_camera', + default_value='true', + description='Enable front camera.' + ), + DeclareLaunchArgument( + 'enable_back_camera', + default_value='true', + description='Enable back camera.' + ), + DeclareLaunchArgument( + 'enable_lidar', + default_value='true', + description='Enable lidar sensor.' + ), + DeclareLaunchArgument( + 'enable_global_ref', + default_value='true', + description='Enable global reference system' + ), + DeclareLaunchArgument( + 'global_ref_topic', + default_value='/global_ref', + description='Global reference system topic (e.g. GPS/Barometer, WGS84).' + ), + DeclareLaunchArgument( + 'reserve', + default_value='false', + description='Reserve variable for future use.' + ) + ] + + # ---------------------------------------------------------------------- + # Use LaunchConfiguration substitutions for all parameters (original code): + # ---------------------------------------------------------------------- + node_parameters = { + "qos_front_camera": LaunchConfiguration("qos_front_camera"), + "qos_back_camera": LaunchConfiguration("qos_back_camera"), + "qos_lidar": LaunchConfiguration("qos_lidar"), + "qos_global_ref": LaunchConfiguration("qos_global_ref"), + + "image_front_topic": LaunchConfiguration("image_front_topic"), + "image_back_topic": LaunchConfiguration("image_back_topic"), + "mask_front_topic": LaunchConfiguration("mask_front_topic"), + "mask_back_topic": LaunchConfiguration("mask_back_topic"), + "lidar_topic": LaunchConfiguration("lidar_topic"), + "dataset_dir": LaunchConfiguration("dataset_dir"), + "pipeline_cfg": LaunchConfiguration("pipeline_cfg"), + "image_resize": LaunchConfiguration("image_resize"), + "enable_front_camera": LaunchConfiguration("enable_front_camera"), + "enable_back_camera": LaunchConfiguration("enable_back_camera"), + "enable_lidar": LaunchConfiguration("enable_lidar"), + "enable_global_ref": LaunchConfiguration("enable_global_ref"), + "global_ref_topic": LaunchConfiguration("global_ref_topic"), + "reserve": LaunchConfiguration("reserve"), + } + + # Create the Node action with parameters from LaunchConfiguration + node = Node( + package='open_place_recognition', + executable='place_recognition_node.py', + name='multimodal_multicamera_lidar_place_recognition', + output='screen', + emulate_tty=True, + parameters=[node_parameters] + ) + + return LaunchDescription(launch_arguments + [node]) + +if __name__ == '__main__': + generate_launch_description() diff --git a/src/open_place_recognition/launch/depth_estimation_new.launch.py b/open_place_recognition/launch/test_depth_estimation.launch.py similarity index 62% rename from src/open_place_recognition/launch/depth_estimation_new.launch.py rename to open_place_recognition/launch/test_depth_estimation.launch.py index 07cfd51..6dc08a2 100644 --- a/src/open_place_recognition/launch/depth_estimation_new.launch.py +++ b/open_place_recognition/launch/test_depth_estimation.launch.py @@ -5,7 +5,7 @@ def generate_launch_description(): return LaunchDescription([ Node( package='open_place_recognition', - executable='depth_estimation', + executable='test_depth_estimation_node.py', name='depth_estimation_with_lidar', output='screen', emulate_tty=True, @@ -23,17 +23,17 @@ def generate_launch_description(): ] ), - Node( - package='tf2_ros', - executable='static_transform_publisher', - name='publish_tf_lidar_to_camera', - arguments="0.061 0.049 -0.131 -0.498, 0.498, -0.495, 0.510 velodyne zed_left_camera_optical_frame".split(" ") - ), + # Node( + # package='tf2_ros', + # executable='static_transform_publisher', + # name='publish_tf_lidar_to_camera', + # arguments="0.061 0.049 -0.131 -0.498, 0.498, -0.495, 0.510 velodyne zed_left_camera_optical_frame".split(" ") + # ), - Node( - package='tf2_ros', - executable='static_transform_publisher', - name='publish_tf_base_to_lidar', - arguments="-0.300 0.014 0.883 -0.016 0.009 -0.015 base_link velodyne".split(" ") - ) + # Node( + # package='tf2_ros', + # executable='static_transform_publisher', + # name='publish_tf_base_to_lidar', + # arguments="-0.300 0.014 0.883 -0.016 0.009 -0.015 base_link velodyne".split(" ") + # ) ]) diff --git a/src/open_place_recognition/launch/visualizer_launch.py b/open_place_recognition/launch/test_visualizer.launch.py similarity index 58% rename from src/open_place_recognition/launch/visualizer_launch.py rename to open_place_recognition/launch/test_visualizer.launch.py index a87bb89..6f42f31 100644 --- a/src/open_place_recognition/launch/visualizer_launch.py +++ b/open_place_recognition/launch/test_visualizer.launch.py @@ -1,18 +1,20 @@ from launch import LaunchDescription from launch_ros.actions import Node +import os def generate_launch_description(): + database_path = os.path.join(os.path.expanduser("~"), "Datasets/00_2023-10-25-night") + return LaunchDescription([ Node( package='open_place_recognition', - executable='visualizer', + executable='test_visualizer_node.py', name='place_recognition_visualizer', output='screen', emulate_tty=True, parameters=[ { - # "database_dir": "/home/docker_opr_ros2/Datasets/itlpcampus_nature_exps/databases/indoor_floor_5", - "database_dir": "/home/docker_opr_ros2/Datasets/itlpcampus_nature_exps/databases/outdoor_2023-04-11-day", + "database_dir": database_path, } ] ) diff --git a/src/open_place_recognition/package.xml b/open_place_recognition/package.xml similarity index 53% rename from src/open_place_recognition/package.xml rename to open_place_recognition/package.xml index 59c8213..685a17b 100644 --- a/src/open_place_recognition/package.xml +++ b/open_place_recognition/package.xml @@ -7,15 +7,16 @@ docker_opr_ros2 TODO: License declaration - rclpy - sensor_msgs - - ament_copyright - ament_flake8 - ament_pep257 - python3-pytest + rclcpp + std_msgs + rclpy + ament_lint_auto + ament_lint_common + rosidl_default_generators + rosidl_default_runtime + rosidl_interface_packages - ament_python + ament_cmake diff --git a/rviz/depth_estimation_outdoor.rviz b/open_place_recognition/rviz/depth_estimation_outdoor.rviz similarity index 64% rename from rviz/depth_estimation_outdoor.rviz rename to open_place_recognition/rviz/depth_estimation_outdoor.rviz index 3e0839d..1ecd00a 100644 --- a/rviz/depth_estimation_outdoor.rviz +++ b/open_place_recognition/rviz/depth_estimation_outdoor.rviz @@ -1,13 +1,16 @@ Panels: - Class: rviz_common/Displays - Help Height: 78 + Help Height: 0 Name: Displays Property Tree Widget: Expanded: - /Global Options1 - /Status1 - Splitter Ratio: 0.5 - Tree Height: 152 + - /Camera1 + - /PointCloud21 + - /Camera2 + Splitter Ratio: 0.3187499940395355 + Tree Height: 167 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -25,7 +28,7 @@ Panels: Experimental: false Name: Time SyncMode: 0 - SyncSource: Lidar point cloud + SyncSource: PointCloud2 Visualization Manager: Class: "" Displays: @@ -47,34 +50,25 @@ Visualization Manager: Plane Cell Count: 10 Reference Frame: Value: true - - Class: rviz_default_plugins/Image + - Class: rviz_default_plugins/Camera Enabled: true - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: Image - Normalize Range: true + Far Plane Distance: 100 + Image Rendering: background and overlay + Name: Camera + Overlay Alpha: 0.5 Topic: Depth: 5 Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /zed_node/left/image_rect_color - Value: true - - Class: rviz_default_plugins/Image - Enabled: true - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: Image - Normalize Range: true - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /depth_estimation/depth + Value: /realsense_back/color/image_raw/compressed Value: true + Visibility: + Camera: true + Grid: true + PointCloud2: true + Value: true + Zoom Factor: 1 - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: @@ -87,13 +81,13 @@ Visualization Manager: Color: 255; 255; 255 Color Transformer: Intensity Decay Time: 0 - Enabled: false + Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 4096 + Max Intensity: 0.729411780834198 Min Color: 0; 0; 0 Min Intensity: 0 - Name: DE point cloud + Name: PointCloud2 Position Transformer: XYZ Selectable: true Size (Pixels): 3 @@ -105,44 +99,29 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /depth_estimation/point_cloud_from_depth + Value: /velodyne_points Use Fixed Frame: true Use rainbow: true - Value: false - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 255; 255; 255 - Color Transformer: Intensity - Decay Time: 0 + Value: true + - Class: rviz_default_plugins/Camera Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 141 - Min Color: 0; 0; 0 - Min Intensity: 1 - Name: Lidar point cloud - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.009999999776482582 - Style: Flat Squares + Far Plane Distance: 100 + Image Rendering: background and overlay + Name: Camera + Overlay Alpha: 0.5 Topic: Depth: 5 Durability Policy: Volatile - Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /velodyne_points - Use Fixed Frame: true - Use rainbow: true + Value: /zed_node/left/image_rect_color/compressed Value: true + Visibility: + Camera: true + Grid: true + PointCloud2: true + Value: true + Zoom Factor: 1 Enabled: true Global Options: Background Color: 48; 48; 48 @@ -189,7 +168,7 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 38.87355041503906 + Distance: 7.723052024841309 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -204,20 +183,20 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.7347962856292725 + Pitch: 0.8153980374336243 Target Frame: Value: Orbit (rviz) - Yaw: 2.868584156036377 + Yaw: 3.248584270477295 Saved: ~ Window Geometry: + Camera: + collapsed: false Displays: collapsed: false - Height: 1016 + Height: 776 Hide Left Dock: false - Hide Right Dock: false - Image: - collapsed: false - QMainWindow State: 000000ff00000000fd0000000400000000000001dc0000035efc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b00000121000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650100000162000001180000002800fffffffb0000000a0049006d0061006700650100000280000001190000002800ffffff000000010000010f0000035efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b0000035e000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d00650100000000000007380000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000004410000035e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd0000000400000000000002820000026efc020000000dfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000000e2000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650000000233000000bc0000000000000000fb0000000c00430061006d0065007200610100000123000000c00000002800fffffffb0000000c00430061006d00650072006101000001e9000000c00000002800fffffffb0000000c00430061006d00650072006101000001b7000000ab0000000000000000fb0000000c00430061006d00650072006101000001f3000000b60000000000000000000000010000010000000227fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b00000227000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006c90000003efc0100000002fb0000000800540069006d00650100000000000006c90000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000004410000026e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -225,7 +204,7 @@ Window Geometry: Tool Properties: collapsed: false Views: - collapsed: false - Width: 1848 - X: 72 - Y: 27 + collapsed: true + Width: 1737 + X: 105 + Y: 57 diff --git a/open_place_recognition/scripts/read_scan copy.py b/open_place_recognition/scripts/read_scan copy.py new file mode 100644 index 0000000..98c934f --- /dev/null +++ b/open_place_recognition/scripts/read_scan copy.py @@ -0,0 +1,59 @@ +#!/usr/bin/env python3 +import os +import struct + +def main(): + # Path to your scan binary file (adjust as needed) + bin_path = "/home/rover2/Downloads/08_2023-10-11-night/lidar/1697045307522036406.bin" + + # Read the entire binary file. + with open(bin_path, "rb") as f: + data = f.read() + + # Print file size and header (first 16 bytes) + print("File size:", len(data)) + header = data[:16] + print("Header (hex):", header.hex()) + + # Assume the first 16 bytes are header metadata. + # The rest of the file is raw float32 data. + float_data = data[16:] + n_floats = len(float_data) // 4 + print("Number of floats:", n_floats) + + # Unpack the data as little-endian float32 values. + floats = struct.unpack("<" + "f" * n_floats, float_data) + + # Assume each point is represented by 4 floats: x, y, z, intensity (or a placeholder) + points_per_entry = 4 + if n_floats % points_per_entry != 0: + print("Warning: Total float count is not a multiple of", points_per_entry) + n_points = n_floats // points_per_entry + print("Number of points:", n_points) + print("First 10 floats:", floats[:10]) + + # Create an ASCII PCD file from these points. + pcd_filename = "1697045307522036406.pcd" + with open(pcd_filename, "w") as f: + # Write PCD header (version 0.7, ASCII format) + f.write("# .PCD v0.7 - Point Cloud Data file format\n") + f.write("VERSION 0.7\n") + f.write("FIELDS x y z intensity\n") + f.write("SIZE 4 4 4 4\n") + f.write("TYPE F F F F\n") + f.write("COUNT 1 1 1 1\n") + f.write("WIDTH {}\n".format(n_points)) + f.write("HEIGHT 1\n") + f.write("VIEWPOINT 0 0 0 1 0 0 0\n") + f.write("POINTS {}\n".format(n_points)) + f.write("DATA ascii\n") + # Write each point (one per line) + for i in range(n_points): + idx = i * points_per_entry + x, y, z, intensity = floats[idx], floats[idx+1], floats[idx+2], floats[idx+3] + f.write(f"{x} {y} {z} {intensity}\n") + + print("PCD file written:", pcd_filename) + +if __name__ == '__main__': + main() diff --git a/open_place_recognition/scripts/read_scan.py b/open_place_recognition/scripts/read_scan.py new file mode 100644 index 0000000..ab07322 --- /dev/null +++ b/open_place_recognition/scripts/read_scan.py @@ -0,0 +1,83 @@ +import struct + +with open("/home/rover2/Downloads/08_2023-10-11-night/lidar/1697045307522036406.bin", "rb") as f: + data = f.read() + +# Print the first 16 bytes in hex for inspection. +print("Header (hex):", data[:16].hex()) + +# Try interpreting the first 4 bytes as little-endian integer. +try: + count_le = struct.unpack('i', data[:4])[0] + print("Big-endian count:", count_be) +except Exception as e: + print("Error unpacking as big-endian integer:", e) + + +import zlib +import struct + +# Open the binary scan file. +with open("/home/rover2/.ros/opr_dataset/my_map/scan/node_1_9_scan.bin", "rb") as f: + data = f.read() + +print("Compressed file size:", len(data)) +print("Header (hex):", data[:16].hex()) + +# Decompress using zlib. +try: + decompressed = zlib.decompress(data) +except Exception as e: + print("Decompression failed:", e) + exit(1) + +print("Decompressed data size:", len(decompressed)) + +# (Optional) Interpret the decompressed data as a sequence of float32 values. +# Note: The internal format of scan data is defined by RTAB-Map. +n_floats = len(decompressed) // 4 # Number of 4-byte floats. +try: + floats = struct.unpack("f" * n_floats, decompressed) + print("First 10 floats:", floats[:10]) +except Exception as e: + print("Error unpacking floats:", e) + + +# Interpret the decompressed data as 32-bit floats. +n_floats = len(decompressed) // 4 +floats = struct.unpack("f" * n_floats, decompressed) + +# Assume each point consists of 4 floats: x, y, z, intensity (or a placeholder) +n_points = n_floats // 4 +print("Number of points:", n_points) +print("First 10 floats:", floats[:10]) + +# Create a PCD file (ASCII format) +pcd_filename = "node_1_9_scan.pcd" +with open(pcd_filename, "w") as f: + # Write the header for an ASCII PCD file. + f.write("# .PCD v0.7 - Point Cloud Data file format\n") + f.write("VERSION 0.7\n") + f.write("FIELDS x y z intensity\n") + f.write("SIZE 4 4 4 4\n") + f.write("TYPE F F F F\n") + f.write("COUNT 1 1 1 1\n") + f.write("WIDTH {}\n".format(n_points)) + f.write("HEIGHT 1\n") + f.write("VIEWPOINT 0 0 0 1 0 0 0\n") + f.write("POINTS {}\n".format(n_points)) + f.write("DATA ascii\n") + # Write each point. + for i in range(n_points): + idx = i * 4 + x, y, z, intensity = floats[idx], floats[idx+1], floats[idx+2], floats[idx+3] + f.write(f"{x} {y} {z} {intensity}\n") + +print("PCD file written:", pcd_filename) diff --git a/open_place_recognition/src/dataset_from_rosbag_node.py b/open_place_recognition/src/dataset_from_rosbag_node.py new file mode 100755 index 0000000..b8a6e24 --- /dev/null +++ b/open_place_recognition/src/dataset_from_rosbag_node.py @@ -0,0 +1,477 @@ +#!/usr/bin/env python3 + +import rclpy +from rclpy.node import Node +from rclpy.serialization import deserialize_message + +import rosbag2_py + +import cv2 +import numpy as np +from cv_bridge import CvBridge +from sensor_msgs.msg import CompressedImage, PointCloud2 +from sensor_msgs.point_cloud2 import read_points + +import matplotlib +import matplotlib.pyplot as plt + +from pathlib import Path +from pandas import DataFrame +from typing import Dict, List, Tuple, Union +from tqdm import tqdm + +matplotlib.use("Agg") + +############################################################################### +# Helper functions (preprocessing) +############################################################################### +def closest_values_indices(in_array: np.ndarray, from_array: np.ndarray) -> np.ndarray: + """For each element in the first array find the closest value from the second array.""" + closest_idxs = np.zeros(len(in_array), dtype=np.int64) + for i, a_val in enumerate(in_array): + abs_diffs = np.abs(from_array - a_val) + closest_idxs[i] = np.argmin(abs_diffs) + return closest_idxs + + +def filter_timestamps( + pose_ts: np.ndarray, + front_ts: np.ndarray, + back_ts: np.ndarray, + lidar_ts: np.ndarray, + max_diff: int = 10000, +) -> Tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]: + """ + Filter timestamps. For each pose_ts, find the closest front_ts, back_ts, lidar_ts + that differ by less than `max_diff`. Return the filtered indices. + """ + filtered_pose_idxs = [] + filtered_front_idxs = [] + filtered_back_idxs = [] + filtered_lidar_idxs = [] + + for i, ts in enumerate(pose_ts): + front_idx = closest_values_indices(np.array([ts]), front_ts)[0] + back_idx = closest_values_indices(np.array([ts]), back_ts)[0] + lidar_idx = closest_values_indices(np.array([ts]), lidar_ts)[0] + + if ( + abs(ts - front_ts[front_idx]) <= max_diff + and abs(ts - back_ts[back_idx]) <= max_diff + and abs(ts - lidar_ts[lidar_idx]) <= max_diff + ): + filtered_pose_idxs.append(i) + filtered_front_idxs.append(front_idx) + filtered_back_idxs.append(back_idx) + filtered_lidar_idxs.append(lidar_idx) + + return ( + np.array(filtered_pose_idxs), + np.array(filtered_front_idxs), + np.array(filtered_back_idxs), + np.array(filtered_lidar_idxs), + ) + + +def filter_by_distance_indices(utm_points: np.ndarray, distance: float = 5.0) -> np.ndarray: + """ + Filter points so that each point is ~ `distance` meters away from the previous one. + """ + filtered_points = np.array([0], dtype=int) # start with index 0 + for i in range(1, utm_points.shape[0]): + # distance from current to last filtered + right_dist = np.linalg.norm(utm_points[i] - utm_points[filtered_points[-1]]) + if right_dist >= distance: + left_dist = np.linalg.norm(utm_points[i - 1] - utm_points[filtered_points[-1]]) + if abs(right_dist - distance) < abs(left_dist - distance): + filtered_points = np.append(filtered_points, i) + else: + filtered_points = np.append(filtered_points, i - 1) + return filtered_points + + +def plot_track_map(utms: np.ndarray) -> np.ndarray: + """ + Plot a 2D track (X-Y) and return it as a BGR image (for saving via OpenCV). + """ + x, y = utms[:, 0], utms[:, 1] + x_min, x_max = np.min(x) - 2, np.max(x) + 2 + y_min, y_max = np.min(y) - 2, np.max(y) + 2 + fig, ax = plt.subplots(dpi=200) + ax.scatter(x, y, s=0.5) + ax.set_xlabel("x") + ax.set_xlim(x_min, x_max) + ax.set_ylabel("y") + ax.set_ylim(y_min, y_max) + ax.set_aspect("equal", adjustable="box") + fig.canvas.draw() + + # Convert canvas to image + img = np.frombuffer(fig.canvas.tostring_rgb(), dtype=np.uint8) + img = img.reshape(fig.canvas.get_width_height()[::-1] + (3,)) + plt.close(fig) + + # convert from RGB to BGR + img_bgr = cv2.cvtColor(img, cv2.COLOR_RGB2BGR) + return img_bgr + + +############################################################################### +# Helper functions (reading + unpacking) +############################################################################### +def merge_dicts(dict1: Dict[str, List[int]], dict2: Dict[str, List[int]]) -> Dict[str, List[int]]: + """Merge two dicts of lists, appending values of matching keys.""" + for k in dict2: + dict1[k] += dict2[k] + return dict1 + + +def read_ros2_bag_messages( + bag_file_path: Path, + wanted_topics: List[str], + max_count: int = -1, +): + """ + Reads messages from a ROS 2 bag using rosbag2_py. + Yields tuples: (topic_name, message, time_ns). + """ + # Create storage and converter options to open the bag + storage_options = rosbag2_py.StorageOptions(uri=str(bag_file_path), storage_id="sqlite3") + converter_options = rosbag2_py.ConverterOptions( + input_serialization_format="cdr", + output_serialization_format="cdr", + ) + + reader = rosbag2_py.SequentialReader() + reader.open(storage_options, converter_options) + + # Map topic name -> type so we can deserialize messages properly + type_map = {} + for topic_metadata in reader.get_all_topics_and_types(): + if topic_metadata.name in wanted_topics: + type_map[topic_metadata.name] = topic_metadata.type + + count = 0 + while reader.has_next(): + (topic, raw_data, t) = reader.read_next() + if topic not in wanted_topics: + continue + # If the topic is known, we can figure out how to deserialize + if type_map[topic] == "sensor_msgs/msg/CompressedImage": + msg = deserialize_message(raw_data, CompressedImage) + elif type_map[topic] == "sensor_msgs/msg/PointCloud2": + msg = deserialize_message(raw_data, PointCloud2) + else: + # For other types: geometry_msgs/msg/TransformStamped, etc. + # This is a minimal example, add more if needed + msg = deserialize_message(raw_data, PointCloud2) # or appropriate type + count += 1 + yield (topic, msg, t) + if max_count > 0 and count >= max_count: + break + + +def list_images_and_points_ros2( + bag_file_path: Union[str, Path], + front_cam_topic: str, + back_cam_topic: str, + lidar_topic: str, +) -> Dict[str, List[int]]: + """ + Return timestamps for images and LiDAR from a ROS 2 bag file. + """ + bag_file_path = Path(bag_file_path) + out_dict = {"front_cam": [], "back_cam": [], "lidar": []} + wanted_topics = [front_cam_topic, back_cam_topic, lidar_topic] + + for topic, msg, t_ns in tqdm( + read_ros2_bag_messages(bag_file_path, wanted_topics), + desc=f"Listing {bag_file_path.name}", + leave=False, + ): + if topic == front_cam_topic: + out_dict["front_cam"].append(t_ns) + elif topic == back_cam_topic: + out_dict["back_cam"].append(t_ns) + elif topic == lidar_topic: + out_dict["lidar"].append(t_ns) + + return out_dict + + +def export_from_bag_ros2( + bag_file_path: Union[str, Path], + output_dir: Union[str, Path], + timestamps_dict: Dict[str, np.ndarray], + front_cam_topic: str, + back_cam_topic: str, + lidar_topic: str, +): + """ + Extract images (PNG) and LiDAR points (BIN) from a ROS 2 bag file. + """ + bag_file_path = Path(bag_file_path) + output_dir = Path(output_dir) + output_dir.mkdir(parents=True, exist_ok=True) + + front_cam_dir = output_dir / "front_cam" + front_cam_dir.mkdir(exist_ok=True) + back_cam_dir = output_dir / "back_cam" + back_cam_dir.mkdir(exist_ok=True) + lidar_dir = output_dir / "lidar" + lidar_dir.mkdir(exist_ok=True) + + bridge = CvBridge() + + # Turn them into sets for quick membership tests + front_cam_set = set(timestamps_dict["front_cam"].tolist()) + back_cam_set = set(timestamps_dict["back_cam"].tolist()) + lidar_set = set(timestamps_dict["lidar"].tolist()) + + wanted_topics = [front_cam_topic, back_cam_topic, lidar_topic] + + for topic, msg, t_ns in tqdm( + read_ros2_bag_messages(bag_file_path, wanted_topics), + desc=f"Exporting {bag_file_path.name}", + leave=False, + ): + if topic == front_cam_topic: + if t_ns in front_cam_set: + # msg is sensor_msgs/CompressedImage + cv_image = bridge.compressed_imgmsg_to_cv2(msg, desired_encoding="bgr8") + cv2.imwrite(str(front_cam_dir / f"{t_ns}.png"), cv_image) + elif topic == back_cam_topic: + if t_ns in back_cam_set: + cv_image = bridge.compressed_imgmsg_to_cv2(msg, desired_encoding="bgr8") + cv2.imwrite(str(back_cam_dir / f"{t_ns}.png"), cv_image) + elif topic == lidar_topic: + if t_ns in lidar_set: + # msg is sensor_msgs/PointCloud2 + points_arr = np.array(list(read_points(msg)), dtype=np.float32) + # scale intensity from [0..255] to [0..1], if needed + points_arr[:, 3] /= 255.0 + points_file_path = lidar_dir / f"{t_ns}.bin" + points_arr[:, :4].tofile(points_file_path) + + +def read_trajectory_bag_ros2(filepath: Path, trajectory_topic: str) -> DataFrame: + """ + Reads a trajectory from a ROS 2 bag file, if it’s stored as e.g. geometry_msgs/TransformStamped. + """ + # We assume the topic is geometry_msgs/msg/TransformStamped + # If your transform is a different type, adjust accordingly. + from geometry_msgs.msg import TransformStamped + + data = { + "timestamp": [], "tx": [], "ty": [], "tz": [], + "qx": [], "qy": [], "qz": [], "qw": [] + } + wanted_topics = [trajectory_topic] + + for topic, msg, t_ns in read_ros2_bag_messages(filepath, wanted_topics): + # deserialize as TransformStamped + # (adjust if your bag actually uses a different type) + trans_msg = deserialize_message(msg.serialize(), TransformStamped) + data["timestamp"].append(t_ns) + data["tx"].append(trans_msg.transform.translation.x) + data["ty"].append(trans_msg.transform.translation.y) + data["tz"].append(trans_msg.transform.translation.z) + data["qx"].append(trans_msg.transform.rotation.x) + data["qy"].append(trans_msg.transform.rotation.y) + data["qz"].append(trans_msg.transform.rotation.z) + data["qw"].append(trans_msg.transform.rotation.w) + + return DataFrame(data=data) + + +def read_trajectory_tum(filepath: Path) -> DataFrame: + """ + Reads a trajectory from a TUM file: + """ + data = {"timestamp": [], "tx": [], "ty": [], "tz": [], "qx": [], "qy": [], "qz": [], "qw": []} + with open(filepath, "r") as f: + for line in f: + vals = line.strip().split() + if len(vals) < 8: + continue + ts_float = float(vals[0]) + timestamp = int(ts_float * 1e9) # float secs -> nanosecs + + tx, ty, tz = [float(x) for x in vals[1:4]] + qx, qy, qz, qw = [float(x) for x in vals[4:8]] + data["timestamp"].append(timestamp) + data["tx"].append(tx) + data["ty"].append(ty) + data["tz"].append(tz) + data["qx"].append(qx) + data["qy"].append(qy) + data["qz"].append(qz) + data["qw"].append(qw) + return DataFrame(data=data) + + +############################################################################### +# Main Node +############################################################################### +class BagConverterNode(Node): + def __init__(self): + super().__init__("bag_converter_node") + + # Declare ROS 2 parameters so we can override them in a launch file. + self.declare_parameter("input_dir", "/path/to/input_dir") + self.declare_parameter("trajectory_file", "/path/to/trajectory.db3") # e.g. a ROS 2 bag + self.declare_parameter("out_dir", "/path/to/output_dir") + self.declare_parameter("distance_threshold", 5.0) + self.declare_parameter("max_diff", 60000000) # 60 ms in nanoseconds + + # Topics to read from the bags + self.declare_parameter("front_cam_topic", "/zed_node/left/image_rect_color/compressed") + self.declare_parameter("back_cam_topic", "/realsense_back/color/image_raw/compressed") + self.declare_parameter("lidar_topic", "/velodyne_points") + # If your trajectory is published as geometry_msgs/TransformStamped on this topic: + self.declare_parameter("trajectory_topic", "/global_trajectory_0") + + # Fetch parameter values + input_dir = Path(self.get_parameter("input_dir").value) + trajectory_file = Path(self.get_parameter("trajectory_file").value) + out_dir = Path(self.get_parameter("out_dir").value) + distance_threshold = float(self.get_parameter("distance_threshold").value) + max_diff = int(self.get_parameter("max_diff").value) + + front_cam_topic = self.get_parameter("front_cam_topic").value + back_cam_topic = self.get_parameter("back_cam_topic").value + lidar_topic = self.get_parameter("lidar_topic").value + trajectory_topic = self.get_parameter("trajectory_topic").value + + # Run the core logic + self.run_conversion( + input_dir, + trajectory_file, + out_dir, + distance_threshold, + max_diff, + front_cam_topic, + back_cam_topic, + lidar_topic, + trajectory_topic, + ) + + def run_conversion( + self, + input_dir: Path, + trajectory_file: Path, + out_dir: Path, + dist_threshold: float, + max_diff_ns: int, + front_cam_topic: str, + back_cam_topic: str, + lidar_topic: str, + trajectory_topic: str, + ): + # Find all ROS 2 bag files in the input directory. By default, ros2 bag creates a folder + # (e.g. "my_bag") containing a "metadata.yaml" and .db3 files. + # You might want to gather all `.db3` or all subfolders. Adjust logic as needed. + bag_files_list = sorted( + f for f in input_dir.iterdir() + if f.is_file() and f.suffix in [".db3", ".mcap"] + ) + + if not bag_files_list: + self.get_logger().error(f"No ROS 2 bag files found in {input_dir}") + return + + # Merge timestamps found in the bag files + timestamps_dict = {"front_cam": [], "back_cam": [], "lidar": []} + for bag_file_path in tqdm(bag_files_list, desc="Reading timestamps", position=0): + sub_dict = list_images_and_points_ros2( + bag_file_path, + front_cam_topic, + back_cam_topic, + lidar_topic, + ) + timestamps_dict = merge_dicts(timestamps_dict, sub_dict) + + # Convert all to NumPy + timestamps_dict = {k: np.array(v) for k, v in timestamps_dict.items()} + + # Read the trajectory from either a TUM file or a ROS 2 bag + if trajectory_file.suffix in [".db3", ".mcap"]: + # assume ROS 2 bag file with geometry_msgs/TransformStamped + poses_df = read_trajectory_bag_ros2(trajectory_file, trajectory_topic) + elif trajectory_file.suffix in [".tum", ".txt"]: + poses_df = read_trajectory_tum(trajectory_file) + else: + self.get_logger().error(f"Unsupported trajectory file: {trajectory_file}") + return + + # Filter timestamps + filtered_indices = filter_timestamps( + pose_ts=poses_df["timestamp"].to_numpy(), + front_ts=timestamps_dict["front_cam"], + back_ts=timestamps_dict["back_cam"], + lidar_ts=timestamps_dict["lidar"], + max_diff=max_diff_ns, + ) + + # Apply the filter + poses_df = poses_df.iloc[filtered_indices[0]] + timestamps_dict["front_cam"] = timestamps_dict["front_cam"][filtered_indices[1]] + timestamps_dict["back_cam"] = timestamps_dict["back_cam"][filtered_indices[2]] + timestamps_dict["lidar"] = timestamps_dict["lidar"][filtered_indices[3]] + + # Filter by distance + distance_filtered_indices = filter_by_distance_indices( + poses_df[["tx", "ty", "tz"]].to_numpy(), distance=dist_threshold + ) + poses_df = poses_df.iloc[distance_filtered_indices] + timestamps_dict["front_cam"] = timestamps_dict["front_cam"][distance_filtered_indices] + timestamps_dict["back_cam"] = timestamps_dict["back_cam"][distance_filtered_indices] + timestamps_dict["lidar"] = timestamps_dict["lidar"][distance_filtered_indices] + + # Plot the final track map + track_map_img = plot_track_map(poses_df[["tx", "ty"]].to_numpy()) + + # Create output directory + if out_dir.exists(): + self.get_logger().warn(f"Output directory already exists: {out_dir}") + else: + self.get_logger().info(f"Creating output directory: {out_dir}") + out_dir.mkdir(parents=True, exist_ok=True) + + # Save the track map + cv2.imwrite(str(out_dir / "track_map.png"), track_map_img) + + # Save final CSV + out_df = poses_df.copy() + out_df["front_cam_ts"] = timestamps_dict["front_cam"] + out_df["back_cam_ts"] = timestamps_dict["back_cam"] + out_df["lidar_ts"] = timestamps_dict["lidar"] + out_df = out_df[ + ["timestamp", "front_cam_ts", "back_cam_ts", "lidar_ts", "tx", "ty", "tz", "qx", "qy", "qz", "qw"] + ] + out_df.to_csv(out_dir / "track.csv", index=False) + + # Extract images + LiDAR from each bag + for bag_file_path in tqdm(bag_files_list, desc="Exporting data", position=0): + export_from_bag_ros2( + bag_file_path, + out_dir, + timestamps_dict, + front_cam_topic, + back_cam_topic, + lidar_topic, + ) + + self.get_logger().info("Dataset conversion is complete.") + + +def main(args=None): + rclpy.init(args=args) + node = BagConverterNode() + # For one-shot execution, just destroy after finishing + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/open_place_recognition/src/dataset_from_rtabmap_node.py b/open_place_recognition/src/dataset_from_rtabmap_node.py new file mode 100755 index 0000000..b2e913f --- /dev/null +++ b/open_place_recognition/src/dataset_from_rtabmap_node.py @@ -0,0 +1,274 @@ +#!/usr/bin/env python3 +import rclpy +from rclpy.node import Node +import os +import sys +from ament_index_python.packages import get_package_share_directory +import time +import sqlite3 +import struct +import math + +# If needed for image decoding: +try: + import cv2 + import numpy as np + OPENCV_AVAILABLE = True +except ImportError: + OPENCV_AVAILABLE = False + + +def euler_to_quaternion(roll, pitch, yaw): + """ + Convert Euler angles (roll, pitch, yaw) to quaternion (qx, qy, qz, qw). + Assuming roll = x-rotation, pitch = y-rotation, yaw = z-rotation. + """ + cy = math.cos(yaw * 0.5) + sy = math.sin(yaw * 0.5) + cp = math.cos(pitch * 0.5) + sp = math.sin(pitch * 0.5) + cr = math.cos(roll * 0.5) + sr = math.sin(roll * 0.5) + + qw = cr * cp * cy + sr * sp * sy + qx = sr * cp * cy - cr * sp * sy + qy = cr * sp * cy + sr * cp * sy + qz = cr * cp * sy - sr * sp * cy + return (qx, qy, qz, qw) + + +class DatasetCreateNode(Node): + def __init__(self): + super().__init__('dataset_create_node') + self.declare_parameter('map_name', 'my_map') + self.declare_parameter('input_path', '~/Sync/map') + self.declare_parameter('output_path', '~/.ros/opr_dataset') + + map_name = self.get_parameter('map_name').value + output_path = self.get_parameter('output_path').value + dataset_dir = os.path.join(os.path.expanduser(self.get_parameter('input_path').value), map_name) + share_directory = self.get_parameter('use_share_directory').value + if not os.path.exists(dataset_dir): + self.get_logger().error(f"Databse directory {dataset_dir} not found.") + sys.exit(1) + + # Actually create the dataset + if not self.create_dataset(dataset_dir, map_name, output_path): + self.get_logger().error(f"Error creating dataset for map {map_name}") + sys.exit(1) + + def create_dataset(self, dataset_dir: str, map_name: str, output_path: str): + """ + Main routine to: + 1. Connect to the RTAB-Map database + 2. Extract node poses, camera timestamps, LIDAR timestamps, etc. + 3. Write them to a CSV + 4. Extract images from the DB + """ + self.get_logger().info(f"[CREATE] Creating dataset for map '{map_name}' at '{dataset_dir}' ...") + + # The RTAB-Map database file + rtabmap_db = os.path.join(dataset_dir, f"{map_name}.db") + + if not os.path.exists(rtabmap_db): + self.get_logger().error(f"RTAB-Map DB '{rtabmap_db}' does not exist!") + return False + + # Prepare output directories + dataset_path = os.path.join(os.path.expanduser(output_path), map_name) + if not os.path.exists(dataset_path): + os.makedirs(dataset_path) + + csv_path = os.path.join(dataset_path, "tracker.csv") + images_dir = os.path.join(dataset_path, "images") + if not os.path.exists(images_dir): + os.makedirs(images_dir) + + # -------------------- + # Extract data from DB + # -------------------- + try: + # Connect to the .db + conn = sqlite3.connect(rtabmap_db) + c = conn.cursor() + + # 1) Gather Node info (id, mapId, stamp, pose, etc.) + # + # Typically the Nodes table might be called "Node" or "Nodes". + # The columns often are something like: + # - id (PRIMARY KEY) + # - mapId + # - stamp + # - pose (BLOB) + # - ground_truth_pose (BLOB) + # - ... + # + # Adjust to match your DB schema. + c.execute("SELECT id, mapId, stamp, pose FROM Node ORDER BY stamp ASC") + node_rows = c.fetchall() + + # We will store aggregated info in a dict keyed by node_id + # so we can combine cameras, lidar, pose, etc. + node_dict = {} + for (node_id, map_id, stamp, pose_blob) in node_rows: + # Convert stamp from float to int (or keep as float, depending on your usage) + # Some DBs store stamps in seconds, others in nanoseconds, etc. + # For demonstration, let's keep them as is, or cast to int if you prefer: + timestamp = int(stamp) + + # Decode the pose (you may need '6f', '7f', '6d', '7d', etc.): + # Example: x, y, z, roll, pitch, yaw = struct.unpack('6f', pose_blob) + # or x, y, z, qx, qy, qz, qw = struct.unpack('7f', pose_blob) + # Adapt to your exact RTAB-Map version! + + x = y = z = 0.0 + qx = qy = qz = qw = 0.0 + + # Example assume 6 floats [x, y, z, roll, pitch, yaw] + try: + x, y, z, roll, pitch, yaw = struct.unpack('6f', pose_blob) + qx, qy, qz, qw = euler_to_quaternion(roll, pitch, yaw) + except: + # Fallback if your DB actually stores x,y,z,qx,qy,qz,qw + x, y, z, qx, qy, qz, qw = struct.unpack('7f', pose_blob) + + node_dict[node_id] = { + "floor": map_id, + "timestamp": timestamp, # This will be the 'primary' or 'node' timestamp + "front_cam_ts": None, + "back_cam_ts": None, + "lidar_ts": None, + "tx": x, + "ty": y, + "tz": z, + "qx": qx, + "qy": qy, + "qz": qz, + "qw": qw + } + + # 2) Gather camera images (front, back) by node + # + # Typically in the "Images" table: + # - image_id + # - node_id + # - data (BLOB) + # - stamp + # - camera_id + # - ... + # + # Adjust to match your DB schema (and camera numbering). + # We'll guess camera_id=0 => front, camera_id=1 => back. + try: + c.execute("SELECT image_id, node_id, stamp, camera_id, data FROM Images") + image_rows = c.fetchall() + for (img_id, node_id, img_stamp, cam_id, img_blob) in image_rows: + # Convert image_stamp to int if needed + img_ts = int(img_stamp) + + if node_id not in node_dict: + # This might be a node not in the Node table, skip + continue + + # Save the camera stamp in our dictionary + if cam_id == 0: + node_dict[node_id]["front_cam_ts"] = img_ts + elif cam_id == 1: + node_dict[node_id]["back_cam_ts"] = img_ts + + # Optionally decode and save the image + if OPENCV_AVAILABLE: + np_arr = np.frombuffer(img_blob, np.uint8) + img_decoded = cv2.imdecode(np_arr, cv2.IMREAD_COLOR) + if img_decoded is not None: + out_name = f"node_{node_id}_cam_{cam_id}_{img_ts}.jpg" + out_path = os.path.join(images_dir, out_name) + cv2.imwrite(out_path, img_decoded) + else: + # If you can't decode, but the data is e.g. raw JPEG, + # you could just write the BLOB directly to file: + # with open(os.path.join(images_dir, f"node_{node_id}_cam_{cam_id}.jpg"), 'wb') as img_file: + # img_file.write(img_blob) + pass + except sqlite3.OperationalError: + # Possibly no Images table + self.get_logger().warn("No Images table found or query failed. Skipping camera extraction.") + + # 3) Gather LIDAR scans by node (if stored), stamp, etc. + # Typical table might be "LaserScans" with columns: + # - id + # - node_id + # - stamp + # - scan (BLOB) + # ... + try: + c.execute("SELECT node_id, stamp FROM LaserScans") + lidar_rows = c.fetchall() + for (node_id, scan_stamp) in lidar_rows: + if node_id not in node_dict: + continue + node_dict[node_id]["lidar_ts"] = int(scan_stamp) + except sqlite3.OperationalError: + # Possibly no LaserScans table + self.get_logger().warn("No LaserScans table found or query failed. Skipping lidar extraction.") + + conn.close() + + except Exception as e: + self.get_logger().error(f"Error reading RTAB-Map DB: {e}") + return False + + # -------------------- + # Write out the CSV + # -------------------- + self.get_logger().info(f"Writing CSV file: {csv_path}") + with open(csv_path, 'w') as f: + # Header + f.write("track,floor,timestamp,front_cam_ts,back_cam_ts,lidar_ts,tx,ty,tz,qx,qy,qz,qw\n") + + # For your sample, track could be "00_", or directly map_name, etc. + # Just adapt to your usage: + track_name = f"00_{map_name}" + + # We'll iterate in ascending node timestamp order: + sorted_nodes = sorted(node_dict.values(), key=lambda x: x["timestamp"]) + for row in sorted_nodes: + floor = row["floor"] + timestamp = row["timestamp"] + front_cam_ts = row["front_cam_ts"] if row["front_cam_ts"] else 0 + back_cam_ts = row["back_cam_ts"] if row["back_cam_ts"] else 0 + lidar_ts = row["lidar_ts"] if row["lidar_ts"] else 0 + tx, ty, tz = row["tx"], row["ty"], row["tz"] + qx, qy, qz, qw = row["qx"], row["qy"], row["qz"], row["qw"] + + csv_line = ( + f"{track_name}," + f"{floor}," + f"{timestamp}," + f"{front_cam_ts}," + f"{back_cam_ts}," + f"{lidar_ts}," + f"{tx}," + f"{ty}," + f"{tz}," + f"{qx}," + f"{qy}," + f"{qz}," + f"{qw}\n" + ) + f.write(csv_line) + + self.get_logger().info("[CREATE] Dataset creation completed.") + return True + + +def main(args=None): + rclpy.init(args=args) + node = DatasetCreateNode() + # Spin briefly to process log messages + rclpy.spin_once(node, timeout_sec=1) + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/open_place_recognition/src/dataset_indexing_node.py b/open_place_recognition/src/dataset_indexing_node.py new file mode 100755 index 0000000..8a1f6d6 --- /dev/null +++ b/open_place_recognition/src/dataset_indexing_node.py @@ -0,0 +1,233 @@ +#!/usr/bin/env python3 +import os +import sys +import time + +import rclpy +from rclpy.node import Node +from ament_index_python.packages import get_package_share_directory + +import torch +import numpy as np +import faiss +from torch.utils.data import DataLoader +from tqdm import tqdm +from hydra.utils import instantiate +from omegaconf import OmegaConf + +from opr.datasets.itlp import ITLPCampus + +class DatasetIndexingNode(Node): + def __init__(self): + super().__init__('dataset_indexing_node') + # + # --------------------------- + # Declare ROS 2 Parameters + # --------------------------- + # + # General dataset paths + self.declare_parameter('dataset_path', '') + self.declare_parameter('output_path', '~/.ros/opr_dataset') + + # MinkLoc3D / descriptor extraction parameters + self.declare_parameter('batch_size', 64) + self.declare_parameter('num_workers', 4) + self.declare_parameter('device', 'cuda') + self.declare_parameter('model_config_path', 'configs/model/place_recognition/minkloc3d.yaml') + self.declare_parameter('weights_path', 'weights/place_recognition/minkloc3d_nclt.pth') + + # Additional dataset options + self.declare_parameter('mink_quantization_size', 0.5) + self.declare_parameter('load_semantics', False) + self.declare_parameter('load_text_descriptions', False) + self.declare_parameter('load_text_labels', False) + self.declare_parameter('load_aruco_labels', False) + self.declare_parameter('indoor', False) + + # + # --------------------------- + # Fetch Parameter Values + # --------------------------- + # + dataset_path = os.path.expanduser(self.get_parameter('dataset_path').value) + output_path = os.path.expanduser(self.get_parameter('output_path').value) + + # MinkLoc3D / descriptor extraction parameters + dataset_path = os.path.expanduser(self.get_parameter('dataset_path').value) + batch_size = int(self.get_parameter('batch_size').value) + num_workers = int(self.get_parameter('num_workers').value) + device = self.get_parameter('device').value + model_config_path = self.get_parameter('model_config_path').value + weights_path = self.get_parameter('weights_path').value + + # Additional dataset options + mink_quantization_size = float(self.get_parameter('mink_quantization_size').value) + load_semantics = bool(self.get_parameter('load_semantics').value) + load_text_descriptions = bool(self.get_parameter('load_text_descriptions').value) + load_text_labels = bool(self.get_parameter('load_text_labels').value) + load_aruco_labels = bool(self.get_parameter('load_aruco_labels').value) + indoor = bool(self.get_parameter('indoor').value) + + # + # --------------------------- + # Check and prepare directories + # --------------------------- + # + if not os.path.exists(dataset_path): + self.get_logger().error( + f"Datasets directory {dataset_path} does not exist. Please create it and try again." + ) + sys.exit(1) + + if not os.path.exists(output_path): + self.get_logger().error( + f"Output directory {output_path} does not exist. Please create it and try again." + ) + sys.exit(1) + + # + # --------------------------- + # Run the descriptor-extraction (or "train") function + # --------------------------- + # + status_ok = self.run_descriptor_extraction( + dataset_path=dataset_path, + batch_size=batch_size, + num_workers=num_workers, + device=device, + model_config_path=model_config_path, + weights_path=weights_path, + output_path=output_path, + mink_quantization_size=mink_quantization_size, + load_semantics=load_semantics, + load_text_descriptions=load_text_descriptions, + load_text_labels=load_text_labels, + load_aruco_labels=load_aruco_labels, + indoor=indoor + ) + + if not status_ok: + self.get_logger().error("Error during descriptor extraction.") + sys.exit(1) + else: + self.get_logger().info("Descriptor extraction completed successfully.") + + def run_descriptor_extraction( + self, + dataset_path: str, + batch_size: int, + num_workers: int, + device: str, + model_config_path: str, + weights_path: str, + output_path: str, + mink_quantization_size: float, + load_semantics: bool, + load_text_descriptions: bool, + load_text_labels: bool, + load_aruco_labels: bool, + indoor: bool + ): + """ + Load dataset, run the MinkLoc3D model to extract descriptors, and build a FAISS index. + """ + try: + # + # 1. Prepare the dataset + # + self.get_logger().info(f"Loading dataset from: {dataset_path}") + db_dataset = ITLPCampus( + dataset_root=dataset_path, + sensors=["lidar"], # or parameterize if you want + mink_quantization_size=mink_quantization_size, + load_semantics=load_semantics, + load_text_descriptions=load_text_descriptions, + load_text_labels=load_text_labels, + load_aruco_labels=load_aruco_labels, + indoor=indoor, + ) + + self.get_logger().info(f"Dataset size: {len(db_dataset)}") + + db_dataloader = DataLoader( + db_dataset, + batch_size=batch_size, + shuffle=False, + num_workers=num_workers, + collate_fn=db_dataset.collate_fn, + ) + + # + # 2. Load the MinkLoc3D model + # + self.get_logger().info(f"Loading MinkLoc3D config from: {model_config_path}") + model_config = OmegaConf.load(model_config_path) + model = instantiate(model_config) + + self.get_logger().info(f"Loading MinkLoc3D weights from: {weights_path}") + model.load_state_dict(torch.load(weights_path)) + model = model.to(device) + model.eval() + + # + # 3. Extract descriptors + # + descriptors_list = [] + self.get_logger().info("Extracting descriptors...") + with torch.no_grad(): + for batch in tqdm(db_dataloader, desc="Descriptor Extraction", leave=True): + # Move input tensors to the correct device + batch = {k: v.to(device) for k, v in batch.items()} + # Forward pass + output = model(batch) + final_descriptor = output["final_descriptor"] # MinkLoc3D's key + descriptors_list.append(final_descriptor.detach().cpu().numpy()) + + descriptors = np.concatenate(descriptors_list, axis=0) + self.get_logger().info(f"Descriptors shape: {descriptors.shape}") + + # + # 4. Build a FAISS index + # + index_dim = descriptors.shape[1] + index = faiss.IndexFlatL2(index_dim) + index.add(descriptors) + self.get_logger().info(f"FAISS index trained: {index.is_trained}, total vectors: {index.ntotal}") + + # + # 5. Write the index to disk + # + faiss_index_path = os.path.join(output_path, "index.faiss") + faiss.write_index(index, faiss_index_path) + self.get_logger().info(f"FAISS index saved to: {faiss_index_path}") + + # + # 6. List directory contents (optional logging) + # + dirs = [d for d in os.listdir(dataset_path) + if os.path.isdir(os.path.join(dataset_path, d))] + files = [f for f in os.listdir(dataset_path) + if os.path.isfile(os.path.join(dataset_path, f))] + + self.get_logger().info("Directory structure in dataset root:") + for item in sorted(dirs): + self.get_logger().info(f" {item}/") + for item in sorted(files): + self.get_logger().info(f" {item}") + + return True + + except Exception as e: + self.get_logger().error(f"Exception in run_descriptor_extraction: {e}") + return False + +def main(args=None): + rclpy.init(args=args) + node = DatasetIndexingNode() + # Spin once and exit (this node does a one-shot operation) + rclpy.spin_once(node, timeout_sec=1) + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/open_place_recognition/src/dataset_publisher_node.py b/open_place_recognition/src/dataset_publisher_node.py new file mode 100755 index 0000000..76c5d0c --- /dev/null +++ b/open_place_recognition/src/dataset_publisher_node.py @@ -0,0 +1,417 @@ +#!/usr/bin/env python3 + +import os +import sys +import time +import yaml +import numpy as np +import pandas as pd +import cv2 + +import rclpy +from rclpy.node import Node +from rcl_interfaces.msg import ParameterDescriptor +from std_msgs.msg import Header +from sensor_msgs.msg import CompressedImage, Image, PointCloud2, CameraInfo, NavSatFix +from cv_bridge import CvBridge +from sensor_msgs_py import point_cloud2 +import tf2_ros +from geometry_msgs.msg import TransformStamped +from ament_index_python.packages import get_package_share_directory + +# Import QoS-related classes +from rclpy.qos import QoSProfile, QoSReliabilityPolicy + +class DatabasePublisherNode(Node): + def __init__(self): + super().__init__('database_publisher') + + # Declare parameters + self.declare_parameter('dataset_dir', '', ParameterDescriptor(description="Path to the dataset directory.")) + self.declare_parameter('enable_front_camera', True, ParameterDescriptor(description="Enable front camera publishing.")) + self.declare_parameter('enable_back_camera', True, ParameterDescriptor(description="Enable back camera publishing.")) + self.declare_parameter('enable_lidar', True, ParameterDescriptor(description="Enable lidar publishing.")) + self.declare_parameter('enable_global_ref', True, ParameterDescriptor(description="Enable global reference subscription.")) + self.declare_parameter('global_ref_topic', '/global_ref', ParameterDescriptor(description="Global reference topic.")) + self.declare_parameter('reserve', False, ParameterDescriptor(description="Reserved for future use.")) + + # Declare topic parameters + self.declare_parameter('front_cam_topic', '/zed_node/left/image_rect_color/compressed') + self.declare_parameter('front_cam_mask_topic', '/zed_node/left/semantic_segmentation') + self.declare_parameter('front_cam_info_topic', '/zed_node/left/image_rect_color/camera_info') + self.declare_parameter('back_cam_topic', '/realsense_back/color/image_raw/compressed') + self.declare_parameter('back_cam_mask_topic', '/realsense_back/semantic_segmentation') + self.declare_parameter('back_cam_info_topic', '/realsense_back/color/image_raw/camera_info') + self.declare_parameter('lidar_topic', '/velodyne_points') + + # Declare TF frame parameters + self.declare_parameter('tf_parent_frame', 'base_link') + self.declare_parameter('front_cam_frame', 'zed_left') + self.declare_parameter('back_cam_frame', 'realsense_back') + self.declare_parameter('lidar_frame', 'velodyne') + + # Declare QoS parameters + self.declare_parameter('qos_front_cam', 2, ParameterDescriptor(description="QoS for front camera (0=SystemDefault,1=BestEffort,2=Reliable)")) + self.declare_parameter('qos_back_cam', 2, ParameterDescriptor(description="QoS for back camera (0=SystemDefault,1=BestEffort,2=Reliable)")) + self.declare_parameter('qos_lidar', 2, ParameterDescriptor(description="QoS for lidar (0=SystemDefault,1=BestEffort,2=Reliable)")) + self.declare_parameter('qos_global_ref',2, ParameterDescriptor(description="QoS for global ref subscription (0=SystemDefault,1=BestEffort,2=Reliable)")) + + # Retrieve parameters + self.dataset_dir = self.get_parameter('dataset_dir').get_parameter_value().string_value + if not self.dataset_dir: + self.get_logger().error("Dataset directory not provided!") + sys.exit(1) + + self.enable_front_camera = self.get_parameter('enable_front_camera').get_parameter_value().bool_value + self.enable_back_camera = self.get_parameter('enable_back_camera').get_parameter_value().bool_value + self.enable_lidar = self.get_parameter('enable_lidar').get_parameter_value().bool_value + self.enable_global_ref = self.get_parameter('enable_global_ref').get_parameter_value().bool_value + self.global_ref_topic = self.get_parameter('global_ref_topic').get_parameter_value().string_value + self.reserve = self.get_parameter('reserve').get_parameter_value().bool_value + + # Topics and frames + self.front_cam_topic = self.get_parameter('front_cam_topic').get_parameter_value().string_value + self.front_cam_mask_topic = self.get_parameter('front_cam_mask_topic').get_parameter_value().string_value + self.front_cam_info_topic = self.get_parameter('front_cam_info_topic').get_parameter_value().string_value + self.back_cam_topic = self.get_parameter('back_cam_topic').get_parameter_value().string_value + self.back_cam_mask_topic = self.get_parameter('back_cam_mask_topic').get_parameter_value().string_value + self.back_cam_info_topic = self.get_parameter('back_cam_info_topic').get_parameter_value().string_value + self.lidar_topic = self.get_parameter('lidar_topic').get_parameter_value().string_value + + self.tf_parent_frame = self.get_parameter('tf_parent_frame').get_parameter_value().string_value + self.front_cam_frame = self.get_parameter('front_cam_frame').get_parameter_value().string_value + self.back_cam_frame = self.get_parameter('back_cam_frame').get_parameter_value().string_value + self.lidar_frame = self.get_parameter('lidar_frame').get_parameter_value().string_value + + # QoS parameter values + self.qos_front_cam_value = self.get_parameter('qos_front_cam').get_parameter_value().integer_value + self.qos_back_cam_value = self.get_parameter('qos_back_cam').get_parameter_value().integer_value + self.qos_lidar_value = self.get_parameter('qos_lidar').get_parameter_value().integer_value + self.qos_global_ref_value = self.get_parameter('qos_global_ref').get_parameter_value().integer_value + + # Create QoS profiles for each + self.qos_front_cam_profile = self._create_qos_profile(self.qos_front_cam_value) + self.qos_back_cam_profile = self._create_qos_profile(self.qos_back_cam_value) + self.qos_lidar_profile = self._create_qos_profile(self.qos_lidar_value) + self.qos_global_ref_profile = self._create_qos_profile(self.qos_global_ref_value, depth=10) # subscription with depth=10 + + # Create publishers using the parameterized topics + if self.enable_front_camera: + self.pub_front_cam = self.create_publisher(CompressedImage, self.front_cam_topic, self.qos_front_cam_profile) + self.pub_front_cam_mask = self.create_publisher(Image, self.front_cam_mask_topic, self.qos_front_cam_profile) + self.pub_front_cam_info = self.create_publisher(CameraInfo, self.front_cam_info_topic, self.qos_front_cam_profile) + else: + self.pub_front_cam = None + self.pub_front_cam_mask = None + self.pub_front_cam_info = None + + if self.enable_back_camera: + self.pub_back_cam = self.create_publisher(CompressedImage, self.back_cam_topic, self.qos_back_cam_profile) + self.pub_back_cam_mask = self.create_publisher(Image, self.back_cam_mask_topic, self.qos_back_cam_profile) + self.pub_back_cam_info = self.create_publisher(CameraInfo, self.back_cam_info_topic, self.qos_back_cam_profile) + else: + self.pub_back_cam = None + self.pub_back_cam_mask = None + self.pub_back_cam_info = None + + if self.enable_lidar: + self.pub_lidar = self.create_publisher(PointCloud2, self.lidar_topic, self.qos_lidar_profile) + else: + self.pub_lidar = None + + # TF Broadcaster for publishing transforms + self.tf_broadcaster = tf2_ros.TransformBroadcaster(self) + + # Global reference subscription (if enabled) + if self.enable_global_ref: + self.global_ref_sub = self.create_subscription( + NavSatFix, + self.global_ref_topic, + self.global_ref_callback, + self.qos_global_ref_profile + ) + else: + self.global_ref_sub = None + self.global_ref = None + + # Load CSV (track.csv) + csv_path = os.path.join(self.dataset_dir, 'track.csv') + dtypes = {'floor': int, 'timestamp': np.int64, 'front_cam_ts': np.int64, + 'back_cam_ts': np.int64, 'lidar_ts': np.int64} + self.track_df = pd.read_csv(csv_path, dtype=dtypes) + self.get_logger().info(f"Loaded track.csv with {len(self.track_df)} rows.") + + self.cv_bridge = CvBridge() + + # Load sensor configuration from husky.yaml + config_path = os.path.join(get_package_share_directory('open_place_recognition'), + 'configs', 'sensors', 'husky.yaml') + try: + with open(config_path, 'r') as f: + self.sensor_config = yaml.safe_load(f) + self.get_logger().info("Loaded sensor configuration from husky.yaml") + except Exception as e: + self.get_logger().error(f"Failed to load sensor configuration: {e}") + self.sensor_config = None + + self.get_logger().info("DatabasePublisherNode initialized.") + + def _create_qos_profile(self, qos_value, depth=1): + """ + Create a QoSProfile based on integer (0=SystemDefault,1=BestEffort,2=Reliable). + """ + qos_profile = QoSProfile(depth=depth) + if qos_value == 0: + qos_profile.reliability = QoSReliabilityPolicy.SYSTEM_DEFAULT + elif qos_value == 1: + qos_profile.reliability = QoSReliabilityPolicy.BEST_EFFORT + else: + qos_profile.reliability = QoSReliabilityPolicy.RELIABLE + return qos_profile + + def global_ref_callback(self, msg): + self.global_ref = msg + + def publish_sensor_config(self): + if not self.sensor_config: + return + + now = self.get_clock().now().to_msg() + + # Front camera configuration publishing + if self.enable_front_camera and self.pub_front_cam_info is not None: + try: + front = self.sensor_config['front_cam']['left'] + tfs = TransformStamped() + tfs.header.stamp = now + tfs.header.frame_id = self.tf_parent_frame + tfs.child_frame_id = self.front_cam_frame + t = front['baselink2cam']['t'] + q = front['baselink2cam']['q'] + tfs.transform.translation.x = t[0] + tfs.transform.translation.y = t[1] + tfs.transform.translation.z = t[2] + tfs.transform.rotation.x = q[1] + tfs.transform.rotation.y = q[2] + tfs.transform.rotation.z = q[3] + tfs.transform.rotation.w = q[0] + self.tf_broadcaster.sendTransform(tfs) + + cam_info = CameraInfo() + cam_info.header.stamp = now + cam_info.header.frame_id = self.front_cam_frame + resolution = front['resolution'] + cam_info.width = resolution[0] + cam_info.height = resolution[1] + cam_info.distortion_model = "plumb_bob" + cam_info.d = [0.0]*5 + P = front['rect']['P'] + cam_info.p = [elem for row in P for elem in row] + cam_info.k = [P[0][0], P[0][1], P[0][2], + P[1][0], P[1][1], P[1][2], + P[2][0], P[2][1], P[2][2]] + cam_info.r = [1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, 1.0] + self.pub_front_cam_info.publish(cam_info) + except Exception as e: + self.get_logger().error(f"Error publishing front_cam config: {e}") + + # Back camera configuration publishing + if self.enable_back_camera and self.pub_back_cam_info is not None: + try: + back = self.sensor_config['back_cam']['left'] + tbs = TransformStamped() + tbs.header.stamp = now + tbs.header.frame_id = self.tf_parent_frame + tbs.child_frame_id = self.back_cam_frame + t = back['baselink2cam']['t'] + q = back['baselink2cam']['q'] + tbs.transform.translation.x = t[0] + tbs.transform.translation.y = t[1] + tbs.transform.translation.z = t[2] + tbs.transform.rotation.x = q[1] + tbs.transform.rotation.y = q[2] + tbs.transform.rotation.z = q[3] + tbs.transform.rotation.w = q[0] + self.tf_broadcaster.sendTransform(tbs) + + cam_info = CameraInfo() + cam_info.header.stamp = now + cam_info.header.frame_id = self.back_cam_frame + resolution = back['resolution'] + cam_info.width = resolution[0] + cam_info.height = resolution[1] + cam_info.distortion_model = "plumb_bob" + cam_info.d = [0.0]*5 + P = back['rect']['P'] + cam_info.p = [elem for row in P for elem in row] + cam_info.k = [P[0][0], P[0][1], P[0][2], + P[1][0], P[1][1], P[1][2], + P[2][0], P[2][1], P[2][2]] + cam_info.r = [1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, 1.0] + self.pub_back_cam_info.publish(cam_info) + except Exception as e: + self.get_logger().error(f"Error publishing back_cam config: {e}") + + # Lidar configuration publishing + if self.enable_lidar: + try: + lidar = self.sensor_config['lidar'] + tls = TransformStamped() + tls.header.stamp = now + tls.header.frame_id = self.tf_parent_frame + tls.child_frame_id = self.lidar_frame + t = lidar['baselink2lidar']['t'] + q = lidar['baselink2lidar']['q'] + tls.transform.translation.x = t[0] + tls.transform.translation.y = t[1] + tls.transform.translation.z = t[2] + tls.transform.rotation.x = q[1] + tls.transform.rotation.y = q[2] + tls.transform.rotation.z = q[3] + tls.transform.rotation.w = q[0] + self.tf_broadcaster.sendTransform(tls) + except Exception as e: + self.get_logger().error(f"Error publishing lidar config: {e}") + + def global_ref_callback(self, msg): + self.global_ref = msg + + def publish_one_row(self, i): + self.publish_sensor_config() + floor_num = int(self.track_df['floor'][i]) + floor_folder = f"floor_{floor_num}" + back_cam_ts = int(self.track_df['back_cam_ts'][i]) + front_cam_ts = int(self.track_df['front_cam_ts'][i]) + lidar_ts = int(self.track_df['lidar_ts'][i]) + self.get_logger().info(f"Publishing data for {floor_folder}") + + if self.enable_back_camera: + back_cam_path = os.path.join(self.dataset_dir, floor_folder, 'back_cam', f"{back_cam_ts}.png") + msg = self.read_image_as_compressed(back_cam_path, frame_id=self.back_cam_frame) + if msg and self.pub_back_cam: + self.pub_back_cam.publish(msg) + mask_path = os.path.join(self.dataset_dir, floor_folder, 'masks', 'back_cam', f"{back_cam_ts}.png") + mask_msg = self.read_image_as_uncompressed(mask_path, frame_id=self.back_cam_frame) + if mask_msg and self.pub_back_cam_mask: + self.pub_back_cam_mask.publish(mask_msg) + + if self.enable_front_camera: + front_cam_path = os.path.join(self.dataset_dir, floor_folder, 'front_cam', f"{front_cam_ts}.png") + msg = self.read_image_as_compressed(front_cam_path, frame_id=self.front_cam_frame) + if msg and self.pub_front_cam: + self.pub_front_cam.publish(msg) + mask_path = os.path.join(self.dataset_dir, floor_folder, 'masks', 'front_cam', f"{front_cam_ts}.png") + mask_msg = self.read_image_as_uncompressed(mask_path, frame_id=self.front_cam_frame) + if mask_msg and self.pub_front_cam_mask: + self.pub_front_cam_mask.publish(mask_msg) + + if self.enable_lidar: + lidar_path = os.path.join(self.dataset_dir, floor_folder, 'lidar', f"{lidar_ts}.bin") + lidar_msg = self.read_lidar_as_pointcloud2(lidar_path, frame_id=self.lidar_frame) + if lidar_msg and self.pub_lidar: + self.pub_lidar.publish(lidar_msg) + + def publish_loop(self): + if len(self.track_df) < 1: + self.get_logger().warn("track.csv is empty. Nothing to publish.") + return + + timestamps = self.track_df['timestamp'] + for i in range(len(self.track_df)): + self.publish_one_row(i) + if i < len(self.track_df) - 1: + dt_ns = timestamps[i + 1] - timestamps[i] + dt_s = dt_ns / 1e9 + if dt_s < 0 or dt_s > 10: + self.get_logger().warn(f"Skipping sleep due to invalid dt at index {i}.") + continue + time.sleep(dt_s) + self.get_logger().info("Finished publishing all rows from track.csv.") + + def read_image_as_compressed(self, image_path, frame_id='camera'): + if not os.path.exists(image_path): + self.get_logger().error(f"Image not found: {image_path}") + return None + try: + with open(image_path, 'rb') as f: + image_data = f.read() + msg = CompressedImage() + msg.header.stamp = self.get_clock().now().to_msg() + msg.header.frame_id = frame_id + msg.format = "png" + msg.data = list(image_data) + return msg + except Exception as e: + self.get_logger().error(f"Failed to read compressed image: {e}") + return None + + def read_image_as_uncompressed(self, image_path, frame_id='camera'): + if not os.path.exists(image_path): + self.get_logger().error(f"Mask image not found: {image_path}") + return None + + img = cv2.imread(image_path, cv2.IMREAD_UNCHANGED) + if img is None: + self.get_logger().error(f"Failed to read image {image_path}") + return None + + if len(img.shape) == 2: + encoding = "mono8" + elif len(img.shape) == 3: + if img.shape[2] == 1: + encoding = "mono8" + elif img.shape[2] == 3: + encoding = "bgr8" + elif img.shape[2] == 4: + encoding = "bgra8" + else: + encoding = "bgr8" + else: + encoding = "bgr8" + try: + msg = self.cv_bridge.cv2_to_imgmsg(img, encoding=encoding) + except Exception as e: + self.get_logger().error(f"cv_bridge conversion error: {e}") + return None + msg.header.stamp = self.get_clock().now().to_msg() + msg.header.frame_id = frame_id + return msg + + def read_lidar_as_pointcloud2(self, lidar_path, frame_id='velodyne'): + if not os.path.exists(lidar_path): + self.get_logger().error(f"Lidar file not found: {lidar_path}") + return None + try: + points = np.fromfile(lidar_path, dtype=np.float32).reshape(-1, 4) + except Exception as e: + self.get_logger().error(f"Error reading lidar file: {e}") + return None + header = Header() + header.stamp = self.get_clock().now().to_msg() + header.frame_id = frame_id + from sensor_msgs.msg import PointField + fields = [ + PointField(name='x', offset=0, datatype=PointField.FLOAT32, count=1), + PointField(name='y', offset=4, datatype=PointField.FLOAT32, count=1), + PointField(name='z', offset=8, datatype=PointField.FLOAT32, count=1), + PointField(name='intensity', offset=12, datatype=PointField.FLOAT32, count=1), + ] + pc2_msg = point_cloud2.create_cloud(header, fields, points) + return pc2_msg + +def main(args=None): + rclpy.init(args=args) + node = DatabasePublisherNode() + try: + node.publish_loop() + except KeyboardInterrupt: + pass + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/open_place_recognition/src/localization_node.py b/open_place_recognition/src/localization_node.py new file mode 100755 index 0000000..58a5a87 --- /dev/null +++ b/open_place_recognition/src/localization_node.py @@ -0,0 +1,599 @@ +#!/usr/bin/env python3 +""" +Localization Node for image and lidar-based localization. +This node subscribes to front/back camera images, semantic masks, and lidar pointclouds. +It then synchronizes the messages, preprocesses the data, and runs a localization pipeline. +""" + +import os +import cv2 +import numpy as np +import torch +import rclpy +from cv_bridge import CvBridge +from hydra.utils import instantiate +from message_filters import ApproximateTimeSynchronizer, Subscriber +from omegaconf import OmegaConf +from rclpy.node import Node +from rcl_interfaces.msg import ParameterDescriptor +from builtin_interfaces.msg import Time +from geometry_msgs.msg import PoseStamped +from sensor_msgs.msg import Image, CompressedImage, PointCloud2 +from sensor_msgs_py.point_cloud2 import read_points +from std_msgs.msg import Int32 +from torch import Tensor +import albumentations as A +from albumentations.pytorch import ToTensorV2 +from ament_index_python.packages import get_package_share_directory + +# Import QoS classes for separate QoS handling +from rclpy.qos import QoSProfile, QoSReliabilityPolicy + +# Importing required modules from our package +from opr.pipelines.localization import ArucoLocalizationPipeline +from opr_interfaces.msg import DatabaseMatchIndex +from opr.datasets.augmentations import DefaultImageTransform, DefaultSemanticTransform +from opr.datasets.projection import Projector +from opr.datasets.soc_utils import ( + get_points_labels_by_mask, + instance_masks_to_objects, + pack_objects, + semantic_mask_to_instances, +) + + +class ImageTransformWithoutNormalize: + """Custom image transformation without normalization. + + Converts an image in cv2 format to a PyTorch tensor (channel-first) using albumentations. + """ + def __call__(self, img: np.ndarray) -> Tensor: + augmented = A.Compose([ToTensorV2()])(image=img) + return augmented["image"] + + +class LocalizationNode(Node): + """ROS2 node for localization based on synchronized image and lidar data.""" + + def __init__(self): + super().__init__("localization") + + # Declare required parameters directly in __init__ + self.declare_parameter("qos_front_camera", 2, ParameterDescriptor(description="QoS for front camera")) + self.declare_parameter("qos_back_camera", 2, ParameterDescriptor(description="QoS for back camera")) + self.declare_parameter("qos_lidar", 2, ParameterDescriptor(description="QoS for lidar")) + self.declare_parameter("qos_global_ref", 2, ParameterDescriptor(description="QoS for global reference subscription")) + # Declare other parameters + self.declare_parameter("image_front_topic", "", ParameterDescriptor(description="Front camera image topic.")) + self.declare_parameter("image_back_topic", "", ParameterDescriptor(description="Back camera image topic.")) + self.declare_parameter("mask_front_topic", "", ParameterDescriptor(description="Front semantic segmentation mask topic.")) + self.declare_parameter("mask_back_topic", "", ParameterDescriptor(description="Back semantic segmentation mask topic.")) + self.declare_parameter("lidar_topic", "", ParameterDescriptor(description="Lidar pointcloud topic.")) + self.declare_parameter("dataset_dir", "", ParameterDescriptor(description="dataset directory.")) + self.declare_parameter("pipeline_cfg", "", ParameterDescriptor(description="Path to the pipeline configuration file.")) + self.declare_parameter("exclude_dynamic_classes", False, ParameterDescriptor(description="Exclude dynamic objects from the input data.")) + self.declare_parameter("image_resize", rclpy.Parameter.Type.INTEGER_ARRAY, ParameterDescriptor(description="Image resize dimensions.")) + + # New parameters for enabling/disabling sensors and global reference. + self.declare_parameter("enable_front_camera", True, ParameterDescriptor(description="Enable front camera.")) + self.declare_parameter("enable_back_camera", True, ParameterDescriptor(description="Enable back camera.")) + self.declare_parameter("enable_lidar", True, ParameterDescriptor(description="Enable lidar sensor.")) + self.declare_parameter("enable_global_ref", True, ParameterDescriptor(description="Enable global reference system subscription.")) + self.declare_parameter("global_ref_topic", "", ParameterDescriptor(description="Global reference topic (e.g. GPS/Barometer, WGS84).")) + + # Retrieve QoS parameter values + self.qos_front_camera_value = self.get_parameter("qos_front_camera").get_parameter_value().integer_value + self.qos_back_camera_value = self.get_parameter("qos_back_camera").get_parameter_value().integer_value + self.qos_lidar_value = self.get_parameter("qos_lidar").get_parameter_value().integer_value + self.qos_global_ref_value = self.get_parameter("qos_global_ref").get_parameter_value().integer_value + + # Retrieve other parameters + self.image_front_topic = self.get_parameter("image_front_topic").get_parameter_value().string_value + self.image_back_topic = self.get_parameter("image_back_topic").get_parameter_value().string_value + self.mask_front_topic = self.get_parameter("mask_front_topic").get_parameter_value().string_value + self.mask_back_topic = self.get_parameter("mask_back_topic").get_parameter_value().string_value + self.lidar_topic = self.get_parameter("lidar_topic").get_parameter_value().string_value + self.dataset_dir = self.get_parameter("dataset_dir").get_parameter_value().string_value + self.pipeline_cfg = self.get_parameter("pipeline_cfg").get_parameter_value().string_value + self.exclude_dynamic_classes = self.get_parameter("exclude_dynamic_classes").get_parameter_value().bool_value + self.image_resize = self.get_parameter("image_resize").get_parameter_value().integer_array_value + + self.enable_front_camera = self.get_parameter("enable_front_camera").get_parameter_value().bool_value + self.enable_back_camera = self.get_parameter("enable_back_camera").get_parameter_value().bool_value + self.enable_lidar = self.get_parameter("enable_lidar").get_parameter_value().bool_value + self.enable_global_ref = self.get_parameter("enable_global_ref").get_parameter_value().bool_value + self.global_ref_topic = self.get_parameter("global_ref_topic").get_parameter_value().string_value + + # Initialize cv_bridge for converting ROS image messages to OpenCV format. + self.cv_bridge = CvBridge() + + # Create QoS profiles + self.qos_front_cam_profile = self._create_qos_profile(self.qos_front_camera_value) + self.qos_back_cam_profile = self._create_qos_profile(self.qos_back_camera_value) + self.qos_lidar_profile = self._create_qos_profile(self.qos_lidar_value) + self.qos_global_ref_profile = self._create_qos_profile(self.qos_global_ref_value, depth=10) + + # Build a list of subscribers conditionally based on enabled sensors, + # each with its own QoS profile. + subscribers = [] + mapping = {} # Will map sensor name to its index in the synchronizer arguments. + + if self.enable_front_camera: + # Subscriber for front camera image + self.image_front_sub = Subscriber( + self, + CompressedImage, + self.image_front_topic, + qos_profile=self.qos_front_cam_profile + ) + subscribers.append(self.image_front_sub) + mapping['front_image'] = len(subscribers) - 1 + + # Subscriber for front camera mask + self.mask_front_sub = Subscriber( + self, + Image, + self.mask_front_topic, + qos_profile=self.qos_front_cam_profile + ) + subscribers.append(self.mask_front_sub) + mapping['front_mask'] = len(subscribers) - 1 + else: + self.image_front_sub = None + self.mask_front_sub = None + + if self.enable_back_camera: + # Subscriber for back camera image + self.image_back_sub = Subscriber( + self, + CompressedImage, + self.image_back_topic, + qos_profile=self.qos_back_cam_profile + ) + subscribers.append(self.image_back_sub) + mapping['back_image'] = len(subscribers) - 1 + + # Subscriber for back camera mask + self.mask_back_sub = Subscriber( + self, + Image, + self.mask_back_topic, + qos_profile=self.qos_back_cam_profile + ) + subscribers.append(self.mask_back_sub) + mapping['back_mask'] = len(subscribers) - 1 + else: + self.image_back_sub = None + self.mask_back_sub = None + + if self.enable_lidar: + # Subscriber for LiDAR + self.lidar_sub = Subscriber( + self, + PointCloud2, + self.lidar_topic, + qos_profile=self.qos_lidar_profile + ) + subscribers.append(self.lidar_sub) + mapping['lidar'] = len(subscribers) - 1 + else: + self.lidar_sub = None + + # Save mapping for use in the callback. + self.subscriber_mapping = mapping + + # Create synchronizer only if at least one sensor is enabled. + if subscribers: + self.ts = ApproximateTimeSynchronizer( + subscribers, + queue_size=1, + slop=0.05, + ) + self.ts.registerCallback(self.listener_callback) + else: + self.get_logger().error("No sensors enabled; cannot create synchronizer.") + + # Create publishers for pose and database match index (use default QoS or 10). + self.db_match_pose_pub = self.create_publisher(PoseStamped, "/place_recognition/pose", 10) + self.idx_pub = self.create_publisher(DatabaseMatchIndex, "/place_recognition/db_idx", 10) + self.estimated_pose_pub = self.create_publisher(PoseStamped, "/localization/pose", 10) + + # If enabled, create a subscriber for the global reference system with its QoS. + if self.enable_global_ref: + from sensor_msgs.msg import NavSatFix + self.global_ref_sub = self.create_subscription( + NavSatFix, + self.global_ref_topic, + self.global_ref_callback, + self.qos_global_ref_profile + ) + else: + self.global_ref_sub = None + self.global_ref = None + + # Instantiate the localization pipeline from configuration. + if not os.path.exists(self.pipeline_cfg): + exit(1) + cfg = OmegaConf.load(self.pipeline_cfg) + + if not os.path.exists(self.dataset_dir): + exit(1) + + # Check out the open_place_recognition/configs/pipelines/localization_pipeline.yaml + print("This node is not fully developed") + exit(1) + model_weights_path = os.path.join(os.path.expanduser("~"), "OpenPlaceRecognition", cfg.model_weights_path) + if not os.path.exists(self.dataset_dir): + exit(1) + self.get_logger().error(f"dataset_dir does not exist: {self.dataset_dir}") + self.get_logger().error(f"model_weights_path does not exist: {model_weights_path}") + cfg.database_dir = self.dataset_dir + cfg.model_weights_path = model_weights_path + # ---------------------------------- + + self.pipeline = instantiate(cfg) + + # Check if the pipeline uses a SOC (scene object context) module. + if self.pipeline.pr_pipe.model.soc_module is not None: + self.load_soc = True + self.get_logger().info("self.load_soc is set to True.") + sensors_cfg = OmegaConf.load(os.path.join(get_package_share_directory("open_place_recognition"), "configs/sensors/husky.yaml")) + anno_cfg = OmegaConf.load(os.path.join(get_package_share_directory("open_place_recognition"), "configs/anno/oneformer.yaml")) + self.front_cam_proj = Projector(sensors_cfg.front_cam, sensors_cfg.lidar) + self.back_cam_proj = Projector(sensors_cfg.back_cam, sensors_cfg.lidar) + self.max_distance_soc = 50.0 + self.top_k_soc = self.pipeline.pr_pipe.model.soc_module.num_objects + self.special_classes = anno_cfg.special_classes + self.soc_coords_type = "euclidean" + else: + self.load_soc = False + + # Set up image and mask transformations based on the pipeline type. + if isinstance(self.pipeline, ArucoLocalizationPipeline): + self.image_transform = ImageTransformWithoutNormalize() + self.mask_transform = DefaultSemanticTransform(train=False, resize=None) + else: + self.image_transform = DefaultImageTransform(train=False, resize=self.image_resize) + self.mask_transform = DefaultSemanticTransform(train=False, resize=self.image_resize) + + # Dynamic class index to be excluded. + self._ade20k_dynamic_idx = [12] + + # Transformation matrices for lidar to camera conversions. + self.lidar2front = np.array([ + [ 0.01509615, -0.99976457, -0.01558544, 0.04632156], + [ 0.00871086, 0.01571812, -0.99983852, -0.13278588], + [ 0.9998481, 0.01495794, 0.0089461, -0.06092749], + [ 0., 0., 0., 1. ] + ]) + self.lidar2back = np.array([ + [-1.50409674e-02, 9.99886421e-01, 9.55906151e-04, 1.82703304e-02], + [-1.30440106e-02, 7.59716299e-04, -9.99914635e-01, -1.41787545e-01], + [-9.99801792e-01, -1.50521522e-02, 1.30311022e-02, -6.72336358e-02], + [ 0., 0., 0., 1. ] + ]) + self.front_matrix = np.array([ + [683.6199340820312, 0.0, 615.1160278320312], + [0.0, 683.6199340820312, 345.32354736328125], + [0.0, 0.0, 1.0] + ]) + self.front_dist = np.array([0.0, 0.0, 0.0, 0.0, 0.0]) + self.back_matrix = np.array([ + [910.4178466796875, 0.0, 648.44140625], + [0.0, 910.4166870117188, 354.0118408203125], + [0.0, 0.0, 1.0] + ]) + self.back_dist = np.array([0.0, 0.0, 0.0, 0.0, 0.0]) + + self.get_logger().info(f"Initialized {self.__class__.__name__} node.") + + def _create_qos_profile(self, qos_value: int, depth=1) -> QoSProfile: + """ + Create a QoSProfile based on integer (0=SystemDefault,1=BestEffort,2=Reliable). + """ + qos_profile = QoSProfile(depth=depth) + if qos_value == 0: + qos_profile.reliability = QoSReliabilityPolicy.SYSTEM_DEFAULT + elif qos_value == 1: + qos_profile.reliability = QoSReliabilityPolicy.BEST_EFFORT + else: + qos_profile.reliability = QoSReliabilityPolicy.RELIABLE + return qos_profile + + def global_ref_callback(self, msg) -> None: + """Callback to update the global reference message.""" + self.global_ref = msg + self.get_logger().debug(f"Received global reference message: {msg}") + + def _prepare_input( + self, + images: np.ndarray | list[np.ndarray] = None, + masks: np.ndarray | list[np.ndarray] = None, + pointcloud: np.ndarray = None, + ) -> dict[str, Tensor]: + """ + Prepare and transform input data for the pipeline. + + Args: + images: A single image or list of images (cv2 format). + masks: A single mask or list of masks. + pointcloud: Lidar pointcloud data. + + Returns: + A dictionary with transformed images, masks, and pointcloud tensors. + """ + input_data: dict[str, Tensor] = {} + + # Process image inputs using the configured image transform. + if images is not None and isinstance(images, list): + for i, image in enumerate(images): + if image is None: + self.get_logger().info(f"Image {i} is disabled or not available.") + continue + img_processed = image.copy() + if self.exclude_dynamic_classes and masks is not None and i < len(masks) and masks[i] is not None: + for dyn_idx in self._ade20k_dynamic_idx: + img_processed = np.where(masks[i][:, :, np.newaxis] == dyn_idx, 0, img_processed) + input_data[f"image_{i}"] = self.image_transform(img_processed) + elif isinstance(images, np.ndarray): + img_processed = images.copy() + if self.exclude_dynamic_classes and masks is not None: + for dyn_idx in self._ade20k_dynamic_idx: + img_processed = np.where(masks == dyn_idx, 0, img_processed) + input_data["image_0"] = self.image_transform(img_processed) + else: + self.get_logger().warning(f"Invalid type for images in '_prepare_input': {type(images)}") + + # Process mask inputs using the configured mask transform. + if masks is not None and isinstance(masks, list): + for i, mask in enumerate(masks): + if mask is None: + self.get_logger().info(f"Mask {i} is disabled or not available.") + continue + input_data[f"mask_{i}"] = self.mask_transform(mask) + elif isinstance(masks, np.ndarray): + input_data["mask_0"] = self.mask_transform(masks) + else: + self.get_logger().warning(f"Invalid type for masks in '_prepare_input': {type(masks)}") + + # Process pointcloud data. + if pointcloud is not None: + if self.exclude_dynamic_classes and masks is not None: + if isinstance(masks, list): + for mask in masks: + pointcloud = self._remove_dynamic_points(pointcloud, mask, + self.lidar2back, self.back_matrix, self.back_dist) + elif isinstance(masks, np.ndarray): + pointcloud = self._remove_dynamic_points(pointcloud, masks, + self.lidar2back, self.back_matrix, self.back_dist) + pc_tensor = torch.tensor(pointcloud).contiguous() + pc_tensor = pc_tensor[~torch.any(pc_tensor.isnan(), dim=1)] + input_data["pointcloud_lidar_coords"] = pc_tensor[:, :3] + if pc_tensor.shape[1] > 3: + input_data["pointcloud_lidar_feats"] = pc_tensor[:, 3].unsqueeze(1) + else: + input_data["pointcloud_lidar_feats"] = torch.ones_like(pc_tensor[:, :1]) + + # Process SOC (scene object context) if enabled and if masks are available. + if self.load_soc and masks is not None and isinstance(masks, list) and len(masks) >= 2: + input_data["soc"] = self._get_soc(mask_front=masks[0], mask_back=masks[1], lidar_scan=pointcloud) + + return input_data + + def _get_soc(self, mask_front: np.ndarray, mask_back: np.ndarray, lidar_scan: np.ndarray) -> Tensor: + """ + Compute the scene object context (SOC) tensor based on the provided masks and lidar scan. + + Args: + mask_front: Front camera semantic mask. + mask_back: Back camera semantic mask. + lidar_scan: Lidar pointcloud data. + + Returns: + A PyTorch tensor representing packed objects. + """ + coords_front, _, in_image_front = self.front_cam_proj(lidar_scan) + coords_back, _, in_image_back = self.back_cam_proj(lidar_scan) + point_labels = np.zeros(len(lidar_scan), dtype=np.uint8) + point_labels[in_image_front] = get_points_labels_by_mask(coords_front, mask_front) + point_labels[in_image_back] = get_points_labels_by_mask(coords_back, mask_back) + instances_front = semantic_mask_to_instances(mask_front, area_threshold=10, labels_whitelist=self.special_classes) + instances_back = semantic_mask_to_instances(mask_back, area_threshold=10, labels_whitelist=self.special_classes) + objects_front = instance_masks_to_objects(instances_front, coords_front, + point_labels[in_image_front], lidar_scan[in_image_front]) + objects_back = instance_masks_to_objects(instances_back, coords_back, + point_labels[in_image_back], lidar_scan[in_image_back]) + objects = {**objects_front, **objects_back} + packed_objects = pack_objects(objects, self.top_k_soc, self.max_distance_soc, self.special_classes) + + if self.soc_coords_type == "cylindrical_3d": + packed_objects = np.concatenate(( + np.linalg.norm(packed_objects, axis=-1, keepdims=True), + np.arctan2(packed_objects[..., 1], packed_objects[..., 0])[..., None], + packed_objects[..., 2:], + ), axis=-1) + elif self.soc_coords_type == "cylindrical_2d": + packed_objects = np.concatenate(( + np.linalg.norm(packed_objects[..., :2], axis=-1, keepdims=True), + np.arctan2(packed_objects[..., 1], packed_objects[..., 0])[..., None], + packed_objects[..., 2:], + ), axis=-1) + elif self.soc_coords_type == "euclidean": + pass + elif self.soc_coords_type == "spherical": + packed_objects = np.concatenate(( + np.linalg.norm(packed_objects, axis=-1, keepdims=True), + np.arccos(packed_objects[..., 2] / np.linalg.norm(packed_objects, axis=-1, keepdims=True)), + np.arctan2(packed_objects[..., 1], packed_objects[..., 0])[..., None], + ), axis=-1) + else: + raise ValueError(f"Unknown soc_coords_type: {self.soc_coords_type!r}") + objects_tensor = torch.from_numpy(packed_objects).float() + return objects_tensor + + def _remove_dynamic_points(self, pointcloud: np.ndarray, semantic_map: np.ndarray, + lidar2sensor: np.ndarray, sensor_intrinsics: np.ndarray, + sensor_dist: np.ndarray) -> np.ndarray: + """ + Remove points corresponding to dynamic objects from the pointcloud. + + Args: + pointcloud: The original pointcloud data. + semantic_map: The semantic segmentation mask. + lidar2sensor: Transformation matrix from lidar to sensor frame. + sensor_intrinsics: Camera intrinsic matrix. + sensor_dist: Camera distortion coefficients. + + Returns: + The filtered pointcloud with dynamic points removed. + """ + pc_values = np.concatenate([pointcloud, np.ones((pointcloud.shape[0], 1))], axis=1).T + camera_values = lidar2sensor @ pc_values + camera_values = camera_values.T[:, :3] + points_2d, _ = cv2.projectPoints( + camera_values, + np.zeros((3, 1), np.float32), + np.zeros((3, 1), np.float32), + sensor_intrinsics, + sensor_dist + ) + points_2d = points_2d[:, 0, :] + classes_in_map = set(np.unique(semantic_map)) + dynamic_classes = set(self._ade20k_dynamic_idx) + if classes_in_map.intersection(dynamic_classes): + valid = (~np.isnan(points_2d[:, 0])) & (~np.isnan(points_2d[:, 1])) + in_bounds_x = (points_2d[:, 0] >= 0) & (points_2d[:, 0] < 1280) + in_bounds_y = (points_2d[:, 1] >= 0) & (points_2d[:, 1] < 720) + look_forward = (camera_values[:, 2] > 0) + overall_mask = valid & in_bounds_x & in_bounds_y & look_forward + indices = np.where(overall_mask)[0] + mask_for_points = np.full((points_2d.shape[0], 3), True) + dynamic_idx_array = np.array(self._ade20k_dynamic_idx) + semantic_values = semantic_map[ + np.floor(points_2d[indices, 1]).astype(int), + np.floor(points_2d[indices, 0]).astype(int) + ] + matching_indices = np.where(np.isin(semantic_values, dynamic_idx_array)) + mask_for_points[indices[matching_indices[0]]] = np.array([False, False, False]) + return pointcloud[mask_for_points].reshape((-1, 3)) + else: + return pointcloud + + def _create_pose_msg(self, pose: np.ndarray, timestamp: Time) -> PoseStamped: + """ + Create a PoseStamped message from a pose array. + + Args: + pose: A 7-element array [x, y, z, qx, qy, qz, qw]. + timestamp: The message timestamp. + + Returns: + A populated PoseStamped message. + """ + pose_msg = PoseStamped() + pose_msg.header.stamp = timestamp + pose_msg.pose.position.x = pose[0] + pose_msg.pose.position.y = pose[1] + pose_msg.pose.position.z = pose[2] + pose_msg.pose.orientation.x = pose[3] + pose_msg.pose.orientation.y = pose[4] + pose_msg.pose.orientation.z = pose[5] + pose_msg.pose.orientation.w = pose[6] + return pose_msg + + def _create_idx_msg(self, idx: int, timestamp: Time) -> DatabaseMatchIndex: + """ + Create a DatabaseMatchIndex message. + + Args: + idx: The index value. + timestamp: The message timestamp. + + Returns: + A populated DatabaseMatchIndex message. + """ + idx_msg = DatabaseMatchIndex() + idx_msg.header.stamp = timestamp + idx_msg.index = idx + return idx_msg + + def listener_callback(self, *msgs) -> None: + """ + Callback function invoked when synchronized messages are received. + + Converts messages to OpenCV images and numpy arrays, prepares input data, + runs inference through the pipeline, and publishes the results. + """ + self.get_logger().info("Received synchronized messages.") + t_start = self.get_clock().now() + + # Extract messages from the synchronizer based on our mapping. + mapping = self.subscriber_mapping + front_image_msg = msgs[mapping['front_image']] if 'front_image' in mapping else None + front_mask_msg = msgs[mapping['front_mask']] if 'front_mask' in mapping else None + back_image_msg = msgs[mapping['back_image']] if 'back_image' in mapping else None + back_mask_msg = msgs[mapping['back_mask']] if 'back_mask' in mapping else None + lidar_msg = msgs[mapping['lidar']] if 'lidar' in mapping else None + + # Convert image and mask messages to OpenCV format if available. + front_image = self.cv_bridge.compressed_imgmsg_to_cv2(front_image_msg) if front_image_msg is not None else None + back_image = self.cv_bridge.compressed_imgmsg_to_cv2(back_image_msg) if back_image_msg is not None else None + front_mask = self.cv_bridge.imgmsg_to_cv2(front_mask_msg) if front_mask_msg is not None else None + back_mask = self.cv_bridge.imgmsg_to_cv2(back_mask_msg) if back_mask_msg is not None else None + + # Convert lidar message to a numpy pointcloud if available. + if lidar_msg is not None: + points = read_points(lidar_msg, field_names=("x", "y", "z")) + pointcloud = np.array([points["x"], points["y"], points["z"]]).T + else: + pointcloud = None + + # Prepare input data for the pipeline. + input_data = self._prepare_input( + images=[front_image, back_image], + masks=[front_mask, back_mask], + pointcloud=pointcloud + ) + + # Run inference. + output = self.pipeline.infer(input_data) + t_taken = self.get_clock().now() - t_start + self.get_logger().debug(f"Localization inference took: {t_taken.nanoseconds / 1e6} ms.") + self.get_logger().info(f"output['db_match_pose'] = {output['db_match_pose']}") + + # Create and publish messages. + db_match_pose = output["db_match_pose"].tolist() + estimated_pose = output["estimated_pose"] + db_idx = int(output["db_idx"]) + + db_match_pose_msg = self._create_pose_msg( + db_match_pose, + lidar_msg.header.stamp if lidar_msg is not None else self.get_clock().now().to_msg() + ) + estimated_pose_msg = self._create_pose_msg( + estimated_pose, + lidar_msg.header.stamp if lidar_msg is not None else self.get_clock().now().to_msg() + ) + idx_msg = self._create_idx_msg( + db_idx, + lidar_msg.header.stamp if lidar_msg is not None else self.get_clock().now().to_msg() + ) + + self.db_match_pose_pub.publish(db_match_pose_msg) + self.get_logger().info(f"Published db match pose message: {db_match_pose_msg.pose}") + self.estimated_pose_pub.publish(estimated_pose_msg) + self.get_logger().info(f"Published estimated pose message: {estimated_pose_msg.pose}") + self.idx_pub.publish(idx_msg) + self.get_logger().info(f"Published database index message: {idx_msg.index}") + +def main(args=None): + """Main function to initialize and spin the localization node.""" + rclpy.init(args=args) + loc_node = LocalizationNode() + rclpy.spin(loc_node) + loc_node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/open_place_recognition/src/place_recognition_node.py b/open_place_recognition/src/place_recognition_node.py new file mode 100755 index 0000000..a2172f9 --- /dev/null +++ b/open_place_recognition/src/place_recognition_node.py @@ -0,0 +1,457 @@ +#!/usr/bin/env python3 +""" +Place Recognition Node + +This node subscribes to front/back camera images, semantic masks, and lidar pointclouds. +It synchronizes these messages, prepares the input for the place recognition pipeline, +and publishes the resulting pose and database match index. +""" + +import os +import numpy as np +import torch +import rclpy +from cv_bridge import CvBridge +from hydra.utils import instantiate +from message_filters import ApproximateTimeSynchronizer, Subscriber +from omegaconf import OmegaConf +from rclpy.node import Node +from rcl_interfaces.msg import ParameterDescriptor +from builtin_interfaces.msg import Time +from geometry_msgs.msg import PoseStamped +from sensor_msgs.msg import Image, CompressedImage, PointCloud2, NavSatFix +from sensor_msgs_py.point_cloud2 import read_points +from std_msgs.msg import Int32 +from torch import Tensor +from ament_index_python.packages import get_package_share_directory + +# QoS imports +from rclpy.qos import QoSProfile, QoSReliabilityPolicy + +# Import custom message and processing functions +from opr_interfaces.msg import DatabaseMatchIndex +from opr.datasets.augmentations import DefaultImageTransform, DefaultSemanticTransform +from opr.datasets.projection import Projector +from opr.datasets.soc_utils import ( + get_points_labels_by_mask, + instance_masks_to_objects, + pack_objects, + semantic_mask_to_instances, +) +from opr.pipelines.place_recognition import PlaceRecognitionPipeline + +class PlaceRecognitionNode(Node): + """ROS2 Node for Place Recognition using camera images, semantic masks, and lidar data.""" + + def __init__(self): + super().__init__("place_recognition") + + # ---------------------------------------------------------------------- + # New QoS Parameters (separate for front camera, back camera, lidar, global ref): + # ---------------------------------------------------------------------- + self.declare_parameter("qos_front_camera", 2, ParameterDescriptor(description="QoS for front camera (0=SystemDefault,1=BestEffort,2=Reliable)")) + self.declare_parameter("qos_back_camera", 2, ParameterDescriptor(description="QoS for back camera (0=SystemDefault,1=BestEffort,2=Reliable)")) + self.declare_parameter("qos_lidar", 2, ParameterDescriptor(description="QoS for lidar (0=SystemDefault,1=BestEffort,2=Reliable)")) + self.declare_parameter("qos_global_ref", 2, ParameterDescriptor(description="QoS for global ref subscription (0=SystemDefault,1=BestEffort,2=Reliable)")) + + # ---------------------------------------------------------------------- + # Declare required parameters directly in __init__ (original code): + # ---------------------------------------------------------------------- + self.declare_parameter("image_front_topic", "", ParameterDescriptor(description="Front camera image topic.")) + self.declare_parameter("image_back_topic", "", ParameterDescriptor(description="Back camera image topic.")) + self.declare_parameter("mask_front_topic", "", ParameterDescriptor(description="Front semantic segmentation mask topic.")) + self.declare_parameter("mask_back_topic", "", ParameterDescriptor(description="Back semantic segmentation mask topic.")) + self.declare_parameter("lidar_topic", "", ParameterDescriptor(description="Lidar pointcloud topic.")) + self.declare_parameter("dataset_dir", "", ParameterDescriptor(description="dataset directory.")) + self.declare_parameter("pipeline_cfg", "", ParameterDescriptor(description="Path to the pipeline configuration file.")) + self.declare_parameter("image_resize", rclpy.Parameter.Type.INTEGER_ARRAY, ParameterDescriptor(description="Image resize dimensions.")) + self.declare_parameter("enable_front_camera", True, ParameterDescriptor(description="Enable front camera.")) + self.declare_parameter("enable_back_camera", True, ParameterDescriptor(description="Enable back camera.")) + self.declare_parameter("enable_lidar", True, ParameterDescriptor(description="Enable lidar sensor.")) + self.declare_parameter("global_ref_topic", "", ParameterDescriptor(description="Global reference system topic (e.g. GPS/Barometer, WGS84).")) + self.declare_parameter("enable_global_ref", True, ParameterDescriptor(description="Enable global reference subscription.")) + self.declare_parameter("reserve", False, ParameterDescriptor(description="Reserve variable for future use.")) + + # ---------------------------------------------------------------------- + # Retrieve QoS parameter values + # ---------------------------------------------------------------------- + self.qos_front_cam_val = self.get_parameter("qos_front_camera").get_parameter_value().integer_value + self.qos_back_cam_val = self.get_parameter("qos_back_camera").get_parameter_value().integer_value + self.qos_lidar_val = self.get_parameter("qos_lidar").get_parameter_value().integer_value + self.qos_global_ref_val= self.get_parameter("qos_global_ref").get_parameter_value().integer_value + + # Create QoS profiles using the helper method below + self.qos_front_cam_profile = self._create_qos_profile(self.qos_front_cam_val) + self.qos_back_cam_profile = self._create_qos_profile(self.qos_back_cam_val) + self.qos_lidar_profile = self._create_qos_profile(self.qos_lidar_val) + self.qos_global_ref_profile = self._create_qos_profile(self.qos_global_ref_val, depth=10) + + # ---------------------------------------------------------------------- + # Retrieve other parameters (original code) + # ---------------------------------------------------------------------- + image_front_topic = self.get_parameter("image_front_topic").get_parameter_value().string_value + image_back_topic = self.get_parameter("image_back_topic").get_parameter_value().string_value + mask_front_topic = self.get_parameter("mask_front_topic").get_parameter_value().string_value + mask_back_topic = self.get_parameter("mask_back_topic").get_parameter_value().string_value + lidar_topic = self.get_parameter("lidar_topic").get_parameter_value().string_value + dataset_dir = self.get_parameter("dataset_dir").get_parameter_value().string_value + pipeline_cfg = self.get_parameter("pipeline_cfg").get_parameter_value().string_value + image_resize = self.get_parameter("image_resize").get_parameter_value().integer_array_value + + self.enable_front_camera = self.get_parameter("enable_front_camera").get_parameter_value().bool_value + self.enable_back_camera = self.get_parameter("enable_back_camera").get_parameter_value().bool_value + self.enable_lidar = self.get_parameter("enable_lidar").get_parameter_value().bool_value + self.enable_global_ref = self.get_parameter("enable_global_ref").get_parameter_value().bool_value + self.global_ref_topic = self.get_parameter("global_ref_topic").get_parameter_value().string_value + self.reserve = self.get_parameter("reserve").get_parameter_value().bool_value + + # Initialize CvBridge for image conversions. + self.cv_bridge = CvBridge() + + # ---------------------------------------------------------------------- + # Create subscribers with their respective QoS profiles + # ---------------------------------------------------------------------- + subscribers = [] + mapping = {} + + if self.enable_front_camera: + self.image_front_sub = Subscriber( + self, + CompressedImage, + image_front_topic, + qos_profile=self.qos_front_cam_profile + ) + subscribers.append(self.image_front_sub) + mapping["front_image"] = len(subscribers) - 1 + + self.mask_front_sub = Subscriber( + self, + Image, + mask_front_topic, + qos_profile=self.qos_front_cam_profile + ) + subscribers.append(self.mask_front_sub) + mapping["front_mask"] = len(subscribers) - 1 + else: + self.image_front_sub = None + self.mask_front_sub = None + + if self.enable_back_camera: + self.image_back_sub = Subscriber( + self, + CompressedImage, + image_back_topic, + qos_profile=self.qos_back_cam_profile + ) + subscribers.append(self.image_back_sub) + mapping["back_image"] = len(subscribers) - 1 + + self.mask_back_sub = Subscriber( + self, + Image, + mask_back_topic, + qos_profile=self.qos_back_cam_profile + ) + subscribers.append(self.mask_back_sub) + mapping["back_mask"] = len(subscribers) - 1 + else: + self.image_back_sub = None + self.mask_back_sub = None + + if self.enable_lidar: + self.lidar_sub = Subscriber( + self, + PointCloud2, + lidar_topic, + qos_profile=self.qos_lidar_profile + ) + subscribers.append(self.lidar_sub) + mapping["lidar"] = len(subscribers) - 1 + else: + self.lidar_sub = None + + # Save the mapping for use in the callback. + self.subscriber_mapping = mapping + + # Create the synchronizer if any subscribers exist. + if subscribers: + self.ts = ApproximateTimeSynchronizer(subscribers, queue_size=1, slop=1.05) + self.ts.registerCallback(self.listener_callback) + else: + self.get_logger().error("No sensor subscribers created; check sensor enable parameters.") + + # Create publishers for pose and database match index. + self.pose_pub = self.create_publisher(PoseStamped, "/place_recognition/pose", 10) + self.idx_pub = self.create_publisher(DatabaseMatchIndex, "/place_recognition/db_idx", 10) + + # ---------------------------------------------------------------------- + # Global reference subscription (with QoS) if enabled + # ---------------------------------------------------------------------- + if self.enable_global_ref: + self.global_ref_sub = self.create_subscription( + NavSatFix, + self.global_ref_topic, + self.global_ref_callback, + self.qos_global_ref_profile + ) + else: + self.global_ref_sub = None + self.global_ref = None + + # ---------------------------------------------------------------------- + # Instantiate the place recognition pipeline from configuration (original code) + # ---------------------------------------------------------------------- + cfg = OmegaConf.load(pipeline_cfg) + if not os.path.exists(dataset_dir): + self.get_logger().error(f"dataset_dir does not exist: {dataset_dir}") + exit(1) + model_weights_path = os.path.join(os.path.expanduser("~"), "OpenPlaceRecognition", cfg.model_weights_path) + if not os.path.exists(dataset_dir): + self.get_logger().error(f"model_weights_path does not exist: {model_weights_path}") + exit(1) + cfg.database_dir = dataset_dir + cfg.model_weights_path = model_weights_path + + self.pipeline = instantiate(cfg) + + # Check for Scene Object Context (SOC) module support. + if self.pipeline.model.soc_module is not None: + self.load_soc = True + self.get_logger().info("self.load_soc is set to True.") + sensors_cfg = OmegaConf.load(os.path.join(get_package_share_directory("open_place_recognition"), "configs/sensors/husky.yaml")) + anno_cfg = OmegaConf.load(os.path.join(get_package_share_directory("open_place_recognition"), "configs/anno/oneformer.yaml")) + self.front_cam_proj = Projector(sensors_cfg.front_cam, sensors_cfg.lidar) + self.back_cam_proj = Projector(sensors_cfg.back_cam, sensors_cfg.lidar) + self.max_distance_soc = 50.0 + self.top_k_soc = self.pipeline.model.soc_module.num_objects + self.special_classes = anno_cfg.special_classes + self.soc_coords_type = "euclidean" + else: + self.load_soc = False + + # Set up image and mask transformations. + self.image_transform = DefaultImageTransform(train=False, resize=image_resize) + self.mask_transform = DefaultSemanticTransform(train=False, resize=image_resize) + + self.get_logger().info(f"Initialized {self.__class__.__name__} node.") + + # -------------------------------------------------------------------------- + # Helper function to create QoSProfile from integer + # -------------------------------------------------------------------------- + def _create_qos_profile(self, qos_value, depth=1) -> QoSProfile: + """ + Create a QoSProfile based on integer (0=SystemDefault,1=BestEffort,2=Reliable). + """ + profile = QoSProfile(depth=depth) + if qos_value == 0: + profile.reliability = QoSReliabilityPolicy.SYSTEM_DEFAULT + elif qos_value == 1: + profile.reliability = QoSReliabilityPolicy.BEST_EFFORT + else: + profile.reliability = QoSReliabilityPolicy.RELIABLE + return profile + + def global_ref_callback(self, msg: NavSatFix) -> None: + """Callback to update the global reference system message.""" + self.global_ref = msg + self.get_logger().debug(f"Received global reference message: {msg}") + + def _prepare_input( + self, + images: np.ndarray | list[np.ndarray] = None, + masks: np.ndarray | list[np.ndarray] = None, + pointcloud: np.ndarray = None, + ) -> dict[str, Tensor]: + """ + Prepare and transform input data for the pipeline. + + Args: + images: Single image or list of images (in numpy format). + masks: Single mask or list of masks. + pointcloud: Lidar pointcloud data as a numpy array. + + Returns: + Dictionary with transformed images, masks, and pointcloud tensors. + """ + input_data: dict[str, Tensor] = {} + + if images is not None: + if isinstance(images, list): + for i, image in enumerate(images): + if image is not None: + input_data[f"image_{i}"] = self.image_transform(image) + else: + self.get_logger().info(f"Image {i} is disabled or not available.") + elif isinstance(images, np.ndarray): + input_data["image_0"] = self.image_transform(images) + else: + self.get_logger().warning(f"Invalid type for images in '_prepare_input': {type(images)}") + + if masks is not None: + if isinstance(masks, list): + for i, mask in enumerate(masks): + if mask is not None: + input_data[f"mask_{i}"] = self.mask_transform(mask) + else: + self.get_logger().info(f"Mask {i} is disabled or not available.") + elif isinstance(masks, np.ndarray): + input_data["mask_0"] = self.mask_transform(masks) + else: + self.get_logger().warning(f"Invalid type for masks in '_prepare_input': {type(masks)}") + + if pointcloud is not None: + pointcloud_tensor = torch.tensor(pointcloud).contiguous() + input_data["pointcloud_lidar_coords"] = pointcloud_tensor[:, :3] + if pointcloud_tensor.shape[1] > 3: + input_data["pointcloud_lidar_feats"] = pointcloud_tensor[:, 3].unsqueeze(1) + else: + input_data["pointcloud_lidar_feats"] = torch.ones_like(pointcloud_tensor[:, :1]) + + if self.load_soc and masks is not None and isinstance(masks, list) and len(masks) >= 2: + input_data["soc"] = self._get_soc(mask_front=masks[0], mask_back=masks[1], lidar_scan=pointcloud) + + return input_data + + def _get_soc(self, mask_front: np.ndarray, mask_back: np.ndarray, lidar_scan: np.ndarray) -> Tensor: + """ + Compute the Scene Object Context (SOC) tensor based on the provided masks and lidar scan. + + Args: + mask_front: Semantic mask from the front camera. + mask_back: Semantic mask from the back camera. + lidar_scan: Lidar pointcloud data. + + Returns: + A PyTorch tensor representing packed objects. + """ + coords_front, _, in_image_front = self.front_cam_proj(lidar_scan) + coords_back, _, in_image_back = self.back_cam_proj(lidar_scan) + point_labels = np.zeros(len(lidar_scan), dtype=np.uint8) + point_labels[in_image_front] = get_points_labels_by_mask(coords_front, mask_front) + point_labels[in_image_back] = get_points_labels_by_mask(coords_back, mask_back) + instances_front = semantic_mask_to_instances(mask_front, area_threshold=10, labels_whitelist=self.special_classes) + instances_back = semantic_mask_to_instances(mask_back, area_threshold=10, labels_whitelist=self.special_classes) + objects_front = instance_masks_to_objects(instances_front, coords_front, point_labels[in_image_front], lidar_scan[in_image_front]) + objects_back = instance_masks_to_objects(instances_back, coords_back, point_labels[in_image_back], lidar_scan[in_image_back]) + objects = {**objects_front, **objects_back} + packed_objects = pack_objects(objects, self.top_k_soc, self.max_distance_soc, self.special_classes) + + if self.soc_coords_type == "cylindrical_3d": + packed_objects = np.concatenate(( + np.linalg.norm(packed_objects, axis=-1, keepdims=True), + np.arctan2(packed_objects[..., 1], packed_objects[..., 0])[..., None], + packed_objects[..., 2:], + ), axis=-1) + elif self.soc_coords_type == "cylindrical_2d": + packed_objects = np.concatenate(( + np.linalg.norm(packed_objects[..., :2], axis=-1, keepdims=True), + np.arctan2(packed_objects[..., 1], packed_objects[..., 0])[..., None], + packed_objects[..., 2:], + ), axis=-1) + elif self.soc_coords_type == "euclidean": + pass + elif self.soc_coords_type == "spherical": + packed_objects = np.concatenate(( + np.linalg.norm(packed_objects, axis=-1, keepdims=True), + np.arccos(packed_objects[..., 2] / np.linalg.norm(packed_objects, axis=-1, keepdims=True)), + np.arctan2(packed_objects[..., 1], packed_objects[..., 0])[..., None], + ), axis=-1) + else: + raise ValueError(f"Unknown soc_coords_type: {self.soc_coords_type!r}") + objects_tensor = torch.from_numpy(packed_objects).float() + return objects_tensor + + def _create_pose_msg(self, pose: np.ndarray, timestamp: Time) -> PoseStamped: + """ + Create a PoseStamped message from a pose array. + + Args: + pose: A 7-element array [x, y, z, qx, qy, qz, qw]. + timestamp: The message timestamp. + + Returns: + A populated PoseStamped message. + """ + pose_msg = PoseStamped() + pose_msg.header.stamp = timestamp + pose_msg.pose.position.x = pose[0] + pose_msg.pose.position.y = pose[1] + pose_msg.pose.position.z = pose[2] + pose_msg.pose.orientation.x = pose[3] + pose_msg.pose.orientation.y = pose[4] + pose_msg.pose.orientation.z = pose[5] + pose_msg.pose.orientation.w = pose[6] + return pose_msg + + def _create_idx_msg(self, idx: int, timestamp: Time) -> DatabaseMatchIndex: + """ + Create a DatabaseMatchIndex message. + + Args: + idx: The index value. + timestamp: The message timestamp. + + Returns: + A populated DatabaseMatchIndex message with the provided index. + """ + idx_msg = DatabaseMatchIndex() + idx_msg.header.stamp = timestamp + idx_msg.index = idx + return idx_msg + + def listener_callback(self, *msgs) -> None: + """ + Callback for synchronized sensor messages. + + Converts incoming messages to OpenCV and numpy formats, prepares input data, + runs inference through the place recognition pipeline, and publishes the results. + """ + self.get_logger().info("Received synchronized messages.") + t_start = self.get_clock().now() + + mapping = self.subscriber_mapping + front_image_msg = msgs[mapping["front_image"]] if "front_image" in mapping else None + front_mask_msg = msgs[mapping["front_mask"]] if "front_mask" in mapping else None + back_image_msg = msgs[mapping["back_image"]] if "back_image" in mapping else None + back_mask_msg = msgs[mapping["back_mask"]] if "back_mask" in mapping else None + lidar_msg = msgs[mapping["lidar"]] if "lidar" in mapping else None + + front_image = self.cv_bridge.compressed_imgmsg_to_cv2(front_image_msg) if front_image_msg is not None else None + back_image = self.cv_bridge.compressed_imgmsg_to_cv2(back_image_msg) if back_image_msg is not None else None + front_mask = self.cv_bridge.imgmsg_to_cv2(front_mask_msg) if front_mask_msg is not None else None + back_mask = self.cv_bridge.imgmsg_to_cv2(back_mask_msg) if back_mask_msg is not None else None + + if lidar_msg is not None: + points = read_points(lidar_msg, field_names=("x", "y", "z")) + pointcloud = np.array([points["x"], points["y"], points["z"]]).T + else: + pointcloud = None + + input_data = self._prepare_input( + images=[front_image, back_image], + masks=[front_mask, back_mask], + pointcloud=pointcloud + ) + + output = self.pipeline.infer(input_data) + t_taken = self.get_clock().now() - t_start + self.get_logger().info(f"Place recognition inference took: {t_taken.nanoseconds / 1e6} ms.") + + timestamp = lidar_msg.header.stamp if lidar_msg is not None else self.get_clock().now().to_msg() + pose_msg = self._create_pose_msg(output["pose"], timestamp) + idx_msg = self._create_idx_msg(int(output["idx"]), timestamp) + self.pose_pub.publish(pose_msg) + self.get_logger().info(f"Published pose message: {pose_msg.pose}") + self.idx_pub.publish(idx_msg) + self.get_logger().info(f"Published database index message: {idx_msg.index}") + +def main(args=None): + """Main entry point for the Place Recognition node.""" + rclpy.init(args=args) + pr_node = PlaceRecognitionNode() + rclpy.spin(pr_node) + pr_node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/src/open_place_recognition/open_place_recognition/depth_estimation.py b/open_place_recognition/src/test_depth_estimation_node.py old mode 100644 new mode 100755 similarity index 99% rename from src/open_place_recognition/open_place_recognition/depth_estimation.py rename to open_place_recognition/src/test_depth_estimation_node.py index 7f87c34..0e0f99f --- a/src/open_place_recognition/open_place_recognition/depth_estimation.py +++ b/open_place_recognition/src/test_depth_estimation_node.py @@ -1,3 +1,4 @@ +#!/usr/bin/env python3 from rclpy.node import Node import numpy as np import rclpy diff --git a/src/open_place_recognition/open_place_recognition/visualizer.py b/open_place_recognition/src/test_visualizer_node.py old mode 100644 new mode 100755 similarity index 99% rename from src/open_place_recognition/open_place_recognition/visualizer.py rename to open_place_recognition/src/test_visualizer_node.py index d5f1b33..c485667 --- a/src/open_place_recognition/open_place_recognition/visualizer.py +++ b/open_place_recognition/src/test_visualizer_node.py @@ -1,3 +1,4 @@ +#!/usr/bin/env python3 import os from typing import Tuple diff --git a/src/opr_interfaces/CMakeLists.txt b/opr_interfaces/CMakeLists.txt similarity index 100% rename from src/opr_interfaces/CMakeLists.txt rename to opr_interfaces/CMakeLists.txt diff --git a/src/opr_interfaces/msg/DatabaseMatchIndex.msg b/opr_interfaces/msg/DatabaseMatchIndex.msg similarity index 100% rename from src/opr_interfaces/msg/DatabaseMatchIndex.msg rename to opr_interfaces/msg/DatabaseMatchIndex.msg diff --git a/src/opr_interfaces/package.xml b/opr_interfaces/package.xml similarity index 100% rename from src/opr_interfaces/package.xml rename to opr_interfaces/package.xml diff --git a/src/open_place_recognition/launch/depth_estimation.launch.py b/src/open_place_recognition/launch/depth_estimation.launch.py deleted file mode 100644 index 643b12b..0000000 --- a/src/open_place_recognition/launch/depth_estimation.launch.py +++ /dev/null @@ -1,22 +0,0 @@ -from launch import LaunchDescription -from launch_ros.actions import Node - -def generate_launch_description(): - return LaunchDescription([ - Node( - package='open_place_recognition', - executable='depth_estimation', - name='depth_estimation_with_lidar', - output='screen', - emulate_tty=True, - parameters=[ - {"image_front_topic": "/zed_node/left/image_rect_color/compressed", - "camera_info_front_topic": "/zed_node/left/camera_info", - "lidar_topic": "/velodyne_points", - "publish_point_cloud_form_depth": True, - "model_weights_path": "/home/docker_opr_ros2/ros2_ws/dependencies/OpenPlaceRecognition/weights/depth_estimation/res50.pth", - "device": "cuda", - "image_resize": [640, 480]} - ] - ) - ]) diff --git a/src/open_place_recognition/launch/localization_launch.py b/src/open_place_recognition/launch/localization_launch.py deleted file mode 100644 index 26b6a1a..0000000 --- a/src/open_place_recognition/launch/localization_launch.py +++ /dev/null @@ -1,25 +0,0 @@ -from launch import LaunchDescription -from launch_ros.actions import Node - -def generate_launch_description(): - return LaunchDescription([ - Node( - package='open_place_recognition', - executable='localization', - name='hierarchical_localization', - output='screen', - emulate_tty=True, - parameters=[ - { - "image_front_topic": "/zed_node/left/image_rect_color/compressed", - "image_back_topic": "/realsense_back/color/image_raw/compressed", - "mask_front_topic": "/zed_node/left/semantic_segmentation", - "mask_back_topic": "/realsense_back/semantic_segmentation", - "lidar_topic": "/velodyne_points", - "pipeline_cfg": "/home/docker_opr_ros2/ros2_ws/src/open_place_recognition/configs/pipelines/localization_pipeline.yaml", - # "pipeline_cfg": "/home/docker_opr_ros2/ros2_ws/src/open_place_recognition/configs/pipelines/aruco_localization_pipeline.yaml", - "exclude_dynamic_classes": False, - "image_resize": [320, 192]} - ] - ) - ]) diff --git a/src/open_place_recognition/launch/place_recognition_launch.py b/src/open_place_recognition/launch/place_recognition_launch.py deleted file mode 100644 index e65137d..0000000 --- a/src/open_place_recognition/launch/place_recognition_launch.py +++ /dev/null @@ -1,25 +0,0 @@ -from launch import LaunchDescription -from launch_ros.actions import Node - -def generate_launch_description(): - return LaunchDescription([ - Node( - package='open_place_recognition', - executable='place_recognition', - name='multimodal_multicamera_lidar_place_recognition', - output='screen', - emulate_tty=True, - parameters=[ - {"image_front_topic": "/zed_node/left/image_rect_color/compressed", - "image_back_topic": "/realsense_back/color/image_raw/compressed", - "mask_front_topic": "/zed_node/left/semantic_segmentation", - "mask_back_topic": "/realsense_back/semantic_segmentation", - "lidar_topic": "/velodyne_points", - "pipeline_cfg": "/home/docker_opr_ros2/ros2_ws/src/open_place_recognition/configs/pipelines/place_recognition/multimodal_pr.yaml", - # "pipeline_cfg": "/home/docker_opr_ros2/ros2_ws/src/open_place_recognition/configs/pipelines/place_recognition/multimodal_semantic_pr.yaml", - # "pipeline_cfg": "/home/docker_opr_ros2/ros2_ws/src/open_place_recognition/configs/pipelines/place_recognition/multimodal_with_soc_outdoor_pr.yaml", - # "pipeline_cfg": "/home/docker_opr_ros2/ros2_ws/src/open_place_recognition/configs/pipelines/place_recognition/multimodal_semantic_with_soc_outdoor_pr.yaml", - "image_resize": [320, 192]} - ] - ) - ]) diff --git a/src/open_place_recognition/open_place_recognition/localization.py b/src/open_place_recognition/open_place_recognition/localization.py deleted file mode 100644 index 2171707..0000000 --- a/src/open_place_recognition/open_place_recognition/localization.py +++ /dev/null @@ -1,397 +0,0 @@ -import cv2 -import numpy as np -import rclpy -import torch -from cv_bridge import CvBridge -from hydra.utils import instantiate -from message_filters import ApproximateTimeSynchronizer, Subscriber -from omegaconf import OmegaConf -from rclpy.node import Node -from rcl_interfaces.msg import ParameterDescriptor -from builtin_interfaces.msg import Time -from geometry_msgs.msg import PoseStamped -from sensor_msgs.msg import Image, CompressedImage, PointCloud2 -from sensor_msgs_py.point_cloud2 import read_points -from std_msgs.msg import Int32 -from torch import Tensor -import albumentations as A -from albumentations.pytorch import ToTensorV2 - -from opr.pipelines.localization import ArucoLocalizationPipeline -from opr_interfaces.msg import DatabaseMatchIndex -from opr.datasets.augmentations import DefaultImageTransform, DefaultSemanticTransform -from opr.datasets.projection import Projector -from opr.datasets.soc_utils import ( - get_points_labels_by_mask, - instance_masks_to_objects, - pack_objects, - semantic_mask_to_instances, -) - - -TOPIC = "/zed_node/left/image_rect_color/compressed" -DATABASE_DIR = "/path/to/database" -MODEL_CFG = "/path/to/model/config.yaml" -MODEL_WEIGHTS_PATH = "/path/to/model/weights.pth" -DEVICE = "cuda" -IMAGE_RESIZE = (320, 192) - - -class ImageTransformWithoutNormalize: - def __call__(self, img: np.ndarray): - """Applies transformations to the given image. - - Args: - img (np.ndarray): The image in the cv2 format. - - Returns: - Tensor: Augmented PyTorch tensor in the channel-first format. - """ - return A.Compose([ToTensorV2()])(image=img)["image"] - - -class LocalizationNode(Node): - - def __init__(self): - super().__init__("localization") - self._declare_parameters() - - image_front_topic = self.get_parameter("image_front_topic").get_parameter_value().string_value - image_back_topic = self.get_parameter("image_back_topic").get_parameter_value().string_value - mask_front_topic = self.get_parameter("mask_front_topic").get_parameter_value().string_value - mask_back_topic = self.get_parameter("mask_back_topic").get_parameter_value().string_value - lidar_topic = self.get_parameter("lidar_topic").get_parameter_value().string_value - pipeline_cfg = self.get_parameter("pipeline_cfg").get_parameter_value().string_value - exclude_dynamic_classes = self.get_parameter("exclude_dynamic_classes").get_parameter_value().bool_value - image_resize = self.get_parameter("image_resize").get_parameter_value().integer_array_value - - self.cv_bridge = CvBridge() - - self.image_front_sub = Subscriber(self, CompressedImage, image_front_topic) - self.image_back_sub = Subscriber(self, CompressedImage, image_back_topic) - self.mask_front_sub = Subscriber(self, Image, mask_front_topic) - self.mask_back_sub = Subscriber(self, Image, mask_back_topic) - self.lidar_sub = Subscriber(self, PointCloud2, lidar_topic) - - self.ts = ApproximateTimeSynchronizer( - [self.image_front_sub, self.image_back_sub, self.mask_front_sub, self.mask_back_sub, self.lidar_sub], - queue_size=1, - slop=0.05, - ) - self.ts.registerCallback(self.listener_callback) - - self.db_match_pose_pub = self.create_publisher(PoseStamped, "/place_recognition/pose", 10) - self.idx_pub = self.create_publisher(DatabaseMatchIndex, "/place_recognition/db_idx", 10) - self.estimated_pose_pub = self.create_publisher(PoseStamped, "/localization/pose", 10) - - self.pipeline = instantiate(OmegaConf.load(pipeline_cfg)) - - if self.pipeline.pr_pipe.model.soc_module is not None: - self.load_soc = True - self.get_logger().info(f"self.load_soc is set to True.") - sensors_cfg = OmegaConf.load("/home/docker_opr_ros2/ros2_ws/src/open_place_recognition/configs/sensors/husky.yaml") - anno_cfg = OmegaConf.load("/home/docker_opr_ros2/ros2_ws/src/open_place_recognition/configs/anno/oneformer.yaml") - self.front_cam_proj = Projector(sensors_cfg.front_cam, sensors_cfg.lidar) - self.back_cam_proj = Projector(sensors_cfg.back_cam, sensors_cfg.lidar) - self.max_distance_soc = 50.0 - self.top_k_soc = self.pipeline.pr_pipe.model.soc_module.num_objects - self.special_classes = anno_cfg.special_classes - self.soc_coords_type = "euclidean" - else: - self.load_soc = False - - if isinstance(self.pipeline, ArucoLocalizationPipeline): - self.image_transform = ImageTransformWithoutNormalize() - self.mask_transform = DefaultSemanticTransform(train=False, resize=None) - else: - self.image_transform = DefaultImageTransform(train=False, resize=image_resize) - self.mask_transform = DefaultSemanticTransform(train=False, resize=image_resize) - - self._ade20k_dynamic_idx = [12] - self.exclude_dynamic_classes = exclude_dynamic_classes - - self.lidar2front = np.array([[ 0.01509615, -0.99976457, -0.01558544, 0.04632156], - [ 0.00871086, 0.01571812, -0.99983852, -0.13278588], - [ 0.9998481, 0.01495794, 0.0089461, -0.06092749], - [ 0. , 0. , 0. , 1. ]]) - self.lidar2back = np.array([[-1.50409674e-02, 9.99886421e-01, 9.55906151e-04, 1.82703304e-02], - [-1.30440106e-02, 7.59716299e-04, -9.99914635e-01, -1.41787545e-01], - [-9.99801792e-01, -1.50521522e-02, 1.30311022e-02, -6.72336358e-02], - [ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 1.00000000e+00]]) - self.front_matrix = np.array([[683.6199340820312, 0.0, 615.1160278320312, 0.0, 683.6199340820312, 345.32354736328125, 0.0, 0.0, 1.0]]).reshape((3,3)) - self.front_dist = np.array([0.0, 0.0, 0.0, 0.0, 0.0]) - self.back_matrix = np.array([[910.4178466796875, 0.0, 648.44140625, 0.0, 910.4166870117188, 354.0118408203125, 0.0, 0.0, 1.0]]).reshape((3,3)) - self.back_dist = np.array([0.0, 0.0, 0.0, 0.0, 0.0]) - - self.get_logger().info(f"Initialized {self.__class__.__name__} node.") - - def _declare_parameters(self) -> None: - self.declare_parameter( - "image_front_topic", - rclpy.Parameter.Type.STRING, #"/zed_node/left/image_rect_color/compressed", - ParameterDescriptor(description="Front camera image topic.") - ) - self.declare_parameter( - "image_back_topic", - rclpy.Parameter.Type.STRING, #"/realsense_back/color/image_raw/compressed", - ParameterDescriptor(description="Back camera image topic.") - ) - self.declare_parameter( - "mask_front_topic", - rclpy.Parameter.Type.STRING, #"/zed_node/left/semantic_segmentation", - ParameterDescriptor(description="Front semantic segmentation mask topic.") - ) - self.declare_parameter( - "mask_back_topic", - rclpy.Parameter.Type.STRING, #"/realsense_back/semantic_segmentation", - ParameterDescriptor(description="Back semantic segmentation mask topic.") - ) - self.declare_parameter( - "lidar_topic", - rclpy.Parameter.Type.STRING, #"/velodyne_points", - ParameterDescriptor(description="Lidar pointcloud topic.") - ) - self.declare_parameter( - "pipeline_cfg", - rclpy.Parameter.Type.STRING, #"", - ParameterDescriptor(description="Path to the pipeline configuration file.") - ) - self.declare_parameter( - "exclude_dynamic_classes", - rclpy.Parameter.Type.BOOL, - ParameterDescriptor(description="Exclude dynamic objects from the input data.") - ) - self.declare_parameter( - "image_resize", - rclpy.Parameter.Type.INTEGER_ARRAY, #(320, 192), - ParameterDescriptor(description="Image resize dimensions.") - ) - - def _prepare_input( - self, - images: np.ndarray | list[np.ndarray] = None, - masks: np.ndarray | list[np.ndarray] = None, - pointcloud: np.ndarray = None, - ) -> dict[str, Tensor]: - input_data = {} - if images is not None: - if isinstance(images, list): - for i, image in enumerate(images): - input_data[f"image_{i}"] = image - if self.exclude_dynamic_classes and masks is not None: - for index in self._ade20k_dynamic_idx: - input_data[f"image_{i}"] = np.where(masks[i][:, :, np.newaxis] == index, 0, input_data[f"image_{i}"]) - input_data[f"image_{i}"] = self.image_transform(input_data[f"image_{i}"]) - elif isinstance(images, np.ndarray): - input_data["image_0"] = images - if self.exclude_dynamic_classes and masks is not None: - for index in self._ade20k_dynamic_idx: - input_data["image_0"] = np.where(masks == index, 0, input_data["image_0"]) - input_data["image_0"] = self.image_transform(input_data["image_0"]) - else: - self.get_logger().warning(f"Invalid type for images in '_prepare_input': {type(images)}") - if masks is not None: - if isinstance(masks, list): - for i, mask in enumerate(masks): - input_data[f"mask_{i}"] = self.mask_transform(mask) - elif isinstance(masks, np.ndarray): - input_data["mask_0"] = self.mask_transform(masks) - else: - self.get_logger().warning(f"Invalid type for masks in '_prepare_input': {type(masks)}") - if pointcloud is not None: - if self.exclude_dynamic_classes and masks is not None: - if isinstance(masks, list): - for i, mask in enumerate(masks): - pointcloud = self._remove_dynamic_points(pointcloud, mask, self.lidar2back, self.back_matrix, self.back_dist) - elif isinstance(masks, np.ndarray): - pointcloud = self._remove_dynamic_points(pointcloud, masks, self.lidar2back, self.back_matrix, self.back_dist) - pointcloud = torch.tensor(pointcloud).contiguous() - pointcloud = pointcloud[~torch.any(pointcloud.isnan(), dim=1)] - input_data["pointcloud_lidar_coords"] = pointcloud[:, :3] - if pointcloud.shape[1] > 3: - input_data["pointcloud_lidar_feats"] = pointcloud[:, 3].unsqueeze(1) - else: - input_data["pointcloud_lidar_feats"] = torch.ones_like(pointcloud[:, :1]) - if self.load_soc: - input_data["soc"] = self._get_soc(mask_front=masks[0], mask_back=masks[1], lidar_scan=pointcloud) - return input_data - - def _get_soc(self, mask_front: np.ndarray, mask_back: np.ndarray, lidar_scan: np.ndarray) -> Tensor: - coords_front, _, in_image_front = self.front_cam_proj(lidar_scan) - coords_back, _, in_image_back = self.back_cam_proj(lidar_scan) - - point_labels = np.zeros(len(lidar_scan), dtype=np.uint8) - point_labels[in_image_front] = get_points_labels_by_mask(coords_front, mask_front) - point_labels[in_image_back] = get_points_labels_by_mask(coords_back, mask_back) - - instances_front = semantic_mask_to_instances( - mask_front, - area_threshold=10, - labels_whitelist=self.special_classes, - ) - instances_back = semantic_mask_to_instances( - mask_back, - area_threshold=10, - labels_whitelist=self.special_classes, - ) - - objects_front = instance_masks_to_objects( - instances_front, - coords_front, - point_labels[in_image_front], - lidar_scan[in_image_front], - ) - objects_back = instance_masks_to_objects( - instances_back, - coords_back, - point_labels[in_image_back], - lidar_scan[in_image_back], - ) - - objects = {**objects_front, **objects_back} - packed_objects = pack_objects(objects, self.top_k_soc, self.max_distance_soc, self.special_classes) - - if self.soc_coords_type == "cylindrical_3d": - packed_objects = np.concatenate( - ( - np.linalg.norm(packed_objects, axis=-1, keepdims=True), - np.arctan2(packed_objects[..., 1], packed_objects[..., 0])[..., None], - packed_objects[..., 2:], - ), - axis=-1, - ) - elif self.soc_coords_type == "cylindrical_2d": - packed_objects = np.concatenate( - ( - np.linalg.norm(packed_objects[..., :2], axis=-1, keepdims=True), - np.arctan2(packed_objects[..., 1], packed_objects[..., 0])[..., None], - packed_objects[..., 2:], - ), - axis=-1, - ) - elif self.soc_coords_type == "euclidean": - pass - elif self.soc_coords_type == "spherical": - packed_objects = np.concatenate( - ( - np.linalg.norm(packed_objects, axis=-1, keepdims=True), - np.arccos( - packed_objects[..., 2] / np.linalg.norm(packed_objects, axis=-1, keepdims=True) - ), - np.arctan2(packed_objects[..., 1], packed_objects[..., 0])[..., None], - ), - axis=-1, - ) - else: - raise ValueError(f"Unknown soc_coords_type: {self.soc_coords_type!r}") - - objects_tensor = torch.from_numpy(packed_objects).float() - - return objects_tensor - - def _remove_dynamic_points(self, pointcloud: np.ndarray, semantic_map: np.ndarray, lidar2sensor: np.ndarray, - sensor_intrinsics: np.ndarray, sensor_dist: np.ndarray) -> np.ndarray: - pc_values = np.concatenate([pointcloud, np.ones((pointcloud.shape[0], 1))],axis=1).T - camera_values = lidar2sensor @ pc_values - camera_values = np.transpose(camera_values)[:, :3] - - points_2d, _ = cv2.projectPoints(camera_values, - np.zeros((3, 1), np.float32), np.zeros((3, 1), np.float32), - sensor_intrinsics, - sensor_dist) - points_2d = points_2d[:, 0, :] - - classes = set(np.unique(semantic_map)) - dynamic_classes = set(self._ade20k_dynamic_idx) - if classes.intersection(dynamic_classes): - valid = (~np.isnan(points_2d[:,0])) & (~np.isnan(points_2d[:,1])) - in_bounds_x = (points_2d[:,0] >= 0) & (points_2d[:,0] < 1280) - in_bounds_y = (points_2d[:,1] >= 0) & (points_2d[:,1] < 720) - look_forward = (camera_values[:, 2] > 0) - mask = valid & in_bounds_x & in_bounds_y & look_forward - - indices = np.where(mask)[0] - mask_for_points = np.full((points_2d.shape[0], 3), True) - - dynamic_idx = np.array(self._ade20k_dynamic_idx) - semantic_values = semantic_map[np.floor(points_2d[indices, 1]).astype(int), np.floor(points_2d[indices, 0]).astype(int)] - - matching_indices = np.where(np.isin(semantic_values, dynamic_idx)) - - mask_for_points = np.full((points_2d.shape[0], 3), True) - mask_for_points[indices[matching_indices[0]]] = np.array([False, False, False]) - - return pointcloud[mask_for_points].reshape((-1, 3)) - else: - return pointcloud - - def _create_pose_msg(self, pose: np.ndarray, timestamp: Time) -> PoseStamped: - pose_msg = PoseStamped() - pose_msg.header.stamp = timestamp - pose_msg.pose.position.x = pose[0] - pose_msg.pose.position.y = pose[1] - pose_msg.pose.position.z = pose[2] - pose_msg.pose.orientation.x = pose[3] - pose_msg.pose.orientation.y = pose[4] - pose_msg.pose.orientation.z = pose[5] - pose_msg.pose.orientation.w = pose[6] - return pose_msg - - def _create_idx_msg(self, idx: int, timestamp: Time) -> DatabaseMatchIndex: - idx_msg = DatabaseMatchIndex() - idx_msg.header.stamp = timestamp - idx_msg.index = idx - return idx_msg - - def listener_callback( - self, - front_image_msg: CompressedImage, - back_image_msg: CompressedImage, - front_mask_msg: Image, - back_mask_msg: Image, - lidar_msg: PointCloud2, - ) -> None: - self.get_logger().info("Received synchronized messages.") - t_start = self.get_clock().now() - lidar_timestamp = lidar_msg.header.stamp - front_image = self.cv_bridge.compressed_imgmsg_to_cv2(front_image_msg) - back_image = self.cv_bridge.compressed_imgmsg_to_cv2(back_image_msg) - front_mask = self.cv_bridge.imgmsg_to_cv2(front_mask_msg) - back_mask = self.cv_bridge.imgmsg_to_cv2(back_mask_msg) - pointcloud = read_points(lidar_msg, field_names=("x", "y", "z")) - pointcloud = np.array([pointcloud["x"], pointcloud["y"], pointcloud["z"]]).T - input_data = self._prepare_input( - images=[front_image, back_image], masks=[front_mask, back_mask], pointcloud=pointcloud - ) - output = self.pipeline.infer(input_data) - t_taken = self.get_clock().now() - t_start - self.get_logger().debug(f"Localization inference took: {t_taken.nanoseconds / 1e6} ms.") - self.get_logger().info(f"output['db_match_pose'] = {output['db_match_pose']}") - output['db_match_pose'] = output['db_match_pose'].tolist() - db_match_pose_msg = self._create_pose_msg(output["db_match_pose"], lidar_timestamp) - estimated_pose_msg = self._create_pose_msg(output["estimated_pose"], lidar_timestamp) - idx_msg = self._create_idx_msg(int(output["db_idx"]), lidar_timestamp) - self.db_match_pose_pub.publish(db_match_pose_msg) - self.get_logger().info(f"Published db match pose message: {db_match_pose_msg.pose}") - self.estimated_pose_pub.publish(estimated_pose_msg) - self.get_logger().info(f"Published estimated pose message: {estimated_pose_msg.pose}") - self.idx_pub.publish(idx_msg) - self.get_logger().info(f"Published database index message: {idx_msg.index}") - - -def main(args=None): - rclpy.init(args=args) - - loc_node = LocalizationNode() - - rclpy.spin(loc_node) - - # Destroy the node explicitly - # (optional - otherwise it will be done automatically - # when the garbage collector destroys the node object) - loc_node.destroy_node() - rclpy.shutdown() - - -if __name__ == '__main__': - main() diff --git a/src/open_place_recognition/open_place_recognition/place_recognition.py b/src/open_place_recognition/open_place_recognition/place_recognition.py deleted file mode 100644 index e3a4294..0000000 --- a/src/open_place_recognition/open_place_recognition/place_recognition.py +++ /dev/null @@ -1,299 +0,0 @@ -import numpy as np -import rclpy -import torch -from cv_bridge import CvBridge -from hydra.utils import instantiate -from message_filters import ApproximateTimeSynchronizer, Subscriber -from omegaconf import OmegaConf -from rclpy.node import Node -from rcl_interfaces.msg import ParameterDescriptor -from builtin_interfaces.msg import Time -from geometry_msgs.msg import PoseStamped -from sensor_msgs.msg import Image, CompressedImage, PointCloud2 -from sensor_msgs_py.point_cloud2 import read_points -from std_msgs.msg import Int32 -from torch import Tensor - -from opr_interfaces.msg import DatabaseMatchIndex -from opr.datasets.augmentations import DefaultImageTransform, DefaultSemanticTransform -from opr.datasets.projection import Projector -from opr.datasets.soc_utils import ( - get_points_labels_by_mask, - instance_masks_to_objects, - pack_objects, - semantic_mask_to_instances, -) -from opr.pipelines.place_recognition import PlaceRecognitionPipeline - - -TOPIC = "/zed_node/left/image_rect_color/compressed" -DATABASE_DIR = "/path/to/database" -MODEL_CFG = "/path/to/model/config.yaml" -MODEL_WEIGHTS_PATH = "/path/to/model/weights.pth" -DEVICE = "cuda" -IMAGE_RESIZE = (320, 192) - - -class PlaceRecognitionNode(Node): - - def __init__(self): - super().__init__("place_recognition") - self._declare_parameters() - - image_front_topic = self.get_parameter("image_front_topic").get_parameter_value().string_value - image_back_topic = self.get_parameter("image_back_topic").get_parameter_value().string_value - mask_front_topic = self.get_parameter("mask_front_topic").get_parameter_value().string_value - mask_back_topic = self.get_parameter("mask_back_topic").get_parameter_value().string_value - lidar_topic = self.get_parameter("lidar_topic").get_parameter_value().string_value - pipeline_cfg = self.get_parameter("pipeline_cfg").get_parameter_value().string_value - image_resize = self.get_parameter("image_resize").get_parameter_value().integer_array_value - - self.cv_bridge = CvBridge() - - self.image_front_sub = Subscriber(self, CompressedImage, image_front_topic) - self.image_back_sub = Subscriber(self, CompressedImage, image_back_topic) - self.mask_front_sub = Subscriber(self, Image, mask_front_topic) - self.mask_back_sub = Subscriber(self, Image, mask_back_topic) - self.lidar_sub = Subscriber(self, PointCloud2, lidar_topic) - - self.ts = ApproximateTimeSynchronizer( - [self.image_front_sub, self.image_back_sub, self.mask_front_sub, self.mask_back_sub, self.lidar_sub], - queue_size=1, - slop=0.05, - ) - self.ts.registerCallback(self.listener_callback) - - self.pose_pub = self.create_publisher(PoseStamped, "/place_recognition/pose", 10) - self.idx_pub = self.create_publisher(DatabaseMatchIndex, "/place_recognition/db_idx", 10) - - pipeline_config = OmegaConf.load(pipeline_cfg) - self.pr_pipe = instantiate(pipeline_config) - - if self.pr_pipe.model.soc_module is not None: - self.load_soc = True - self.get_logger().info(f"self.load_soc is set to True.") - sensors_cfg = OmegaConf.load("/home/docker_opr_ros2/ros2_ws/src/open_place_recognition/configs/sensors/husky.yaml") - anno_cfg = OmegaConf.load("/home/docker_opr_ros2/ros2_ws/src/open_place_recognition/configs/anno/oneformer.yaml") - self.front_cam_proj = Projector(sensors_cfg.front_cam, sensors_cfg.lidar) - self.back_cam_proj = Projector(sensors_cfg.back_cam, sensors_cfg.lidar) - self.max_distance_soc = 50.0 - self.top_k_soc = self.pr_pipe.model.soc_module.num_objects - self.special_classes = anno_cfg.special_classes - self.soc_coords_type = "euclidean" - else: - self.load_soc = False - - self.image_transform = DefaultImageTransform(train=False, resize=image_resize) - self.mask_transform = DefaultSemanticTransform(train=False, resize=image_resize) - - self.get_logger().info(f"Initialized {self.__class__.__name__} node.") - - def _declare_parameters(self) -> None: - self.declare_parameter( - "image_front_topic", - rclpy.Parameter.Type.STRING, #"/zed_node/left/image_rect_color/compressed", - ParameterDescriptor(description="Front camera image topic.") - ) - self.declare_parameter( - "image_back_topic", - rclpy.Parameter.Type.STRING, #"/realsense_back/color/image_raw/compressed", - ParameterDescriptor(description="Back camera image topic.") - ) - self.declare_parameter( - "mask_front_topic", - rclpy.Parameter.Type.STRING, #"/zed_node/left/semantic_segmentation", - ParameterDescriptor(description="Front semantic segmentation mask topic.") - ) - self.declare_parameter( - "mask_back_topic", - rclpy.Parameter.Type.STRING, #"/realsense_back/semantic_segmentation", - ParameterDescriptor(description="Back semantic segmentation mask topic.") - ) - self.declare_parameter( - "lidar_topic", - rclpy.Parameter.Type.STRING, #"/velodyne_points", - ParameterDescriptor(description="Lidar pointcloud topic.") - ) - self.declare_parameter( - "pipeline_cfg", - rclpy.Parameter.Type.STRING, #"", - ParameterDescriptor(description="Path to the pipeline configuration file.") - ) - self.declare_parameter( - "image_resize", - rclpy.Parameter.Type.INTEGER_ARRAY, #(320, 192), - ParameterDescriptor(description="Image resize dimensions.") - ) - - def _prepare_input( - self, - images: np.ndarray | list[np.ndarray] = None, - masks: np.ndarray | list[np.ndarray] = None, - pointcloud: np.ndarray = None, - ) -> dict[str, Tensor]: - input_data = {} - if images is not None: - if isinstance(images, list): - for i, image in enumerate(images): - input_data[f"image_{i}"] = self.image_transform(image) - elif isinstance(images, np.ndarray): - input_data["image_0"] = self.image_transform(images) - else: - self.get_logger().warning(f"Invalid type for images in '_prepare_input': {type(images)}") - if masks is not None: - if isinstance(masks, list): - for i, mask in enumerate(masks): - input_data[f"mask_{i}"] = self.mask_transform(mask) - elif isinstance(masks, np.ndarray): - input_data["mask_0"] = self.mask_transform(masks) - else: - self.get_logger().warning(f"Invalid type for masks in '_prepare_input': {type(masks)}") - if pointcloud is not None: - pointcloud = torch.tensor(pointcloud).contiguous() - input_data["pointcloud_lidar_coords"] = pointcloud[:, :3] - if pointcloud.shape[1] > 3: - input_data["pointcloud_lidar_feats"] = pointcloud[:, 3].unsqueeze(1) - else: - input_data["pointcloud_lidar_feats"] = torch.ones_like(pointcloud[:, :1]) - if self.load_soc: - input_data["soc"] = self._get_soc(mask_front=masks[0], mask_back=masks[1], lidar_scan=pointcloud) - return input_data - - def _get_soc(self, mask_front: np.ndarray, mask_back: np.ndarray, lidar_scan: np.ndarray) -> Tensor: - coords_front, _, in_image_front = self.front_cam_proj(lidar_scan) - coords_back, _, in_image_back = self.back_cam_proj(lidar_scan) - - point_labels = np.zeros(len(lidar_scan), dtype=np.uint8) - point_labels[in_image_front] = get_points_labels_by_mask(coords_front, mask_front) - point_labels[in_image_back] = get_points_labels_by_mask(coords_back, mask_back) - - instances_front = semantic_mask_to_instances( - mask_front, - area_threshold=10, - labels_whitelist=self.special_classes, - ) - instances_back = semantic_mask_to_instances( - mask_back, - area_threshold=10, - labels_whitelist=self.special_classes, - ) - - objects_front = instance_masks_to_objects( - instances_front, - coords_front, - point_labels[in_image_front], - lidar_scan[in_image_front], - ) - objects_back = instance_masks_to_objects( - instances_back, - coords_back, - point_labels[in_image_back], - lidar_scan[in_image_back], - ) - - objects = {**objects_front, **objects_back} - packed_objects = pack_objects(objects, self.top_k_soc, self.max_distance_soc, self.special_classes) - - if self.soc_coords_type == "cylindrical_3d": - packed_objects = np.concatenate( - ( - np.linalg.norm(packed_objects, axis=-1, keepdims=True), - np.arctan2(packed_objects[..., 1], packed_objects[..., 0])[..., None], - packed_objects[..., 2:], - ), - axis=-1, - ) - elif self.soc_coords_type == "cylindrical_2d": - packed_objects = np.concatenate( - ( - np.linalg.norm(packed_objects[..., :2], axis=-1, keepdims=True), - np.arctan2(packed_objects[..., 1], packed_objects[..., 0])[..., None], - packed_objects[..., 2:], - ), - axis=-1, - ) - elif self.soc_coords_type == "euclidean": - pass - elif self.soc_coords_type == "spherical": - packed_objects = np.concatenate( - ( - np.linalg.norm(packed_objects, axis=-1, keepdims=True), - np.arccos( - packed_objects[..., 2] / np.linalg.norm(packed_objects, axis=-1, keepdims=True) - ), - np.arctan2(packed_objects[..., 1], packed_objects[..., 0])[..., None], - ), - axis=-1, - ) - else: - raise ValueError(f"Unknown soc_coords_type: {self.soc_coords_type!r}") - - objects_tensor = torch.from_numpy(packed_objects).float() - - return objects_tensor - - def _create_pose_msg(self, pose: np.ndarray, timestamp: Time) -> PoseStamped: - pose_msg = PoseStamped() - pose_msg.header.stamp = timestamp - pose_msg.pose.position.x = pose[0] - pose_msg.pose.position.y = pose[1] - pose_msg.pose.position.z = pose[2] - pose_msg.pose.orientation.x = pose[3] - pose_msg.pose.orientation.y = pose[4] - pose_msg.pose.orientation.z = pose[5] - pose_msg.pose.orientation.w = pose[6] - return pose_msg - - def _create_idx_msg(self, idx: int, timestamp: Time) -> DatabaseMatchIndex: - idx_msg = DatabaseMatchIndex() - idx_msg.header.stamp = timestamp - idx_msg.index = idx - return idx_msg - - def listener_callback( - self, - front_image_msg: CompressedImage, - back_image_msg: CompressedImage, - front_mask_msg: Image, - back_mask_msg: Image, - lidar_msg: PointCloud2, - ) -> None: - self.get_logger().info("Received synchronized messages.") - t_start = self.get_clock().now() - lidar_timestamp = lidar_msg.header.stamp - front_image = self.cv_bridge.compressed_imgmsg_to_cv2(front_image_msg) - back_image = self.cv_bridge.compressed_imgmsg_to_cv2(back_image_msg) - front_mask = self.cv_bridge.imgmsg_to_cv2(front_mask_msg) - back_mask = self.cv_bridge.imgmsg_to_cv2(back_mask_msg) - pointcloud = read_points(lidar_msg, field_names=("x", "y", "z")) - pointcloud = np.array([pointcloud["x"], pointcloud["y"], pointcloud["z"]]).T - input_data = self._prepare_input( - images=[front_image, back_image], masks=[front_mask, back_mask], pointcloud=pointcloud - ) - output = self.pr_pipe.infer(input_data) - t_taken = self.get_clock().now() - t_start - self.get_logger().debug(f"Place recognition inference took: {t_taken.nanoseconds / 1e6} ms.") - pose_msg = self._create_pose_msg(output["pose"], lidar_timestamp) - idx_msg = self._create_idx_msg(int(output["idx"]), lidar_timestamp) - self.pose_pub.publish(pose_msg) - self.get_logger().info(f"Published pose message: {pose_msg.pose}") - self.idx_pub.publish(idx_msg) - self.get_logger().info(f"Published database index message: {idx_msg.index}") - - -def main(args=None): - rclpy.init(args=args) - - pr_node = PlaceRecognitionNode() - - rclpy.spin(pr_node) - - # Destroy the node explicitly - # (optional - otherwise it will be done automatically - # when the garbage collector destroys the node object) - pr_node.destroy_node() - rclpy.shutdown() - - -if __name__ == '__main__': - main() diff --git a/src/open_place_recognition/resource/open_place_recognition b/src/open_place_recognition/resource/open_place_recognition deleted file mode 100644 index e69de29..0000000 diff --git a/src/open_place_recognition/setup.cfg b/src/open_place_recognition/setup.cfg deleted file mode 100644 index af4ef08..0000000 --- a/src/open_place_recognition/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/open_place_recognition -[install] -install_scripts=$base/lib/open_place_recognition diff --git a/src/open_place_recognition/setup.py b/src/open_place_recognition/setup.py deleted file mode 100644 index 5715a06..0000000 --- a/src/open_place_recognition/setup.py +++ /dev/null @@ -1,32 +0,0 @@ -import os -from glob import glob -from setuptools import find_packages, setup - -package_name = 'open_place_recognition' - -setup( - name=package_name, - version='0.0.0', - packages=find_packages(exclude=['test']), - data_files=[ - ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), - ('share/' + package_name, ['package.xml']), - (os.path.join('share', package_name), glob('launch/*launch.[pxy][yma]*')), - ], - install_requires=['setuptools'], - zip_safe=True, - maintainer='docker_opr_ros2', - maintainer_email='amelekhin96@gmail.com', - description='TODO: Package description', - license='TODO: License declaration', - tests_require=['pytest'], - entry_points={ - 'console_scripts': [ - 'place_recognition = open_place_recognition.place_recognition:main', - 'visualizer = open_place_recognition.visualizer:main', - 'localization = open_place_recognition.localization:main', - 'depth_estimation = open_place_recognition.depth_estimation:main' - ], - }, -) diff --git a/src/open_place_recognition/test/test_copyright.py b/src/open_place_recognition/test/test_copyright.py deleted file mode 100644 index 97a3919..0000000 --- a/src/open_place_recognition/test/test_copyright.py +++ /dev/null @@ -1,25 +0,0 @@ -# Copyright 2015 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_copyright.main import main -import pytest - - -# Remove the `skip` decorator once the source file(s) have a copyright header -@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') -@pytest.mark.copyright -@pytest.mark.linter -def test_copyright(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found errors' diff --git a/src/open_place_recognition/test/test_flake8.py b/src/open_place_recognition/test/test_flake8.py deleted file mode 100644 index 27ee107..0000000 --- a/src/open_place_recognition/test/test_flake8.py +++ /dev/null @@ -1,25 +0,0 @@ -# Copyright 2017 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_flake8.main import main_with_errors -import pytest - - -@pytest.mark.flake8 -@pytest.mark.linter -def test_flake8(): - rc, errors = main_with_errors(argv=[]) - assert rc == 0, \ - 'Found %d code style errors / warnings:\n' % len(errors) + \ - '\n'.join(errors) diff --git a/src/open_place_recognition/test/test_pep257.py b/src/open_place_recognition/test/test_pep257.py deleted file mode 100644 index b234a38..0000000 --- a/src/open_place_recognition/test/test_pep257.py +++ /dev/null @@ -1,23 +0,0 @@ -# Copyright 2015 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_pep257.main import main -import pytest - - -@pytest.mark.linter -@pytest.mark.pep257 -def test_pep257(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found code style errors / warnings'