Added LIO and local goalpose support

Dockerfile

@@ -0,0 +1,39 @@
+# Start from ROS Jazzy + CUDA base
+FROM dtcpronto/ros-jazzy:cuda
+
+USER root
+
+RUN apt-get update && apt-get install -y --no-install-recommends \
+    vim \
+    tmux \
+    cmake \
+    gcc \
+    g++ \
+    git \
+    build-essential \
+    sudo \
+    wget \
+    curl \
+    zip \
+    unzip \
+    ros-jazzy-gtsam \
+    ros-jazzy-gps-msgs \
+    python3-colcon-common-extensions \
+    libgoogle-glog-dev \
+    && rm -rf /var/lib/apt/lists/*
+
+# Switch to dtc user
+USER dtc
+WORKDIR /home/dtc/ws
+
+# Clone glider
+COPY --chown=dtc:dtc ./glider /home/dtc/ws/src/glider
+
+# Build workspace
+RUN /bin/bash -c "source /opt/ros/jazzy/setup.bash && \
+    colcon build --symlink-install"
+
+COPY --chown=dtc:dtc ./entrypoint.bash /home/dtc/entrypoint.bash
+RUN chmod +x /home/dtc/entrypoint.bash
+
+ENTRYPOINT ["/home/dtc/entrypoint.bash"]

build.bash

@@ -0,0 +1,2 @@
+#!/bin/bash
+docker build --build-arg user_id=$(id -u) --build-arg USER=$(whoami) --build-arg NAME=glider --rm -t dtc-jackal-`hostname`:glider .

entrypoint.bash

@@ -0,0 +1,22 @@
+#!/bin/bash
+
+source /opt/ros/jazzy/setup.bash
+source /home/dtc/ws/install/setup.bash
+
+if [ "$RMW_IMPLEMENTATION" = "rmw_zenoh_cpp" ]; then
+    echo "[GLIDER] Starting Zenoh router..."
+    ros2 run rmw_zenoh_cpp rmw_zenohd > /tmp/zenoh_router.log 2>&1 &
+    sleep 2
+fi
+
+if [ "$RUN" = "true" ]; then
+    echo "[GLIDER] Starting foxglove_bridge..."
+    nohup ros2 run foxglove_bridge foxglove_bridge --ros-args -p address:='0.0.0.0' -p port:=8765 > /dev/null 2>&1 &
+    sleep 3
+    echo "[GLIDER] Launching glider..."
+    ros2 launch glider glider-node.launch.py
+else
+    echo "[GLIDER] RUN=false, keeping container alive..."
+fi
+
+exec "$@"

glider/CMakeLists.txt

@@ -44,6 +44,8 @@ if (BUILD_ROS)
   find_package(std_msgs REQUIRED)
   find_package(nav_msgs REQUIRED)
   find_package(gps_msgs REQUIRED)
+  find_package(geometry_msgs REQUIRED)
+  find_package(tf2_ros REQUIRED)
 
   set(node_plugins "")
 endif()
@@ -97,6 +99,7 @@ if (BUILD_ROS)
     geometry_msgs
     nav_msgs
     gps_msgs
+    tf2_ros
   )
 
   add_executable(${PROJECT_NAME}_node

glider/config/glider-params.yaml

@@ -9,9 +9,12 @@ imu:
   frame: "enu"
 gps: 
   covariance: 2.0
+odom:
+  covariance: 0.1
 dgps:
   enable: true
   covariance: 0.03
+  rejection_limit: 1.7
 dgpsfm:
   enable: false
   integration_threshold: 1.0

glider/config/ros-params.yaml

@@ -2,10 +2,15 @@ glider_node:
   ros__parameters:
     publishers:
       rate: 0.0
-      nav_sat_fix: false
+      nav_sat_fix: true
+      utm_zone: "18S"   # Pennovation : 18S, College station : 14R
+      map_frame: "map"
+      odom_frame: "rko_odom"
+      base_link_frame: "base_link"
       viz:
         use: true
-        origin_easting: 753912.0063845584
+        origin_easting: 753912.0063845584   # These needs to be changed based on the location
         origin_northing: 3385461.6073698294
     subscribers:
-      use_odom: false
+      use_odom: true
+      dgps_topic: "/dgps/antenna1/fix"

glider/include/glider/core/factor_manager.hpp

@@ -62,6 +62,9 @@ class FactorManager
          * factor manager
          * @param params: the parameters loaded from the yaml file*/
         FactorManager(const Parameters& params);    
+        /*! @brief initializes all parameters in the factor manager 
+         * @param params: the parameters loaded from the yaml file*/
+        void initialize(const Parameters& params);
 
         // state predictors
         /*! @brief calls the pim predict method
@@ -79,20 +82,26 @@ class FactorManager
         /*! @brief adds the gps measurement and pim to the factor graph
          *  @param timestamp: time of the gps measurement
          *  @param gps: GPS measurement in the UTM frame */
