Merge pull request #78 from CMU-cabot/muratams/fix-mapping-with-gnss

Update mapping post-process to use GNSS fix data

cabot_mf_localization/script/mapping_post_process.sh

@@ -26,12 +26,16 @@ QUIT_WHEN_ROSBAG_FINISH=${QUIT_WHEN_ROSBAG_FINISH:-true}
 PLAYBAG_RATE_CARTOGRAPHER=${PLAYBAG_RATE_CARTOGRAPHER:-1.0}
 PLAYBAG_RATE_PC2_CONVERT=${PLAYBAG_RATE_PC2_CONVERT:-1.0}
 LIDAR_MODEL=${LIDAR_MODEL:-VLP16}
+MAPPING_USE_GNSS=${MAPPING_USE_GNSS:-false}
 
 gazebo=${PROCESS_GAZEBO_MAPPING:-0}
 
 # topic
 points2_topic='/velodyne_points'
 imu_topic=/imu/data
+fix_topic=/ublox/fix
+fix_velocity_topic=/ublox/fix_velocity
+fix_filtered_topic=/ublox/fix_filtered
 convert_points=true  # VLP16 (default LIDAR_MODEL)
 
 if [[ $gazebo -eq 1 ]]; then
@@ -43,6 +47,11 @@ fi
 
 echo "LIDAR_MODEL=$LIDAR_MODEL"
 
