Modified docker, added basestation commands

Dockerfile

 # syntax=docker/dockerfile:1
 FROM nvidia/cuda:11.5.1-runtime-ubuntu20.04 AS builder
-ENV PATH="/root/miniconda3/bin:${PATH}"
-ARG PATH="/root/miniconda3/bin:${PATH}"
+ARG PYTHON_VERSION=3.8
 SHELL ["/bin/bash", "-c"]
 # ARG SSH_KEY=docker_id_rsa
 # ADD ${SSH_KEY} /root/.ssh/id_rsa
@@ -34,18 +33,35 @@ RUN apt-get update && apt-get install -y \
     pkg-config && \
     wget -O- http://neuro.debian.net/lists/xenial.us-ca.full |  tee /etc/apt/sources.list.d/neurodebian.sources.list && \
     apt-key adv --recv-keys --keyserver hkp://keyserver.ubuntu.com:80 0xA5D32F012649A5A9 && \
-    wget https://repo.anaconda.com/miniconda/Miniconda3-latest-Linux-x86_64.sh \
-    && mkdir /root/.conda \
-    && bash Miniconda3-latest-Linux-x86_64.sh -b \
-    && rm -f Miniconda3-latest-Linux-x86_64.sh && \
+    apt-get install -y unzip wget software-properties-common && \
+    add-apt-repository ppa:deadsnakes/ppa && \
+    apt-get -y update && \
+    apt-get install -y python${PYTHON_VERSION}
+
+RUN apt-get install python3-distutils && wget https://bootstrap.pypa.io/get-pip.py && python${PYTHON_VERSION} get-pip.py && \
     mkdir -p ~/catkin_ws/src
     # conda update -n base -c defaults conda && \
+RUN update-alternatives --install /usr/bin/python3 python3 /usr/bin/python${PYTHON_VERSION} 1
 
-COPY ./environment.yml /root/environment.yml
 RUN wget --no-check-certificate https://cvg-data.inf.ethz.ch/hloc/netvlad/Pitts30K_struct.mat -O /root/VGG16-NetVLAD-Pitts30K.mat
-RUN conda update -n base -c defaults conda && conda env create -f /root/environment.yml 
+COPY ./requirements.txt /root/requirements.txt
+RUN pip3 install --upgrade pip && pip3 install -r requirements.txt 
 #INSTALL ROS