-        void addGpsFactor(int64_t timestamp, const Eigen::Vector3d& gps);
+        void addGpsFactor(int64_t timestamp, const Eigen::Vector3d& gps, const double sigma = 0.0);
         /*! @brief adds the gps measurement and a heading from dgps
          *  @param timestamp: time of the gps measurement 
          *  @param gps: GPS measurement in the UTM frame
          *  @param heading: heading from dgpsfm in the ENU frame 
          *  @param fuse: whether or not to add the heading measurement
-         *  to the factor graph */
-        void addGpsFactor(int64_t timestamp, const Eigen::Vector3d& gps, const double& heading, const bool fuse);
+         *  to the factor graph 
+         *  @param sigma: the standard deviation of the gps measurement, if 0 use param */
+        void addGpsFactor(int64_t timestamp, const Eigen::Vector3d& gps, const double& heading, const bool fuse, const double sigma = 0.0);
         /*! @brief adds the imu measurements to the pim and saves the orientation
          *  @param timestamp: time of the imu measurement 
          *  @param accel: the accelerometer reading
          *  @param gyro: gyroscopre reading
          *  @param orient: orientation in quaternion (w,x,y,z) format */
         void addImuFactor(int64_t timestamp, const Eigen::Vector3d& accel, const Eigen::Vector3d& gyro, const Eigen::Vector4d& orient);
+        /*! @brief adds an odometry measurement 
+         *  @param timestamp: time of the odometry
+         *  @param odom: estimated odometry pose 
+         *  @return true if a graph node was created */
+        bool addOdomFactor(int64_t timestamp, const Eigen::Isometry3d& odom);
         /*! @brief adds a landmark factor for an estimated utm point and covariance 
          *  @param timestamp: time of the landmark measurements
          *  @param landmark_id: a unique id for the landmark
@@ -127,6 +136,16 @@ class FactorManager
         /*! @brief gets the key index 
          *  @return the current key index */
         gtsam::Key getKeyIndex() const;
+        /*! @brief gets the gps offset
+         *  @return the 3D gps offset */
+        Eigen::Vector3d getGpsOffset() const;
+        /*! @brief checks if the gps offset has been initialized
+         *  @return true if the gps offset has been initialized else false */
+        bool isGpsOffsetInitialized() const;
+
+        /*! @brief gets the current parameters
+         *  @return the current parameters */
+        const Parameters& params() const { return params_; }
 
     private: 
         /*! @brief handles the optimization call with the specified 
@@ -151,6 +170,8 @@ class FactorManager
         // @brief a mutex to use accross function that access the pim 
         // as the pim could be accessd by multiple threads
         static std::mutex mutex_;
+        // @brief a mutex to protect the factor graph and its variables
+        mutable std::mutex graph_mutex_;
 
         // parameters
         // @brief parameters for the isam2 optimizer  
@@ -186,6 +207,8 @@ class FactorManager
         gtsam::noiseModel::Base::shared_ptr orient_noise_;
         // @brief noise in the heading estimate of differential gps
         gtsam::noiseModel::Base::shared_ptr dgpsfm_noise_;
+        // @brief noise for the odometry constraints
+        gtsam::noiseModel::Diagonal::shared_ptr odom_noise_;
 
         // factor graph
         // @brief tracks the number of times the optimizer has been called
@@ -222,6 +245,17 @@ class FactorManager
         bool imu_initialized_;
         // @param tracks if a gps measurement has been received
         bool gps_initialized_;
+        // @param tracks if an odom measurement has been received
+        bool odom_initialized_;
+        // @param true if the first graph node originated from Odometry (LIO), making it a local origin
+        bool using_local_origin_;
+        bool gps_offset_initialized_;
+        Eigen::Vector3d gps_offset_;
+        double last_node_time_;
+        // @param previous odometry measurement for relative constraints
+        Eigen::Isometry3d last_odom_meas_;
+        // @param accumulated odometry transform between nodes
+        Eigen::Isometry3d accumulated_odom_delta_;
 
         // landmark variables
         std::unordered_map<size_t, Eigen::Matrix3d> landmark_info_;

glider/include/glider/core/glider.hpp

@@ -34,18 +34,40 @@ class Glider
          *  @param gps: gps measurement in (lat, lon, alt) format, 
          *  should be in degree decimal and altitude in meters. Altitude
          *  frame does not matter */
-        void addGps(int64_t timestamp, Eigen::Vector3d& gps);
-        void addGpsWithHeading(int64_t timestamp, Eigen::Vector3d& gps);
-        void addGpsWithHeading(int64_t timestamp, Eigen::Vector3d& gps, Eigen::Vector2d& heading);
+        void addGps(int64_t timestamp, Eigen::Vector3d& gps, const double sigma = 0.0);
+        /*! @brief adds the gps measurement and heading info to the factor 
+         * graph 
+         * @param timestamp: time of measurement 
+         * @param gps: lat, lon, alt coordinates 
+         * @param heading: track, error track 
+         * @param sigma: standard deviation of the gps position measurement */
+        void addGpsWithHeading(int64_t timestamp, Eigen::Vector3d& gps, Eigen::Vector2d& heading, const double sigma = 0.0);
+        /*! @brief adds the gps measurement and calculates a heading based on previous 
+         *  GPS measurements
+         * @param timestamp: time of measurement
+         * @param gps: lat, lon, alt coordinates 
+         * @param sigma: standard deviation of the gps position measurement */
+        void addGpsWithHeading(int64_t timestamp, Eigen::Vector3d& gps, const double sigma = 0.0);
         /*! @brief converts the imu measurements into the ENU frame if
          *  they are not in that frame already.
          *  @param timestamp: time the imu measurement was taken
          *  @param accel: acceleration measurement in the imu's frame
          *  @param gyro: gyroscope measurement in the imu's frame
          *  @param quat: the orientation measurement in the imu's frame */
         void addImu(int64_t timestamp, Eigen::Vector3d& accel, Eigen::Vector3d& gyro, Eigen::Vector4d& quat);
