Formatting of the code + adding Beluga API to init, predict, update pose and covariance

Author: JesusSilvaUtrera

docker/images/humble/Dockerfile

@@ -21,9 +21,7 @@ RUN mkdir -p /tmp/ws/src \
 
 FROM ros:humble-ros-base-jammy AS openvdb-builder
 
-RUN sed -i 's|http://archive.ubuntu.com|http://security.ubuntu.com|g' /etc/apt/sources.list
-
-RUN apt-get update -y --fix-missing \
+RUN apt-get update \
   && apt-get install --no-install-recommends -y \
   git \
   libboost-all-dev \
@@ -43,21 +41,17 @@ RUN git clone -b v8.2.0 https://github.com/AcademySoftwareFoundation/openvdb.git
 # Build pcl_perception here to avoid installing boost again
 RUN mkdir -p /opt/perception/src
 
-RUN sed -i 's|http://archive.ubuntu.com|http://security.ubuntu.com|g' /etc/apt/sources.list
-
-RUN apt-get update -y --fix-missing \
+RUN apt-get update \
   && apt-get install --no-install-recommends -y \
   libeigen3-dev \
   libpcl-dev \
   && rm -rf /var/lib/apt/lists/*
 
 WORKDIR /opt/perception/src
 
-RUN sed -i 's|http://archive.ubuntu.com|http://security.ubuntu.com|g' /etc/apt/sources.list
-
 RUN git clone -b 2.6.1 https://github.com/ros-perception/perception_pcl.git \
   && cd .. \
-  && sudo apt-get update -y --fix-missing \
+  && sudo apt-get update \
   && . /opt/ros/humble/setup.sh \
   && rosdep update \
   && rosdep install -i -y --from-path src \
@@ -70,9 +64,7 @@ ENV SHELL=/bin/bash
 
 ENV DEBIAN_FRONTEND=noninteractive
 
-RUN sed -i 's|http://archive.ubuntu.com|http://security.ubuntu.com|g' /etc/apt/sources.list
-
-RUN apt-get update -y --fix-missing \
+RUN apt-get update \
   && apt-get install --no-install-recommends -y \
   ccache \
   curl \
@@ -87,10 +79,9 @@ RUN apt-get update -y --fix-missing \
   && rm -rf /var/lib/apt/lists/*
 
 ARG GZ_VERSION="harmonic"
-RUN sed -i 's|http://archive.ubuntu.com|http://security.ubuntu.com|g' /etc/apt/sources.list
 RUN curl https://packages.osrfoundation.org/gazebo.gpg --output /usr/share/keyrings/pkgs-osrf-archive-keyring.gpg \
   && echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/pkgs-osrf-archive-keyring.gpg] http://packages.osrfoundation.org/gazebo/ubuntu-stable $(lsb_release -cs) main" > /etc/apt/sources.list.d/gazebo-stable.list \
-  && apt-get update -y --fix-missing && apt-get install --no-install-recommends -y $(if [[ "${GZ_VERSION}" > "fortress" ]]; then echo gz; else echo ignition; fi)-${GZ_VERSION}
+  && apt-get update && apt-get install --no-install-recommends -y $(if [[ "${GZ_VERSION}" > "fortress" ]]; then echo gz; else echo ignition; fi)-${GZ_VERSION}
 
 RUN pip install pre-commit==2.20.0
 
@@ -158,7 +149,7 @@ COPY --from=openvdb-builder --chown=$USER:$GROUP /opt/perception/install/percept
 # Install project dependencies
 
 COPY --from=package_cacher --chown=$USER:$GROUP /tmp/ws/ $USER_WS/
-RUN sudo apt-get update -y --fix-missing \
+RUN sudo apt-get update \
   && . /opt/ros/humble/setup.sh \
   && rosdep update \
   && rosdep install -i -y --from-path src \
@@ -209,4 +200,4 @@ RUN cd /opt/openvdb/build \
 RUN sudo dpkg -r libopenvdb8.1 \
   libopenvdb-dev
 
-ENTRYPOINT ["fixuid", "-q", "/ros_entrypoint.sh", "/bin/bash"]
+ENTRYPOINT ["fixuid", "-q", "/ros_entrypoint.sh", "/bin/bash"]

localization/beluga_demo_mh_amcl/include/beluga_demo_mh_amcl/map_matcher.hpp

@@ -27,73 +27,76 @@
 #include <tf2/LinearMath/Transform.h>
 #include <tf2/transform_datatypes.h>
 
-namespace mh_amcl {
-
-/**
- * @brief Struct containing a transform and its associated weight
- */
-typedef struct {
-  float weight;
-  tf2::Transform transform;
-} TransformWeighted;
-
-/**
- * @brief Overload of the operator '<' to be able to compare TransformWeighted
- * structs
- */
-bool operator<(const TransformWeighted &tw1, const TransformWeighted &tw2);
-
-/**
- * MapMatcher class
- *
- * Performs the multi-resolution map matching process. It receives an occupancy
- * grid map and laser scan data to find transformations (poses) that align the
- * scan data with the map. It weights all potential transformations to select
- * the higher quality candidates.
- */
-class MapMatcher {
-public:
-  explicit MapMatcher(std::shared_ptr<const nav_msgs::msg::OccupancyGrid> map);
+namespace mh_amcl
+{
 
   /**
-   * @brief Get the candidate transformations from the laser information and the
-   * map
+   * @brief Struct containing a transform and its associated weight
    */
-  std::list<TransformWeighted>
-  get_matches(const sensor_msgs::msg::LaserScan &scan);
+  typedef struct
+  {
+    float weight;
+    tf2::Transform transform;
+  } TransformWeighted;
 
-protected:
-  static const int NUM_LEVEL_SCALE_COSTMAP = 4;
+  /**
+   * @brief Overload of the operator '<' to be able to compare TransformWeighted
+   * structs
+   */
+  bool operator<(const TransformWeighted &tw1, const TransformWeighted &tw2);
 
   /**
-   * @brief Reduce the resolution of the map to half to perform the
-   * multi-resolution map-matching
+   * MapMatcher class
+   *
+   * Performs the multi-resolution map matching process. It receives an occupancy
+   * grid map and laser scan data to find transformations (poses) that align the
+   * scan data with the map. It weights all potential transformations to select
+   * the higher quality candidates.
    */
-  std::shared_ptr<beluga_ros::OccupancyGrid>
-  half_scale(std::shared_ptr<beluga_ros::OccupancyGrid> costmap_in);
+  class MapMatcher
+  {
+  public:
+    explicit MapMatcher(std::shared_ptr<const nav_msgs::msg::OccupancyGrid> map);
+
+    /**
+     * @brief Get the candidate transformations from the laser information and the
+     * map
+     */
+    std::list<TransformWeighted>
+    get_matches(const sensor_msgs::msg::LaserScan &scan);
+
+  protected:
+    static const int NUM_LEVEL_SCALE_COSTMAP = 4;
+
+    /**
+     * @brief Reduce the resolution of the map to half to perform the
+     * multi-resolution map-matching
+     */
+    std::shared_ptr<beluga_ros::OccupancyGrid>
+    half_scale(std::shared_ptr<beluga_ros::OccupancyGrid> costmap_in);
+
+    /**
+     * @brief Transform the LaserScan message in a vector of 3D points
+     */
+    std::vector<tf2::Vector3>
+    laser2points(const sensor_msgs::msg::LaserScan &scan);
+
+    // Auxiliary methods to get the matches between the laser data and the map
+    std::list<TransformWeighted>
+    get_matches(int scale, const std::vector<tf2::Vector3> &scan, float min_x,
+                float min_y, float max_y, float max_x);
+    float match(int scale, std::shared_ptr<beluga_ros::OccupancyGrid> costmap,
+                const std::vector<tf2::Vector3> &scan, tf2::Transform &transform);
+
+    // Current costmaps
+    std::vector<std::shared_ptr<beluga_ros::OccupancyGrid>> costmaps_;
+  };
 
   /**
-   * @brief Transform the LaserScan message in a vector of 3D points
+   * @brief wrapper to transform the costmap to ROS Occupancy grid message
    */
-  std::vector<tf2::Vector3>
-  laser2points(const sensor_msgs::msg::LaserScan &scan);
-
-  // Auxiliary methods to get the matches between the laser data and the map
-  std::list<TransformWeighted>
-  get_matches(int scale, const std::vector<tf2::Vector3> &scan, float min_x,
-              float min_y, float max_y, float max_x);
-  float match(int scale, std::shared_ptr<beluga_ros::OccupancyGrid> costmap,
-              const std::vector<tf2::Vector3> &scan, tf2::Transform &transform);
-
-  // Current costmaps
-  std::vector<std::shared_ptr<beluga_ros::OccupancyGrid>> costmaps_;
-};
-
-/**
- * @brief wrapper to transform the costmap to ROS Occupancy grid message
- */
-nav_msgs::msg::OccupancyGrid
-toMsg(std::shared_ptr<beluga_ros::OccupancyGrid> costmap);
+  nav_msgs::msg::OccupancyGrid
+  toMsg(std::shared_ptr<beluga_ros::OccupancyGrid> costmap);
 
 } // namespace mh_amcl