-RUN source activate meshroom && conda install -c conda-forge -c robostack ros-noetic-desktop && pip install catkin_tools
+# RUN curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | apt-key add -
+
+RUN echo "Installing ROS Noetic..." && \
+    apt-get update && \
+    apt-get install -q -y --no-install-recommends \
+        curl && \
+    echo "deb http://packages.ros.org/ros/ubuntu focal main" > \
+        /etc/apt/sources.list.d/ros-latest.list && \
+    curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | \
+        apt-key add - && \
+    apt-get update && \
+    apt-get install -y --no-install-recommends \
+        ros-noetic-desktop && \
+    rm -rf /var/lib/apt/lists/* && \
+    echo "source /opt/ros/noetic/setup.bash" >> ~/.bashrc
 
 COPY . /root/catkin_ws/src/ari_hloc/
 WORKDIR /root/catkin_ws/src/ari_hloc/
@@ -54,7 +70,7 @@ RUN bash init.sh
 
 # BUILD
 WORKDIR /root/catkin_ws
-RUN source activate meshroom && pip uninstall -y enum34 && catkin build 
+RUN pip3 uninstall -y enum34 && source /opt/ros/noetic/setup.bash && catkin build 
 
 RUN chmod +x /root/catkin_ws/src/ari_hloc/scripts/entrypoint.sh
 ENTRYPOINT ["/root/catkin_ws/src/ari_hloc/scripts/entrypoint.sh","roslaunch","ari_hloc","hloc_map_loc.launch"] 

basestation_commands

+
+MAPPING: 
+
+docker run -it --rm --name wp2_hloc -e ROS_IP=192.168.1.10 --env ROS_MASTER_URI=http://ari-15c:11311 --network=host --gpus all -v /var/run/docker.sock:/var/run/docker.sock -v /home/erm/hloc_cache/:/root/catkin_ws/src/ari_hloc/pipelines/MeshroomCache registry.gitlab.inria.fr/spring/dockers/wp2_hloc:calibrated hloc_localization_mode:=false cache_folder:=/home/erm/hloc_cache/ mapping_front_fisheye_topic:=/front_fisheye_basestation/image_raw/compressed mapping_rear_fisheye_topic:=/rear_fisheye_basestation/image_raw/compressed  
+docker exec -it wp2_hloc bash -c 'source activate meshroom; source /root/catkin_ws/devel/setup.bash;rosservice call /stop_capture '
+
+
+LOCALIZATION: 
+
+docker run -it --rm --name wp2_hloc -e ROS_IP=192.168.1.10 --env ROS_MASTER_URI=http://ari-15c:11311 --network=host --gpus all -v /home/erm/hloc_cache/:/root/catkin_ws/src/ari_hloc/pipelines/MeshroomCache -v /home/erm/tmp/hloc_initializer.py:/root/catkin_ws/src/ari_hloc/scripts/hloc_initializer.py registry.gitlab.inria.fr/spring/dockers/wp2_hloc:calibrated cache_folder:=/home/erm/hloc_cache/ localization_camera:=front num_pairs_matching:=50 localization_front_fisheye_topic:=/front_fisheye_basestation/image_raw/compressed localization_rear_fisheye_topic:=/rear_fisheye_basestation/image_raw/compressed time_ok_max:=10 time_warn_max:=20 movement_threshold:=0.1
+
+docker run -it --rm --name wp2_hloc -e ROS_IP=192.168.1.163 --env ROS_MASTER_URI=http://ari-15c:11311 --network=host --gpus all -v /home/hloc_mapping/hloc_cache/:/root/catkin_ws/src/ari_hloc/pipelines/MeshroomCache registry.gitlab.inria.fr/spring/dockers/wp2_hloc:calibrated cache_folder:=/home/hloc_mapping/hloc_cache/ localization_camera:=front num_pairs_matching:=50 localization_front_fisheye_topic:=/front_fisheye_basestation/image_raw/compressed localization_rear_fisheye_topic:=/rear_fisheye_basestation/image_raw/compressed time_ok_max:=10 time_warn_max:=20
\ No newline at end of file

environment.yml

 name: meshroom
 channels:
   - pytorch
-  - conda-forge
   - defaults
+  - conda-forge
 dependencies:
   - python=3.8
-  - cudatoolkit=10.2
-  - pytorch==1.12.0
-  - torchvision
   - numpy
   - pip  
   - pip:
@@ -27,8 +24,4 @@ dependencies:
       - pybind11==2.8.1
       - plyfile
       - open3d
-  # Debugging & development
-  - jedi-language-server=0.34.11
-  - jupyterlab-lsp=3.9.1
-  - jupyterlab=3.2.4
 

launch/hloc_map_loc.launch

@@ -12,6 +12,7 @@
     <arg name="localization_rear_fisheye_topic" default="/rear_camera/fisheye/image_raw/compressed" />
     <arg name="localization_camera" default="front" />
     <arg name="num_pairs_matching" default = "25" />
+    <arg name="movement_threshold" default = "0.2" />
     
     <!-- Below values are based on calibration in CVUT. Might differ for other partners' ARIs -->
     <arg name="use_calib"   default="true"/>
@@ -42,10 +43,12 @@
     <param name="rel_q_y"  value="$(arg rel_q_y)" />
     <param name="rel_q_z"  value="$(arg rel_q_z)" />
     <param name="rel_q_w"  value="$(arg rel_q_w)" />
+    <!-- use_calib whether to use calibration values or just use TF. -->
     <param name="use_calib" value="$(arg use_calib)" />
     </node>
 
     <node if="$(arg hloc_localization_mode)" pkg="ari_hloc" type="hloc_initializer.py" name="hloc_loc">
+    <param name="movement_threshold"  value="$(arg movement_threshold)" />
     <param name="front_fisheye_topic" type="string" value="$(arg localization_front_fisheye_topic)" />
     <param name="rear_fisheye_topic"  type="string" value="$(arg localization_rear_fisheye_topic)" />
     <param name="localization_camera"  type="string" value="$(arg localization_camera)" />
@@ -69,7 +72,7 @@
     <param name="rel_q_z"  value="$(arg rel_q_z)" />
     <param name="rel_q_w"  value="$(arg rel_q_w)" />
     <param name="use_calib" value="$(arg use_calib)" />
-    
+
     </node>
 
   <node if="$(arg hloc_localization_mode)" pkg="ari_hloc" type="hloc_client.py" args="$(arg time_ok_min) $(arg time_ok_max) $(arg time_warn_max)" name="hloc_client" />

requirements.txt

+numpy
+scipy
+pillow
+transforms3d
+pydegensac
+h5py
+psutil
+enum34
+markdown
+pybind11
+plyfile
+open3d
+torch
+torchvision
+catkin_tools
\ No newline at end of file

scripts/entrypoint.sh

 #!/bin/bash
 echo $@
-source activate meshroom 
+source /opt/ros/noetic/setup.bash
 source /root/catkin_ws/devel/setup.bash
 chmod +x /root/catkin_ws/src/ari_hloc/scripts/capture_and_map.py
 chmod +x /root/catkin_ws/src/ari_hloc/scripts/hloc_localizer.py

scripts/hloc_initializer.py

@@ -36,7 +36,7 @@ rel_R = tf.transformations.quaternion_matrix(rel_q)[:3,:3]
 rel_transform = np.eye(4, dtype=np.float64)
 rel_transform[:3,:3] = rel_R
 rel_transform[:3,-1] = rel_t
-
+hloc_movement_threshold = rospy.get_param("/hloc_loc/movement_threshold")
 opt=rospy.get_param("/hloc_loc/use_calib")
 localization_from = None
 class Camera():
@@ -79,13 +79,23 @@ def initialize_pose(req, front_fisheye_image, rear_fisheye_image, pose_pub, hloc
     st = rospy.Time.now()
     succ=True
     tf_listener = tf.TransformListener()
+    tf_listener.waitForTransform("base_footprint", "odom", rospy.Time(), rospy.Duration(1.0))            
     tf_listener.waitForTransform("base_footprint", "front_fisheye_camera_optical_frame", rospy.Time(), rospy.Duration(1.0))            
+    (t,q) = tf_listener.lookupTransform("odom", "base_footprint", rospy.Time())
+    position_before = np.asarray(t) 
     try:
         meshroom_compute.execute("/root/catkin_ws/src/ari_hloc/pipelines/ARI_localizer.mg", additional_params=(hloc_object.models,hloc_object.db_data, hloc_object.num_matching_pairs) )
     except:
         succ=False
     hloc_pose.header.stamp = st
     if succ:
+        tf_listener.waitForTransform("base_footprint", "odom", rospy.Time(), rospy.Duration(0.5))            
+        (t,q) = tf_listener.lookupTransform('odom', 'base_footprint', rospy.Time())
+        position_after = np.asarray(t)
+        if np.linalg.norm(position_after-position_before) > hloc_movement_threshold:
+            rospy.logwarn("ROBOT HAS MOVED. NOT UPADING TF BASED ON HLOC")
+            return True
+        
         loc_folder = "/root/catkin_ws/src/ari_hloc/pipelines/MeshroomCache/HlocLocalizer"
         cache_folder = os.path.join(loc_folder, os.listdir("/root/catkin_ws/src/ari_hloc/pipelines/MeshroomCache/HlocLocalizer")[0])
         loc_res_file = os.path.join(cache_folder, 'query_localization_results.txt')

third_party/Hierarchical-Localization/build_dataset.py

@@ -36,6 +36,7 @@ results = outputs / f'Aachen_hloc_superpoint+superglue_netvlad{args.num_loc}.txt
 
 
 class Hloc():
+    # torch.backends.cudnn.enabled = False
     def __init__(self, map_path):
         map_path = Path(map_path)        
         self.retrieval_conf = extract_features.confs['netvlad']

third_party/Hierarchical-Localization/hloc/utils/tools.py

-from torch._six import string_classes
+# from torch._six import string_classes
 import collections.abc as collections
 import torch
+string_classes = str
 
 
 def map_tensor(input_, func):

third_party/Hierarchical-Localization/requirements.txt

-torch==1.4.0
-torchvision==0.5.0 -f https://download.pytorch.org/whl/torch_stable.html
+torch
+torchvision
 numpy
 opencv-python
 tqdm