+        bool addOdom(int64_t timestamp, const Eigen::Isometry3d& pose);
         void addLandmark(int64_t timestamp, size_t lid, const Eigen::Vector3d& utm, const Eigen::Matrix3d& cov);
         PointWithCovariance getLandmark(size_t lid);
+        Eigen::Vector3d getGpsOffset() const;
+        /*! @brief gets the auto-detected UTM zone
+         *  @return the UTM zone string */
+        std::string getUtmZone() const { return utm_zone_; }
+
+        const Parameters& params() const { return factor_manager_.params(); }
+
+        bool isGpsInitialized() const { return factor_manager_.isGpsInitialized(); }
+        bool isGpsOffsetInitialized() const { return factor_manager_.isGpsOffsetInitialized(); }
+        bool isSystemInitialized() const { return factor_manager_.isSystemInitialized(); }
 
 
         /*! @brief calls the factor manager to interpolate between GPS 
@@ -92,5 +114,6 @@ class Glider
         // @brief save the state estimate from 
         // the optimizer
         OdometryWithCovariance current_odom_;
+        std::string utm_zone_;
 };
 }

glider/include/glider/core/odometry.hpp

@@ -102,6 +102,10 @@ class Odometry
          *  @return true if odometry is initialized otherwise false */
         bool isInitialized() const;
 
+        /*! @brief check if the gps offset is initialized
+         *  @return true if it is otherwise false */
+        bool isGpsOffsetInitialized() const;
+
         /*! @brief get the latitude of the current position
          *  @param zone: the utm zone ex "18S"
          *  @return the latitude in degrees decimal from the UTM position */
@@ -114,7 +118,7 @@ class Odometry
          *  @param zone: the utm zone, ex "18S"
          *  @return latitude and longitude in degrees decimal as a pair 
          *  where lat is first and lon is second */
-        std::pair<double, double> getLatLon(const char* zone);
+        std::pair<double, double> getLatLon(const char* zone, const Eigen::Vector3d& offset = Eigen::Vector3d::Zero());
         /*! @brief get the timestamp of the odometry
          *  @return nanosec time in integer format */
         int64_t getTimestamp() const;
@@ -124,6 +128,11 @@ class Odometry
          *  otherwise false */
         void setInitializedStatus(bool init);
 
+        /*! @brief set the gps offset initalization status
+         *  @param init: true if you want the gps offset to be initialized
+         *  otherwise false */
+        void setGpsOffsetInitialized(bool init);
+
     protected:
         /*! @brief a helper function to convert gtsam Pose3 to a pair
          *  of Eigen objects 
@@ -157,5 +166,7 @@ class Odometry
         int64_t timestamp_;
         // @brief is this initialized, default to false
         bool initialized_{false};
+        // @brief is the gps offset initialized, default to false
+        bool is_gps_offset_initialized_{false};
 };
 } // namespace glider

glider/include/glider/utils/parameters.hpp

@@ -46,6 +46,8 @@ struct Parameters
     // @brief covariance of the GPS position estimate
     // TODO make this gps_cov to match 
     double gps_noise;
+    // @brief covariance of the odometry position estimate
+    double odom_cov;
 
     // @brief gravity as read from your IMU
     double gravity;
@@ -83,6 +85,7 @@ struct Parameters
 
     bool use_dgps;
     double dgps_cov;
+    double dgps_rejection_limit;
 
     // @brief translation from the GPS to the IMU
     Eigen::Vector3d t_imu_gps;

glider/include/ros/conversions.hpp

@@ -37,10 +37,10 @@ class Conversions
         static Output eigenToRos(const Input& vec);
 
         template<typename Output>
-        static Output odomToRos(Glider::Odometry& odom, const char* zone = nullptr);
+        static Output odomToRos(Glider::Odometry& odom, std::string frame_id, const char* zone = nullptr, const Eigen::Vector3d& offset = Eigen::Vector3d::Zero());
 
         template<typename Output>
-        static Output odomToRos(Glider::OdometryWithCovariance& odom_wc, const char* zone = nullptr);
+        static Output odomToRos(Glider::OdometryWithCovariance& odom_wc, std::string frame_id, const char* zone = nullptr, const Eigen::Vector3d& offset = Eigen::Vector3d::Zero());
 
         template<typename T>
         static void addCovariance(const Glider::OdometryWithCovariance& odom_wc, T& msg);