+function red {
+    echo -en "\033[31m"  ## red
+    echo $@
+    echo -en "\033[0m"  ## reset color
+}
 function blue {
     echo -en "\033[36m"  ## blue
     echo $@
@@ -73,6 +82,8 @@ if [[ ! -e $WORKDIR/${BAG_FILENAME}.carto-converted ]]; then
     ros2 launch mf_localization_mapping convert_rosbag_for_cartographer.launch.py \
 	      points2:=${points2_topic} \
 	      imu:=${imu_topic} \
+	      fix:=${fix_topic} \
+	      fix_velocity:=${fix_velocity_topic} \
 	      rate:=${PLAYBAG_RATE_PC2_CONVERT} \
 	      convert_points:=$convert_points \
 	      bag_filename:=$WORKDIR/${BAG_FILENAME}
@@ -90,19 +101,34 @@ if [[ ! -e $WORKDIR/${BAG_FILENAME}.carto-converted.loc.samples.json ]] || [[ !
 	      rate:=${PLAYBAG_RATE_CARTOGRAPHER} \
 	      quit_when_rosbag_finish:=${QUIT_WHEN_ROSBAG_FINISH} \
 	      bag_filename:=$WORKDIR/${BAG_FILENAME}.carto-converted"
+
     if [ $cabot_model != "" ]; then
         com="$com cabot_model:=$cabot_model"
     else
         com="$com robot:=$robot"
     fi
+
+    # outdoor mapping
+    if [ "$MAPPING_USE_GNSS" = true ]; then
+        com="$com \
+            save_pose:=true \
+            fix:=$fix_filtered_topic \
+            configuration_basename:=cartographer_2d_mapping_gnss.lua \
+            interpolate_samples_by_trajectory:=true \
+            "
+    fi
+
+    # copy output to a file
+    com="$com | tee $WORKDIR/${BAG_FILENAME}.carto-converted.log"
+
     echo $com
     eval $com
 else
     blue "skipping $WORKDIR/${BAG_FILENAME}.carto-converted.loc.samples.json"
     blue "skipping $WORKDIR/${BAG_FILENAME}.carto-converted.pbstream"
 fi
 
-
+# convert pbstream to pgm
 if [[ ! -e $WORKDIR/${BAG_FILENAME}.carto-converted.pgm ]]; then
     ros2 run cartographer_ros cartographer_pbstream_to_ros_map \
 	   -pbstream_filename $WORKDIR/${BAG_FILENAME}.carto-converted.pbstream \
@@ -129,4 +155,30 @@ if [[ ! -e $WORKDIR/${BAG_FILENAME}.carto-converted.info.txt ]]; then
         | tee -a $WORKDIR/${BAG_FILENAME}.carto-converted.info.txt
 else
     blue "skipping $WORKDIR/${BAG_FILENAME}.carto-converted.info.txt"
+fi
+
+# post process for mapping with gnss data
+if [ "$MAPPING_USE_GNSS" = true ]; then
+    # extract option info from log
+    if [[ ! -e $WORKDIR/${BAG_FILENAME}.carto-converted.node-options.yaml ]]; then
+        ros2 run mf_localization_mapping extract_node_options.py \
+            -i $WORKDIR/${BAG_FILENAME}.carto-converted.log \
+            -o $WORKDIR/${BAG_FILENAME}.carto-converted.node-options.yaml
+    else
+        blue "skipping $WORKDIR/${BAG_FILENAME}.carto-converted.node-options.yaml"
+    fi
+
+    # export gnss from bag
+    if [[ ! -e $WORKDIR/${BAG_FILENAME}.carto-converted.gnss.csv ]]; then
+        ros2 run mf_localization_mapping export_gnss_fix.py \
+            -i $WORKDIR/${BAG_FILENAME}.carto-converted \
+            -o $WORKDIR/${BAG_FILENAME}.carto-converted.gnss.csv
+    else
+        blue "skipping $WORKDIR/${BAG_FILENAME}.carto-converted.gnss.csv"
+    fi
+
+    # calculate error between trajectory and gnss
+    ros2 run mf_localization_mapping compare_trajectory_and_gnss.py \
+        --trajectory $WORKDIR/${BAG_FILENAME}.carto-converted.trajectory.csv \
+        --gnss $WORKDIR/${BAG_FILENAME}.carto-converted.gnss.csv --plot
 fi

docker/localization/Dockerfile.debug

@@ -0,0 +1,127 @@
+ARG PREFIX=cabot
+ARG FROM_IMAGE=${PREFIX}__jammy-humble-custom-mesa
+
+FROM $FROM_IMAGE as build
+
+ARG TZ=UTC
+ENV TZ=$TZ
+RUN sudo ln -snf /usr/share/zoneinfo/$TZ /etc/localtime && echo $TZ | sudo tee /etc/timezone
+
+# TODO replace with rosdep install
+RUN apt update && \
+    apt install -y --no-install-recommends \
+    imagemagick \
+    libpcap-dev \
+    nodejs \
+    npm \
+    python-is-python3 \
+    python3-pip \
+    python3-matplotlib \
+    python3-sklearn \
+    ros-${ROS_DISTRO}-diagnostic-updater \
+    ros-${ROS_DISTRO}-gazebo-msgs \
+    ros-${ROS_DISTRO}-realsense2-description \
+    ros-${ROS_DISTRO}-rosbridge-suite \
+    ros-${ROS_DISTRO}-tf-transformations \
+    ros-${ROS_DISTRO}-tf2-geometry-msgs \
+    ros-${ROS_DISTRO}-velodyne-description \
+    ros-${ROS_DISTRO}-rmw-cyclonedds-cpp \
+    && \
+    apt clean && \
+    rm -rf /var/lib/apt/lists/*
+
+RUN pip install --no-cache-dir \
+    requests \
+    certifi \
+    orjson==3.10.6 \
+    pyproj \
+    rosbags
+
+## install packages for ublox node and RTKLIB
+RUN apt update && \
+    apt-get install -y \
+    libasio-dev \
+    ros-${ROS_DISTRO}-rtcm-msgs \
+    ros-${ROS_DISTRO}-nmea-msgs \
+    gfortran \
+    && \
+    rm -rf /var/lib/apt/lists/*
+
+## install RTKLIB
+WORKDIR /opt
+COPY ./RTKLIB/ ./RTKLIB/
+RUN (cd RTKLIB/lib/iers/gcc/; make) && \
+    (cd RTKLIB/app/consapp; make; make install) && \
+    rm -rf RTKLIB
+
+# TODO merge
+RUN pip install --no-cache-dir \
+	transforms3d==0.4.1 \
+	pyserial
+RUN apt update && \
+    apt-get install -y \
+    ros-${ROS_DISTRO}-navigation2 \
+    ros-${ROS_DISTRO}-pointcloud-to-laserscan \
+    xterm \
+    && \
+    rm -rf /var/lib/apt/lists/*
+# end TODO
+
+# start cartographer_ws setup
+ENV CARTO_WS=/opt/cartographer_ws
+
+RUN mkdir -p $CARTO_WS/src
+COPY ./bluespace_ai_xsens_ros_mti_driver /opt/cartographer_ws/src/bluespace_ai_xsens_ros_mti_driver
+COPY ./ublox /opt/cartographer_ws/src/ublox
+COPY ./ntrip_client /opt/cartographer_ws/src/ntrip_client
+
+WORKDIR /opt/cartographer_ws/
+
+# build packages in /opt/cartographer_ws other than cartographer
+RUN . /opt/custom_ws/install/setup.sh && \
+	colcon build --cmake-args -DCMAKE_BUILD_TYPE=RelWithDebInfo
+
+# start build cartographer
+COPY ./cartographer /opt/cartographer_ws/src/cartographer
+COPY ./cartographer_ros /opt/cartographer_ws/src/cartographer_ros
+
+RUN apt update && rosdep update && rosdep install -q -y \
+--from-paths src \
+	--ignore-src
+
+RUN src/cartographer/scripts/install_debs_cmake.sh && rm -rf /var/lib/apt/lists/*
+RUN src/cartographer/scripts/install_abseil.sh && rm -rf /var/lib/apt/lists/*
+
+RUN . /opt/custom_ws/install/setup.sh && \
+	colcon build --cmake-args -DCMAKE_BUILD_TYPE=RelWithDebInfo
+# end build cartographer
+
+ARG ROS_DISTRO
+RUN sed -i 's:custom_ws:cartographer_ws:' /ros_entrypoint.sh
+
+# end cartographer_ws setup
+
+WORKDIR /opt
+
+COPY ./launch.sh /
+COPY ./launch_rtklib.sh /
+
+# setup for display
+ENV USERNAME developer
+
+### replace 1000 with your user/group id
+ARG UID=1000
+RUN useradd -m $USERNAME && \
+    echo "$USERNAME:$USERNAME" | chpasswd && \
+    usermod --shell /bin/bash $USERNAME && \
+    usermod -aG sudo $USERNAME && \
+    mkdir -p /etc/sudoers.d/ && \
+    echo "$USERNAME ALL=(ALL) NOPASSWD:ALL" >> /etc/sudoers.d/$USERNAME && \
+    chmod 0440 /etc/sudoers.d/$USERNAME && \
+    usermod  --uid $UID $USERNAME && \
+    groupmod --gid $UID $USERNAME
+
+USER $USERNAME
+
+ENV HOME /home/$USERNAME
+WORKDIR $HOME/loc_ws

mf_localization/CMakeLists.txt

@@ -56,6 +56,9 @@ install(PROGRAMS
   script/multi_floor_wireless_rss_localizer.py
   script/tf2_beacons_listener.py
   script/tf2_listener.py
+  script/tracked_pose_listener.py
+  script/trajectory_based_interpolator.py
+  script/trajectory_recorder.py
   script/trajectory_restarter.py
   script/str2str_node.py
   script/ublox_converter.py

mf_localization/script/fix_filter.py

@@ -29,22 +29,23 @@
 
 class FixFilterNode:
     def __init__(self, node, status_threshold, stdev_threshold):
-        self.node = node
-        self.status_threshold = status_threshold
-        self.stdev_threshold = stdev_threshold
-        self.fix_sub = self.node.create_subscription(NavSatFix, "fix", self.fix_callback, 10)
-        self.fix_pub = self.node.create_publisher(NavSatFix, "fix_filtered", 10)
+        self._node = node
+        self._clock = node.get_clock()
+        self._status_threshold = status_threshold
+        self._stdev_threshold = stdev_threshold
+        self._fix_sub = self._node.create_subscription(NavSatFix, "fix", self.fix_callback, 10)
+        self._fix_pub = self._node.create_publisher(NavSatFix, "fix_filtered", 10)
 
     def fix_callback(self, msg: NavSatFix):
         navsat_status = msg.status
         status = navsat_status.status
         position_covariance = np.reshape(msg.position_covariance, (3, 3))
         stdev = np.sqrt(position_covariance[0, 0])
 
-        if self.status_threshold <= status and stdev <= self.stdev_threshold:
-            msg.header.stamp = self.clock.now()
+        if self._status_threshold <= status and stdev <= self._stdev_threshold:
+            msg.header.stamp = self._clock.now().to_msg()
             msg.altitude = 0.0
-            self.fix_pub.publish(msg)
+            self._fix_pub.publish(msg)
 
 
 def main():
@@ -55,10 +56,7 @@ def main():
 
     FixFilterNode(node, status_threshold, stdev_threshold)
 
-    try:
-        rclpy.spin(node)
-    except:  # noqa: E722
-        pass
+    rclpy.spin(node)
 
 
 if __name__ == "__main__":

mf_localization/script/tf2_beacons_listener.py

@@ -30,6 +30,9 @@
 import tf2_ros
 from std_msgs.msg import String
 
+from trajectory_based_interpolator import TrajectoryBasedInterpolator
+from trajectory_recorder import TrajectoryRecorder
+
 
 class BeaconMapper:
     def __init__(self, node, save_empty_beacon_sample, data_inverval=1.2, position_interval=0.5, verbose=False):
@@ -107,6 +110,8 @@ def set_current_position(self, position):
                 if d2d < self._position_interval:
                     return
 
+            # convert timestamp to float because timestamp object is not JSON serializable
+            timestamp_sec = timestamp.nanoseconds * 1.0e-9
             fp_data = {
                 "information": {
                     "x": t.transform.translation.x,
@@ -121,15 +126,15 @@ def set_current_position(self, position):
                     }
                 },
                 "data": {
-                    "timestamp": timestamp,
+                    "timestamp": timestamp_sec,
                     "beacons": []  # empty list
                 }
             }
             self._fingerprints.append(fp_data)
             self._previous_fingerprint_time = timestamp
             self._previous_fingerprint_position = self._current_position
             if self._verbose:
-                self.logger.info(F"dummy fingerprint data created at t={timestamp}, x={t.transform.translation.x}, y={t.transform.translation.y}")
+                self.logger.info(F"dummy fingerprint data created at t={timestamp_sec}, x={t.transform.translation.x}, y={t.transform.translation.y}")
 
 
 def main():
@@ -149,13 +154,21 @@ def main():
     fingerprint_data_interval = node.declare_parameter("fingerprint_data_interval", 1.2).value  # should be larger than 1.0 s because beacon data interval is about 1.0 s.
     fingerprint_position_interval = node.declare_parameter("fingerprint_position_interval", 0.5).value  # to prevent the mapper from creating dummy fingerprint data at the same position
 
+    # use trajectory recorder extention
+    output_trajectory = node.declare_parameter("output_trajectory", '').value
+    interpolate_by_trajectory = node.declare_parameter("interpolate_by_trajectory", False).value
+    trajectory_recoder = None  # default
+
     mapper = BeaconMapper(node,
                           save_empty_beacon_sample=save_empty_beacon_sample,
                           data_inverval=fingerprint_data_interval,
                           position_interval=fingerprint_position_interval,
                           verbose=verbose
                           )
 
+    if output_trajectory != '' or interpolate_by_trajectory:
+        trajectory_recoder = TrajectoryRecorder(node, output_trajectory)
+
     subscribers = []
     for sub_topic in sub_topics:
         logger.info("set " + sub_topic + " subscriber.")
@@ -182,11 +195,23 @@ def transform_check_loop():
     except KeyboardInterrupt:
         node.get_logger().info("caught KeyboardInterrupt")
 
+    # trajectory recorder post process
+    if trajectory_recoder is not None:
+        trajectory_recoder.on_shutdown()
+
     # save to a file after node shutdown
     if output and 0 < len(mapper._fingerprints):
         node.get_logger().info("output fingerprint data before exiting")
+        if interpolate_by_trajectory:
+            trajectory = trajectory_recoder.get_trajectory()
+            interpolator = TrajectoryBasedInterpolator(trajectory)
+            samples = interpolator.interpolate_samples(mapper._fingerprints)
+            node.get_logger().info(F"Reconstructed fingerprint poses from trajectory data. #samples={len(mapper._fingerprints)} -> #samples_converted={len(samples)}")
+        else:
+            samples = mapper._fingerprints
+
         with open(output, "w") as f:
-            json.dump(mapper._fingerprints, f)
+            json.dump(samples, f)
 
 
 if __name__ == "__main__":