diff --git a/Dockerfile b/Dockerfile new file mode 100644 index 0000000..6a117f8 --- /dev/null +++ b/Dockerfile @@ -0,0 +1,40 @@ +# 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 +COPY --chown=dtc:dtc ./dgps_msgs /home/dtc/ws/src/dgps_msgs + +# 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"] diff --git a/README.md b/README.md index 054bc48..72937ea 100644 --- a/README.md +++ b/README.md @@ -3,8 +3,8 @@ ![Jazzy CI](https://github.com/KumarRobotics/glider/actions/workflows/jazzy-ci.yml/badge.svg?branch=ros2) -Glider is a G-INS system built on [GTSAM](https://github.com/borglab/gtsam). It currently takes in GPS and 9-DOF IMU and provides a full -state estimate up to the rate of you IMU. Glider is designed to be configured to your system. +Glider is a G-INS system built on [GTSAM](https://github.com/borglab/gtsam). It accepts GPS, a 9-DOF IMU, and optional local odometry, and provides a full +state estimate at up to the IMU rate. Glider is designed to be configured for a specific sensor suite. ## Building Glider To run glider you can use the provided docker images, ROS2 jazzy and humble are both supported, simply use the `build.bash` and `run.bash` files. You can mounted volumes in the run files if necessary. If you want to inlcude this in another ROS2 workspace, you may need to install the following dependencies: @@ -18,7 +18,7 @@ colcon build --packages-select glider If you only want the api you can build with: ``` cmake -S . -B build -DBUILD_ROS=OFF -cmake --build build +cmake --build build ``` ## Running Glider @@ -40,7 +40,13 @@ the parameters mean: - `publishers.viz.use`: if true will publish an `Odometry` topic for visualization centered around the origin. - `publishers.viz.origin_easting`: the easting value you want to viz odometry to center around. - `publishers.viz.origin_northing`: the northing value you want the viz odometry to center around. - - `subscribers.use_odom`: Still under development. + - `subscribers.imu_topic`, `gps_topic`, `dgps_topic`, `odom_topic`: input topic names. + - `subscribers.use_gps`, `use_dgps`, `use_odom`: enable each aiding source. Avoid enabling both GPS inputs when they represent the same receiver fix. + - `subscribers.gps_rejection_variance`: hard GPS variance ceiling in m². Measurements above it are rejected while local odometry remains active. + +The checked-in ROS parameters use a VectorNav IMU on `/vectornav/imu`, LIO on +`/rko_lio/odometry`, and the ENU DGPS fix on `/sept/enu/dfix`. LIO remains active +while GPS is absent or rejected. ## Glider Setup You can configure glider itself in `config/glider-params.yaml`, this is where you can specify the parameters for the factor graph. Here's more detail on each parameter: @@ -61,19 +67,18 @@ You can configure glider itself in `config/glider-params.yaml`, this is where yo - `logging.stdout`: output log statements to terminal in addition to the logfile - `optimizer.smooth`: if true the factor graph will optimize using a fixed lag smoother, otherwise it will use iSAM2. - `optimizer.lag_time`: period of time the fixed lag smoother should look at in seconds. - - `gps_to_imu`: the relative transformation from your gps to your imu in the FLU frame. + - `extrinsics`: the single hardware-calibration section. Sensor poses are entered relative to the LiDAR using ROS FLU axes (+X forward, +Y left, +Z up), metres, and XYZ roll/pitch/yaw degrees. Glider converts them to its body frame internally. ### Building and Running Unit Tests -We use GTest to run unit tests. You can build the tests with -``` +We use GTest to run unit tests. You can build the tests with +``` cd glider cmake -S . -B build -DBUILD_TESTS=ON cmake --build build ``` and run with: ``` -cd build +cd build ctest ``` Note these tests are run on PR's and pushes to the `ros2` branch. - diff --git a/build.bash b/build.bash new file mode 100755 index 0000000..41bea5b --- /dev/null +++ b/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 . \ No newline at end of file diff --git a/dgps_msgs/CMakeLists.txt b/dgps_msgs/CMakeLists.txt new file mode 100644 index 0000000..e8ef3fe --- /dev/null +++ b/dgps_msgs/CMakeLists.txt @@ -0,0 +1,14 @@ +cmake_minimum_required(VERSION 3.8) +project(dgps_msgs) + +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(sensor_msgs REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/DifferentialNavSatFix.msg" + DEPENDENCIES sensor_msgs +) + +ament_export_dependencies(rosidl_default_runtime) +ament_package() diff --git a/dgps_msgs/msg/DifferentialNavSatFix.msg b/dgps_msgs/msg/DifferentialNavSatFix.msg new file mode 100644 index 0000000..4f62c7c --- /dev/null +++ b/dgps_msgs/msg/DifferentialNavSatFix.msg @@ -0,0 +1,11 @@ +# Center/primary antenna navigation fix. +sensor_msgs/NavSatFix nmea + +# Frame-corrected ENU yaw, normalized to [0, 2*pi), in radians. +float32 heading + +# The same heading expressed in degrees, normalized to [0, 360). +float32 heading_deg + +# Heading variance in rad^2. +float32 heading_covariance diff --git a/dgps_msgs/package.xml b/dgps_msgs/package.xml new file mode 100644 index 0000000..cdca467 --- /dev/null +++ b/dgps_msgs/package.xml @@ -0,0 +1,18 @@ + + + dgps_msgs + 0.1.0 + Differential GPS fix and dual-antenna heading message + Shenbax + BSD-3-Clause + + ament_cmake + rosidl_default_generators + sensor_msgs + rosidl_default_runtime + rosidl_interface_packages + + + ament_cmake + + diff --git a/entrypoint.bash b/entrypoint.bash new file mode 100755 index 0000000..a5cd1ba --- /dev/null +++ b/entrypoint.bash @@ -0,0 +1,13 @@ +#!/bin/bash + +source /opt/ros/jazzy/setup.bash +source /home/dtc/ws/install/setup.bash + +if [ "$RUN" = "true" ]; then + echo "[GLIDER] Launching glider..." + ros2 launch glider glider-node.launch.py use_sim_time:="${USE_SIM_TIME:-false}" +else + echo "[GLIDER] RUN=false, keeping container alive..." +fi + +exec "$@" diff --git a/glider/CMakeLists.txt b/glider/CMakeLists.txt index f0e1550..d824bd8 100644 --- a/glider/CMakeLists.txt +++ b/glider/CMakeLists.txt @@ -44,6 +44,9 @@ if (BUILD_ROS) find_package(std_msgs REQUIRED) find_package(nav_msgs REQUIRED) find_package(gps_msgs REQUIRED) + find_package(dgps_msgs REQUIRED) + find_package(geometry_msgs REQUIRED) + find_package(tf2_ros REQUIRED) set(node_plugins "") endif() @@ -97,6 +100,8 @@ if (BUILD_ROS) geometry_msgs nav_msgs gps_msgs + dgps_msgs + tf2_ros ) add_executable(${PROJECT_NAME}_node @@ -146,6 +151,7 @@ if (BUILD_ROS) sensor_msgs geometry_msgs nav_msgs + dgps_msgs Eigen3 ) diff --git a/glider/config/glider-params.yaml b/glider/config/glider-params.yaml index 1e77d8a..27d7ae9 100644 --- a/glider/config/glider-params.yaml +++ b/glider/config/glider-params.yaml @@ -3,12 +3,14 @@ imu: accelerometer: 0.00001 gyroscope: 0.00001 integration: 0.001 - heading: 0.09 + heading: 0.09 roll_pitch: 0.001 bias: 0.001 frame: "enu" -gps: +gps: covariance: 2.0 +odom: + covariance: 0.1 dgps: enable: true covariance: 0.03 @@ -24,9 +26,30 @@ logging: stdout: true directory: "/tmp/glider" optimizer: - smooth: true + # Set true to use the incremental fixed-lag smoother instead of iSAM2. + smooth: false lag_time: 5.0 -gps_to_imu: - x: 0.0 - y: 0.0 - z: 0.0 +# All sensor locations are expressed relative to the LiDAR using ROS FLU axes: +# +X forward, +Y left, +Z up. Thus right/down/behind are negative Y/Z/X. +# Translations are metres; rotations are XYZ roll/pitch/yaw in degrees. +extrinsics: + reference_frame: "os_sensor" + body: + frame: "rko_base_link" + translation: {x: -0.2, y: 0.0, z: -0.338} + rotation_rpy_deg: {roll: 0.0, pitch: 0.0, yaw: 0.0} + lidar: + frame: "os_sensor" + translation: {x: 0.0, y: 0.0, z: 0.0} + rotation_rpy_deg: {roll: 0.0, pitch: 0.0, yaw: 0.0} + imu: + frame: "vectornav" + # 12 cm right and 17 cm down from the LiDAR. + translation: {x: 0.0, y: -0.12, z: -0.17} + rotation_rpy_deg: {roll: 90.6107, pitch: 3.1974, yaw: -148.5656} + gps: + frame: "navsat_link" + # 25 cm directly behind the LiDAR along the X axis. + translation: {x: -0.25, y: 0.0, z: 0.0} + rotation_rpy_deg: {roll: 0.0, pitch: 0.0, yaw: 0.0} + heading_offset_deg: 0.0 diff --git a/glider/config/ros-params.yaml b/glider/config/ros-params.yaml index 0a06d4c..549b273 100644 --- a/glider/config/ros-params.yaml +++ b/glider/config/ros-params.yaml @@ -2,10 +2,23 @@ glider_node: ros__parameters: publishers: rate: 0.0 - nav_sat_fix: false + nav_sat_fix: true + utm_zone: "18S" + map_frame: "map" + base_link_frame: "base_link" viz: use: true origin_easting: 753912.0063845584 origin_northing: 3385461.6073698294 subscribers: - use_odom: false + use_odom: true + use_gps: false + use_dgps: true + imu_topic: "/vectornav/imu" + odom_topic: "/rko_lio/odometry" + gps_topic: "/ublox/fix" + dgps_topic: "/sept/enu/dfix" + # Reject fixes when any reported position variance exceeds this value. + gps_rejection_variance: 1.0 + max_stamp_skew_sec: 1.0 + gps_loss_timeout_sec: 3.0 diff --git a/glider/include/glider/core/factor_manager.hpp b/glider/include/glider/core/factor_manager.hpp index 521aa58..8f3f7d1 100644 --- a/glider/include/glider/core/factor_manager.hpp +++ b/glider/include/glider/core/factor_manager.hpp @@ -2,7 +2,7 @@ * Jason Hughes * April 2025 * - * This manages everything with the factor graph. It adds measurements, + * This manages everything with the factor graph. It adds measurements, * runs the optimization with the smoother or isam and predicts with the pim */ @@ -49,7 +49,7 @@ using gtsam::symbol_shorthand::B; // Bias using gtsam::symbol_shorthand::V; // Velocity using gtsam::symbol_shorthand::X; // Pose -namespace Glider +namespace Glider { class FactorManager @@ -58,16 +58,19 @@ class FactorManager // Constructos /*! @brief default constructor */ FactorManager() = default; - /*! @brief constructor that initalizes all parameters in the + /*! @brief constructor that initalizes all parameters in the * factor manager * @param params: the parameters loaded from the yaml file*/ - FactorManager(const Parameters& params); - + 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 * @param timestamp: the time at which this method is being called * @return the odometry from the pim prediction */ - Odometry predict(int64_t timestamp); + Odometry predict(int64_t timestamp); /*! @brief the runner takes care of everything with the optimization, it calls the * optimizer, and resets everything after optimization is done * @param timestamp: time at which the runner is called @@ -79,21 +82,30 @@ 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 timestamp: time of the gps measurement * @param gps: GPS measurement in the UTM frame - * @param heading: heading from dgpsfm in the ENU 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, const double heading_sigma = 0.0); /*! @brief adds the imu measurements to the pim and saves the orientation - * @param timestamp: time of the imu measurement + * @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 a landmark factor for an estimated utm point and covariance + /*! @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, + const Eigen::Vector3d& velocity = Eigen::Vector3d::Zero(), + double velocity_sigma = 1.0); + /*! @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 * @param utm: the estimated utm coordinate of the landmark @@ -103,59 +115,71 @@ class FactorManager // getters and checkers /*! @brief gets the estimated landmark utm coordinate and covariance - * @param landmark_id: the uinque id for the landmark + * @param landmark_id: the uinque id for the landmark * @return the estimated utm point and covariance */ PointWithCovariance getLandmarkPoint(size_t landmark_id) const; /*! @brief gets the complete factor graph */ gtsam::ExpressionFactorGraph getGraph(); - /*! @brief checks if the imu has been initialized + /*! @brief checks if the imu has been initialized * @return true if imu bias calibration is complete else false*/ bool isImuInitialized() const; /*! @brief checks if the gps is initialized * @return true if the gps reading has been added to the graph else false */ bool isGpsInitialized() const; - /*! @brief checks if the odometry system is initialized + /*! @brief checks if the odometry system is initialized * @return true if graph has been optimized more than specified * number of times else false */ bool isSystemInitialized() const; - /*! @brief gets the matrix used for bias estimation + /*! @brief gets the matrix used for bias estimation * @return 6-by-bias_num_measurements matrix */ Eigen::MatrixXd getBiasEstimate() const; - /*! @brief gets the current pim object + /*! @brief gets the current pim object * @return the current pim object dereferenced */ gtsam::PreintegratedCombinedMeasurements getPim() const; - /*! @brief gets the key index + /*! @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 - * optimizer, either isam or fixed lag smoother + private: + /*! @brief handles the optimization call with the specified + * optimizer, either isam or fixed lag smoother * @return the output estimates from the optimization */ gtsam::Values optimize(); - - /*! @brief initializes all the parameters for the pim + + /*! @brief initializes all the parameters for the pim * @param g: gravity as defined in the yaml config * @return pim parameters as a shared_ptr */ boost::shared_ptr defaultImuParams(double g); - + /*! @brief helper function that sets initial values in the graph */ void initializeGraph(); - /*! @brief estiamtes the bias using the specified number of measurements + /*! @brief estiamtes the bias using the specified number of measurements * up initialization, and saves the orientation as the initial orientation - * @param accel_meas: accelerometer measurement + * @param accel_meas: accelerometer measurement * @param gytro_meas: gytroscop measurement * @param orient: the 3D orientation of the robot as a quaternion from the imu*/ void initializeImu(const Eigen::Vector3d& accel_meas, const Eigen::Vector3d& gyro_meas, const Eigen::Vector4d& orient); - // @brief a mutex to use accross function that access the pim + // @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 + // @brief parameters for the isam2 optimizer gtsam::ISAM2Params isam_params_; - // @brief parameters for the pim + // @brief parameters for the pim boost::shared_ptr imu_params_; // @brief parameters set in the config file Parameters params_; @@ -171,14 +195,14 @@ class FactorManager // @brief 6-by-bias_num_measurements matrix to store measurements // from accel and gyro to measurem bias Eigen::MatrixXd bias_estimate_vec_; - + // @brief saves the bias estimate from gtsam optimization gtsam::imuBias::ConstantBias bias_; // @brief the pim for imu measurements std::shared_ptr pim_; // noise - // @brief noise on the prior estimate + // @brief noise on the prior estimate gtsam::noiseModel::Isotropic::shared_ptr prior_noise_; // @brief noise on the gps position estimate gtsam::noiseModel::Isotropic::shared_ptr gps_noise_; @@ -186,6 +210,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 +248,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 landmark_info_; diff --git a/glider/include/glider/core/glider.hpp b/glider/include/glider/core/glider.hpp index deea476..1c08b19 100644 --- a/glider/include/glider/core/glider.hpp +++ b/glider/include/glider/core/glider.hpp @@ -31,12 +31,23 @@ class Glider /*! @brief converts the gps measurement from lat, lon to UTM * and passes that to the factor manager * @param timestamp: time that the gps measurement was taken - * @param gps: gps measurement in (lat, lon, alt) format, + * @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 @@ -44,13 +55,26 @@ class Glider * @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); + bool addOdom(int64_t timestamp, const Eigen::Isometry3d& pose, + const Eigen::Vector3d& velocity, double velocity_sigma); 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_; } - /*! @brief calls the factor manager to interpolate between GPS + 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 * measurements using the pim - * @param timestamp: time at which you want to interpolate + * @param timestamp: time at which you want to interpolate * @return odometry object tracking the predicted navstate * from the pim */ Odometry interpolate(int64_t timestamp); @@ -59,7 +83,7 @@ class Glider * @return the full odometry estimate with covariance from the results * of the gtsam optimization */ OdometryWithCovariance optimize(int64_t timestamp); - + private: /*! @brief initializes glog with the specified logging * parameters @@ -80,17 +104,21 @@ class Glider std::string frame_; // @brief the relative translation from the gps to // the imu - Eigen::Vector3d t_imu_gps_; + Eigen::Vector3d t_body_imu_; + Eigen::Matrix3d r_body_imu_; + Eigen::Vector3d t_body_gps_; + double gps_heading_offset_; // @brief the rotation matrix from ned to enu frame Eigen::Matrix3d r_enu_ned_; // @brief whether or not to use differential gps // from motion for heading bool use_dgpsfm_; - // @brief object to handle differential gps + // @brief object to handle differential gps // from motion Geodetics::DifferentialGpsFromMotion dgps_; - // @brief save the state estimate from + // @brief save the state estimate from // the optimizer OdometryWithCovariance current_odom_; + std::string utm_zone_; }; } diff --git a/glider/include/glider/core/odometry.hpp b/glider/include/glider/core/odometry.hpp index d3d3d84..a0900fd 100644 --- a/glider/include/glider/core/odometry.hpp +++ b/glider/include/glider/core/odometry.hpp @@ -3,10 +3,10 @@ * Jason Hughes * May 2025 * - * Struct to keep track of the odometry output + * Struct to keep track of the odometry output * from the factor graph. This keeps track of * everything the gtsam NavState does but adds - * a timestamp, gyroscope reading and initilization + * a timestamp, gyroscope reading and initilization * status. */ @@ -35,17 +35,17 @@ namespace Glider class Odometry { public: - /*! @brief default constructor of Odometry object, note that this + /*! @brief default constructor of Odometry object, note that this * sets initialized_ to false */ Odometry() = default; - /*! @brief initialize the Odometry from the result of optimization, designed to used + /*! @brief initialize the Odometry from the result of optimization, designed to used * by child class upon inheritance * @param val: results from gtsam optimization * @param timestamp: timestamp passed to the optimizer * @param key: current key_index to get current results from val - * @param init: should this constuctor call initialize the odometry */ + * @param init: should this constuctor call initialize the odometry */ Odometry(gtsam::Values& val, int64_t timestamp, gtsam::Key key, bool init = true); - /*! @brief initialize the Odometry from a NavState, likely from calling the the pim + /*! @brief initialize the Odometry from a NavState, likely from calling the the pim * predict * @param ns: the current NavState from gtsam * @param timestamp: the current timestamp @@ -60,7 +60,7 @@ class Odometry * @type T: std::pair */ template T getPose() const; - /*! @brief gets the 3D positon from odometry + /*! @brief gets the 3D positon from odometry * @return position in 3D in UTM ENU frame (easting, northing, altitude) * @type T: gtsam::Point3 * @type T: Eigen::Vector3d*/ @@ -71,7 +71,7 @@ class Odometry * @type T: gtsam::Rot3 * @type T: gtsam::Quaternion * @type T: Eigen::Vector4d - * @type T: Eigen::Quaterniond */ + * @type T: Eigen::Quaterniond */ template T getOrientation() const; /* @brief gets the 3D velocity from odometry @@ -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 */ @@ -112,9 +116,9 @@ class Odometry double getLongitude(const char* zone); /*! @brief get the latitude and longitude as a pair * @param zone: the utm zone, ex "18S" - * @return latitude and longitude in degrees decimal as a pair + * @return latitude and longitude in degrees decimal as a pair * where lat is first and lon is second */ - std::pair getLatLon(const char* zone); + std::pair 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,10 +128,15 @@ 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 - * @return a pair of Eigen objects where first is position and second in orientation as a quaternion + * of Eigen objects + * @return a pair of Eigen objects where first is position and second in orientation as a quaternion * @type TF: Eigen::Vector3d * @type TS: Eigen::Vector4d * @type TS: Eigen::Quaterniond */ @@ -138,7 +147,7 @@ class Odometry double latitude_; // @brief the longitude from the input UTM pose double longitude_; - + // @brief the 3D velocity in m/s gtsam::Point3 velocity_; // @brief the 3D position in UTM and ENU frames @@ -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 diff --git a/glider/include/glider/utils/parameters.hpp b/glider/include/glider/utils/parameters.hpp index 3160331..8b9a4f3 100644 --- a/glider/include/glider/utils/parameters.hpp +++ b/glider/include/glider/utils/parameters.hpp @@ -44,8 +44,10 @@ struct Parameters // @brief covariance of the IMU's bias estimate double bias_cov; // @brief covariance of the GPS position estimate - // TODO make this gps_cov to match + // 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; @@ -61,7 +63,7 @@ struct Parameters // @brief if true this logs to stdout and log file otherwise it logs // just to a file bool log; - // @brief the directory to save the log file, glog needs an + // @brief the directory to save the log file, glog needs an // absolute path std::string log_dir; @@ -70,7 +72,7 @@ struct Parameters bool smooth; // @brief amount of time in seconds for the fixed lag smoother to // smooth over. - double lag_time; + double lag_time; // @brief wheather or not to integrate differential gps from motion heading, // if false orientation from the IMU will be integrated @@ -78,13 +80,16 @@ struct Parameters // @brief velocity in m/s that the robot should be moving at to integrate // dgpsfm double dgpsfm_threshold; - // @brief heading noise for differential gps from motion + // @brief heading noise for differential gps from motion double dgpsfm_cov; bool use_dgps; double dgps_cov; - - // @brief translation from the GPS to the IMU - Eigen::Vector3d t_imu_gps; + // Sensor poses transformed from the configured reference frame to body. + Eigen::Vector3d t_body_imu; + Eigen::Matrix3d r_body_imu; + Eigen::Vector3d t_body_gps; + double gps_heading_offset; + std::string body_frame; }; } diff --git a/glider/include/ros/conversions.hpp b/glider/include/ros/conversions.hpp index 26a1262..50ed501 100644 --- a/glider/include/ros/conversions.hpp +++ b/glider/include/ros/conversions.hpp @@ -37,10 +37,10 @@ class Conversions static Output eigenToRos(const Input& vec); template - static Output odomToRos(Glider::Odometry& odom, const char* zone = nullptr); + static Output odomToRos(Glider::Odometry& odom, std::string frame_id = "enu", const char* zone = nullptr, const Eigen::Vector3d& offset = Eigen::Vector3d::Zero()); template - static Output odomToRos(Glider::OdometryWithCovariance& odom_wc, const char* zone = nullptr); + static Output odomToRos(Glider::OdometryWithCovariance& odom_wc, std::string frame_id = "enu", const char* zone = nullptr, const Eigen::Vector3d& offset = Eigen::Vector3d::Zero()); template static void addCovariance(const Glider::OdometryWithCovariance& odom_wc, T& msg); @@ -48,7 +48,7 @@ class Conversions static std::chrono::milliseconds hzToDuration(const double freq); private: - + struct RosToEigen { static Eigen::Vector3d vector3Convert(const geometry_msgs::msg::Vector3& vec); diff --git a/glider/include/ros/glider_node.hpp b/glider/include/ros/glider_node.hpp index 121d5f1..7729951 100644 --- a/glider/include/ros/glider_node.hpp +++ b/glider/include/ros/glider_node.hpp @@ -14,7 +14,11 @@ #include #include #include +#include #include +#include +#include +#include #include "glider/core/glider.hpp" #include "glider/core/odometry.hpp" @@ -33,19 +37,25 @@ class GliderNode : public rclcpp::Node private: std::unique_ptr glider_; - // timer callbacks + // timer callbacks void interpolationCallback(); // subscriber callbacks - void dgpsCallback(const gps_msgs::msg::GPSFix::ConstSharedPtr msg); + void dgpsCallback(const dgps_msgs::msg::DifferentialNavSatFix::ConstSharedPtr msg); void gpsCallback(const sensor_msgs::msg::NavSatFix::ConstSharedPtr msg); void imuCallback(const sensor_msgs::msg::Imu::ConstSharedPtr msg); void magCallback(const sensor_msgs::msg::MagneticField::ConstSharedPtr msg); void odomCallback(const nav_msgs::msg::Odometry::ConstSharedPtr msg); void poseCallback(const geometry_msgs::msg::PoseStamped::ConstSharedPtr msg); + void gpsGoalCallback(const sensor_msgs::msg::NavSatFix::ConstSharedPtr msg); // utility functions int64_t getTime(const builtin_interfaces::msg::Time& stamp) const; + int64_t getSynchronizedTime(const builtin_interfaces::msg::Time& stamp, const char* source, + std::optional& clock_offset); + void updateEnvironmentState(int64_t timestamp); + void markGpsAccepted(int64_t timestamp); + void markGpsUnavailable(); void publishOdometry(Glider::OdometryWithCovariance& state) const; void publishOdometry(Glider::Odometry& odom) const; void publishNavSatFix(Glider::OdometryWithCovariance& state) const; @@ -53,35 +63,53 @@ class GliderNode : public rclcpp::Node void publishOdometryViz(nav_msgs::msg::Odometry viz_msg) const; // subscriptions - rclcpp::Subscription::ConstSharedPtr dgps_sub_; + rclcpp::Subscription::ConstSharedPtr dgps_sub_; rclcpp::Subscription::ConstSharedPtr gps_sub_; rclcpp::Subscription::ConstSharedPtr imu_sub_; rclcpp::Subscription::ConstSharedPtr mag_sub_; rclcpp::Subscription::ConstSharedPtr odom_sub_; rclcpp::Subscription::ConstSharedPtr pose_sub_; + rclcpp::Subscription::SharedPtr gps_goal_sub_; // groups rclcpp::CallbackGroup::SharedPtr imu_group_; rclcpp::CallbackGroup::SharedPtr gps_group_; - - // publishers + + // publishers rclcpp::Publisher::SharedPtr odom_pub_; rclcpp::Publisher::SharedPtr odom_viz_pub_; rclcpp::Publisher::SharedPtr gps_pub_; + rclcpp::Publisher::SharedPtr goal_pub_; + + std::unique_ptr tf_broadcaster_; // timers rclcpp::TimerBase::SharedPtr timer_; // parameters - bool initialized_; bool publish_nsf_; bool viz_; + bool use_odom_; + bool use_gps_; + bool use_dgps_; + double gps_rejection_variance_; + double max_stamp_skew_sec_; + double gps_loss_timeout_sec_; std::string utm_zone_; + std::string map_frame_; + std::string base_link_frame_; double origin_easting_; double origin_northing_; double freq_; // tracker Glider::OdometryWithCovariance current_state_; + std::optional imu_clock_offset_; + std::optional gps_clock_offset_; + std::optional dgps_clock_offset_; + std::optional odom_clock_offset_; + enum class EnvironmentState { Unknown, Outdoor, Indoor }; + EnvironmentState environment_state_{EnvironmentState::Unknown}; + std::optional last_accepted_gps_time_; }; } diff --git a/glider/launch/glider-node.launch.py b/glider/launch/glider-node.launch.py index 7e05987..6a8167d 100644 --- a/glider/launch/glider-node.launch.py +++ b/glider/launch/glider-node.launch.py @@ -23,10 +23,10 @@ def generate_launch_description(): default_value='false', description='Use simulation time' ) - + # Get launch configurations use_sim_time = LaunchConfiguration('use_sim_time') - + # Find package share directory glider_share = FindPackageShare('glider') glider_share_dir = get_package_share_directory('glider') @@ -37,13 +37,13 @@ def generate_launch_description(): 'config', 'ros-params.yaml' ]) - + graph_params_file = PathJoinSubstitution([ glider_share, 'config', 'glider-params.yaml' ]) - + # create logging directory with open(os.path.join(glider_share_dir, "config", "glider-params.yaml")) as f: config = yaml.safe_load(f) @@ -58,14 +58,8 @@ def generate_launch_description(): parameters=[ ros_params_file, {'path': graph_params_file, - 'use_sim_time': use_sim_time, - 'use_odom': False} - ], - remappings=[ - ('/dgps', '/dgps/fix'), - ('/imu', '/vectornav/imu'), - ('/odom', '/Odometry'), + 'use_sim_time': use_sim_time} ] ) - + return LaunchDescription([use_sim_time_arg, glider_node]) diff --git a/glider/package.xml b/glider/package.xml index 849bb37..0fa16b0 100644 --- a/glider/package.xml +++ b/glider/package.xml @@ -17,6 +17,8 @@ sensor_msgs nav_msgs gps_msgs + dgps_msgs + tf2_ros tf2_eigen message_filters diff --git a/glider/ros/conversions.cpp b/glider/ros/conversions.cpp index 0034c04..c589080 100644 --- a/glider/ros/conversions.cpp +++ b/glider/ros/conversions.cpp @@ -1,12 +1,14 @@ /* * Jason Hughes -* July 2025 +* July 2025 * * convert between ros and eigen */ #include "ros/conversions.hpp" +#include + using namespace GliderROS; template @@ -84,7 +86,7 @@ Eigen::Isometry3d Conversions::RosToEigen::poseConvert(const geometry_msgs::msg: Eigen::Vector3d Conversions::RosToEigen::vector3Convert(const geometry_msgs::msg::Vector3& msg) { return Eigen::Vector3d(msg.x, msg.y, msg.z); -} +} Eigen::Vector4d Conversions::RosToEigen::orientConvert(const geometry_msgs::msg::Quaternion& msg) { @@ -105,7 +107,7 @@ std::pair Conversions::RosToEigen::dgpsConvert } Eigen::Isometry3d Conversions::RosToEigen::odomConvert(const nav_msgs::msg::Odometry& msg) -{ +{ Eigen::Quaterniond quat(msg.pose.pose.orientation.w, msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z); Eigen::Vector3d trans(msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z); @@ -175,29 +177,42 @@ std_msgs::msg::Header Conversions::getHeader(int64_t timestamp, std::string fram } template -Output Conversions::odomToRos(Glider::Odometry& odom, const char* zone) +Output Conversions::odomToRos(Glider::Odometry& odom, std::string frame_id, const char* zone, const Eigen::Vector3d& offset) { if constexpr (std::is_same_v) { sensor_msgs::msg::NavSatFix msg; - if (zone == nullptr) + // Backward compatibility: the original two-argument API used its + // string argument as the UTM zone and always published in ENU. + std::string legacy_zone; + if (zone == nullptr && frame_id.size() >= 2 && std::isdigit(frame_id.front())) + { + legacy_zone = frame_id; + zone = legacy_zone.c_str(); + frame_id = "enu"; + } + if (zone == nullptr || std::string(zone) == "") { throw std::invalid_argument("specify a zone for UTM to GPS converstion"); } else { - std::pair latlon = odom.getLatLon(zone); + std::pair latlon = odom.getLatLon(zone, offset); + + msg.status.status = odom.isInitialized() ? + sensor_msgs::msg::NavSatStatus::STATUS_FIX : + sensor_msgs::msg::NavSatStatus::STATUS_NO_FIX; msg.latitude = latlon.first; msg.longitude = latlon.second; - msg.altitude = odom.getAltitude(); + msg.altitude = odom.getAltitude() + offset(2); msg.position_covariance_type = 3; - msg.header = getHeader(odom.getTimestamp(), "enu"); + msg.header = getHeader(odom.getTimestamp(), frame_id); } return msg; } else if constexpr (std::is_same_v) - { + { nav_msgs::msg::Odometry msg; Eigen::Vector3d p = odom.getPosition(); @@ -216,7 +231,7 @@ Output Conversions::odomToRos(Glider::Odometry& odom, const char* zone) msg.twist.twist.linear.x = v(0); msg.twist.twist.linear.y = v(1); msg.twist.twist.linear.z = v(2); - msg.header = getHeader(odom.getTimestamp(), "enu"); + msg.header = getHeader(odom.getTimestamp(), frame_id); return msg; } @@ -228,32 +243,36 @@ Output Conversions::odomToRos(Glider::Odometry& odom, const char* zone) } template -Output Conversions::odomToRos(Glider::OdometryWithCovariance& odom_wc, const char* zone) +Output Conversions::odomToRos(Glider::OdometryWithCovariance& odom_wc, std::string frame_id, const char* zone, const Eigen::Vector3d& offset) { if constexpr (std::is_same_v) { sensor_msgs::msg::NavSatFix msg; - if (zone == nullptr) + if (zone == nullptr || std::string(zone) == "") { throw std::invalid_argument("specify a zone for UTM to GPS conversion"); } else { - std::pair latlon = odom_wc.getLatLon(zone); + std::pair latlon = odom_wc.getLatLon(zone, offset); + + msg.status.status = odom_wc.isGpsOffsetInitialized() ? + sensor_msgs::msg::NavSatStatus::STATUS_FIX : + sensor_msgs::msg::NavSatStatus::STATUS_NO_FIX; msg.latitude = latlon.first; msg.longitude = latlon.second; - msg.altitude = odom_wc.getAltitude(); + msg.altitude = odom_wc.getAltitude() + offset(2); msg.position_covariance_type = 3; Eigen::Matrix3d cov = odom_wc.getPositionCovariance(); - for (int i = 0; i < cov.rows(); ++i) + for (int i = 0; i < cov.rows(); ++i) { - for (int j = 0; j < cov.cols(); ++j) + for (int j = 0; j < cov.cols(); ++j) { msg.position_covariance[i * 3 + j] = cov(i, j); } } - msg.header = getHeader(odom_wc.getTimestamp(), "enu"); + msg.header = getHeader(odom_wc.getTimestamp(), frame_id); } return msg; } @@ -273,9 +292,9 @@ Output Conversions::odomToRos(Glider::OdometryWithCovariance& odom_wc, const cha msg.pose.pose.orientation.z = q.z(); Eigen::MatrixXd cov = odom_wc.getPoseCovariance(); - for (int i = 0; i < cov.rows(); ++i) + for (int i = 0; i < cov.rows(); ++i) { - for (int j = 0; j < cov.cols(); ++j) + for (int j = 0; j < cov.cols(); ++j) { msg.pose.covariance[i * cov.rows() + j] = cov(i, j); } @@ -286,16 +305,16 @@ Output Conversions::odomToRos(Glider::OdometryWithCovariance& odom_wc, const cha msg.twist.twist.linear.x = v(0); msg.twist.twist.linear.y = v(1); msg.twist.twist.linear.z = v(2); - + cov = odom_wc.getVelocityCovariance(); - for (int i = 0; i < cov.rows(); ++i) + for (int i = 0; i < cov.rows(); ++i) { - for (int j = 0; j < cov.cols(); ++j) + for (int j = 0; j < cov.cols(); ++j) { msg.twist.covariance[i * cov.rows() + j] = cov(i, j); } } - msg.header = getHeader(odom_wc.getTimestamp(), "enu"); + msg.header = getHeader(odom_wc.getTimestamp(), frame_id); return msg; } else @@ -314,7 +333,7 @@ std::chrono::milliseconds Conversions::hzToDuration(double freq) } double period_seconds = 1.0 / freq; - + std::chrono::milliseconds period_ms = std::chrono::milliseconds(static_cast(period_seconds * 1e3)); return period_ms; } @@ -325,28 +344,28 @@ void Conversions::addCovariance(const Glider::OdometryWithCovariance& odom_wc, T if constexpr (std::is_same_v) { Eigen::Matrix3d cov = odom_wc.getPositionCovariance(); - for (int i = 0; i < cov.rows(); ++i) + for (int i = 0; i < cov.rows(); ++i) { - for (int j = 0; j < cov.cols(); ++j) + for (int j = 0; j < cov.cols(); ++j) { msg.position_covariance[i * 3 + j] = cov(i, j); } } } else if constexpr (std::is_same_v) - { + { Eigen::MatrixXd cov = odom_wc.getPoseCovariance(); - for (int i = 0; i < cov.rows(); ++i) + for (int i = 0; i < cov.rows(); ++i) { - for (int j = 0; j < cov.cols(); ++j) + for (int j = 0; j < cov.cols(); ++j) { msg.pose.covariance[i * cov.rows() + j] = cov(i, j); } } cov = odom_wc.getVelocityCovariance(); - for (int i = 0; i < cov.rows(); ++i) + for (int i = 0; i < cov.rows(); ++i) { - for (int j = 0; j < cov.cols(); ++j) + for (int j = 0; j < cov.cols(); ++j) { msg.twist.covariance[i * cov.rows() + j] = cov(i, j); } @@ -371,11 +390,11 @@ template geometry_msgs::msg::Quaternion Conversions::eigenToRos(const Eigen::Vector3d& vec); template geometry_msgs::msg::PoseStamped Conversions::eigenToRos(const Eigen::Isometry3d& vec); -template nav_msgs::msg::Odometry Conversions::odomToRos(Glider::Odometry& odom, const char* zone); -template sensor_msgs::msg::NavSatFix Conversions::odomToRos(Glider::Odometry& odom, const char* zone); +template nav_msgs::msg::Odometry Conversions::odomToRos(Glider::Odometry& odom, std::string frame_id, const char* zone, const Eigen::Vector3d& offset); +template sensor_msgs::msg::NavSatFix Conversions::odomToRos(Glider::Odometry& odom, std::string frame_id, const char* zone, const Eigen::Vector3d& offset); -template nav_msgs::msg::Odometry Conversions::odomToRos(Glider::OdometryWithCovariance& odom_wc, const char* zone); -template sensor_msgs::msg::NavSatFix Conversions::odomToRos(Glider::OdometryWithCovariance& odom_wc, const char* zone); +template nav_msgs::msg::Odometry Conversions::odomToRos(Glider::OdometryWithCovariance& odom_wc, std::string frame_id, const char* zone, const Eigen::Vector3d& offset); +template sensor_msgs::msg::NavSatFix Conversions::odomToRos(Glider::OdometryWithCovariance& odom_wc, std::string frame_id, const char* zone, const Eigen::Vector3d& offset); template void Conversions::addCovariance(const Glider::OdometryWithCovariance& odom_wc, nav_msgs::msg::Odometry& msg); template void Conversions::addCovariance(const Glider::OdometryWithCovariance& odom_wc, sensor_msgs::msg::NavSatFix& msg); diff --git a/glider/ros/glider_node.cpp b/glider/ros/glider_node.cpp index 17040b7..660c615 100644 --- a/glider/ros/glider_node.cpp +++ b/glider/ros/glider_node.cpp @@ -5,6 +5,10 @@ #include "ros/glider_node.hpp" +#include +#include +#include + using namespace GliderROS; GliderNode::GliderNode(const rclcpp::NodeOptions& options) : rclcpp::Node("glider_node", options) @@ -15,8 +19,20 @@ GliderNode::GliderNode(const rclcpp::NodeOptions& options) : rclcpp::Node("glide declare_parameter("publishers.viz.use", false); declare_parameter("publishers.viz.origin_easting", 0.0); declare_parameter("publishers.viz.origin_northing", 0.0); - + declare_parameter("publishers.utm_zone", "14R"); + declare_parameter("publishers.map_frame", "map"); + declare_parameter("publishers.base_link_frame", "base_link"); + + declare_parameter("subscribers.dgps_topic", "/dgps"); + declare_parameter("subscribers.gps_topic", "/gps"); + declare_parameter("subscribers.imu_topic", "/imu"); + declare_parameter("subscribers.odom_topic", "/odom"); + declare_parameter("subscribers.use_gps", true); + declare_parameter("subscribers.use_dgps", true); declare_parameter("subscribers.use_odom", false); + declare_parameter("subscribers.gps_rejection_variance", 100.0); + declare_parameter("subscribers.max_stamp_skew_sec", 1.0); + declare_parameter("subscribers.gps_loss_timeout_sec", 3.0); declare_parameter("path", ""); @@ -27,12 +43,22 @@ GliderNode::GliderNode(const rclcpp::NodeOptions& options) : rclcpp::Node("glide viz_ = this->get_parameter("publishers.viz.use").as_bool(); origin_easting_ = this->get_parameter("publishers.viz.origin_easting").as_double(); origin_northing_ = this->get_parameter("publishers.viz.origin_northing").as_double(); + map_frame_ = this->get_parameter("publishers.map_frame").as_string(); + base_link_frame_ = this->get_parameter("publishers.base_link_frame").as_string(); + + use_odom_ = this->get_parameter("subscribers.use_odom").as_bool(); + use_gps_ = this->get_parameter("subscribers.use_gps").as_bool(); + use_dgps_ = this->get_parameter("subscribers.use_dgps").as_bool(); + gps_rejection_variance_ = this->get_parameter("subscribers.gps_rejection_variance").as_double(); + max_stamp_skew_sec_ = this->get_parameter("subscribers.max_stamp_skew_sec").as_double(); + gps_loss_timeout_sec_ = this->get_parameter("subscribers.gps_loss_timeout_sec").as_double(); - bool use_odom = this->get_parameter("subscribers.use_odom").as_bool(); - std::string path = this->get_parameter("path").as_string(); glider_ = std::make_unique(path); + utm_zone_ = this->get_parameter("publishers.utm_zone").as_string(); + + tf_broadcaster_ = std::make_unique(*this); current_state_ = Glider::OdometryWithCovariance::Uninitialized(); imu_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); @@ -41,38 +67,42 @@ GliderNode::GliderNode(const rclcpp::NodeOptions& options) : rclcpp::Node("glide // Create subscribers with callback groups auto imu_sub_options = rclcpp::SubscriptionOptions(); imu_sub_options.callback_group = imu_group_; - imu_sub_ = this->create_subscription("/imu", 20, + auto imu_topic = this->get_parameter("subscribers.imu_topic").as_string(); + imu_sub_ = this->create_subscription(imu_topic, rclcpp::SensorDataQoS(), std::bind(&GliderNode::imuCallback, this, std::placeholders::_1), imu_sub_options); - + auto gps_sub_options = rclcpp::SubscriptionOptions(); gps_sub_options.callback_group = gps_group_; - gps_sub_ = this->create_subscription("/gps", 1, + auto gps_topic = this->get_parameter("subscribers.gps_topic").as_string(); + gps_sub_ = this->create_subscription(gps_topic, rclcpp::SensorDataQoS(), std::bind(&GliderNode::gpsCallback, this, std::placeholders::_1), gps_sub_options); - dgps_sub_ = this->create_subscription("/dgps", 1, + auto dgps_topic = this->get_parameter("subscribers.dgps_topic").as_string(); + dgps_sub_ = this->create_subscription(dgps_topic, rclcpp::SensorDataQoS(), std::bind(&GliderNode::dgpsCallback, this, std::placeholders::_1), gps_sub_options); + gps_goal_sub_ = this->create_subscription("/glider/gps_goal", 1, + std::bind(&GliderNode::gpsGoalCallback, this, std::placeholders::_1)); + auto odom_sub_options = rclcpp::SubscriptionOptions(); odom_sub_options.callback_group = gps_group_; - odom_sub_ = this->create_subscription("/odom", 1, + auto odom_topic = this->get_parameter("subscribers.odom_topic").as_string(); + odom_sub_ = this->create_subscription(odom_topic, rclcpp::SensorDataQoS(), std::bind(&GliderNode::odomCallback, this, std::placeholders::_1), odom_sub_options); + LOG(INFO) << "[GLIDER] Publishing Odometry msg on /glider/odom"; + LOG(INFO) << "[GLIDER] Using prediction rate: " << freq_; + odom_pub_ = this->create_publisher("/glider/odom", 10); + if (publish_nsf_) { LOG(INFO) << "[GLIDER] Publishing NavSatFix msg on /glider/fix"; - LOG(INFO) << "[GLIDER] Using prediction rate: " << freq_; gps_pub_ = this->create_publisher("/glider/fix", 10); } - else - { - LOG(INFO) << "[GLIDER] Publishing Odometry msg on /glider/odom"; - LOG(INFO) << "[GLIDER] Using prediction rate: " << freq_; - odom_pub_ = this->create_publisher("/glider/odom", 10); - } if(viz_) { @@ -80,6 +110,9 @@ GliderNode::GliderNode(const rclcpp::NodeOptions& options) : rclcpp::Node("glide odom_viz_pub_ = this->create_publisher("/glider/odom/viz", 10); } + LOG(INFO) << "[GLIDER] Publishing Goal Pose on /goal_pose"; + goal_pub_ = this->create_publisher("/goal_pose", 10); + if (freq_ > 0) { std::chrono::milliseconds d = GliderROS::Conversions::hzToDuration(freq_); @@ -93,6 +126,64 @@ int64_t GliderNode::getTime(const builtin_interfaces::msg::Time& stamp) const return (static_cast(stamp.sec) * 1000000000LL) + static_cast(stamp.nanosec); } +int64_t GliderNode::getSynchronizedTime(const builtin_interfaces::msg::Time& stamp, const char* source, + std::optional& clock_offset) +{ + const int64_t message_time = getTime(stamp); + const int64_t ros_time = getTime(this->now()); + + // Recorded header stamps already use the simulated clock timeline. + if (this->get_parameter("use_sim_time").as_bool()) + return message_time > 0 ? message_time : ros_time; + + const int64_t max_skew = static_cast(max_stamp_skew_sec_ * 1e9); + if (message_time <= 0) return ros_time; + + if (!clock_offset.has_value()) + { + clock_offset = std::llabs(message_time - ros_time) > max_skew ? ros_time - message_time : 0; + if (*clock_offset != 0) + LOG(INFO) << "[GLIDER] Aligning " << source << " clock to the active ROS clock"; + } + + int64_t synchronized_time = message_time + *clock_offset; + if (std::llabs(synchronized_time - ros_time) > max_skew) + { + *clock_offset = ros_time - message_time; + synchronized_time = ros_time; + LOG_FIRST_N(WARNING, 5) << "[GLIDER] " << source << " clock jumped; realigning"; + } + return synchronized_time; +} + +void GliderNode::updateEnvironmentState(int64_t timestamp) +{ + if (environment_state_ != EnvironmentState::Outdoor || !last_accepted_gps_time_) return; + const double elapsed = static_cast(timestamp - *last_accepted_gps_time_) / 1e9; + if (elapsed >= gps_loss_timeout_sec_) + { + environment_state_ = EnvironmentState::Indoor; + LOG(INFO) << "[GLIDER] Outdoor → Indoor"; + } +} + +void GliderNode::markGpsAccepted(int64_t timestamp) +{ + if (environment_state_ == EnvironmentState::Indoor) + LOG(INFO) << "[GLIDER] Indoor → Outdoor"; + environment_state_ = EnvironmentState::Outdoor; + last_accepted_gps_time_ = timestamp; +} + +void GliderNode::markGpsUnavailable() +{ + if (environment_state_ == EnvironmentState::Outdoor) + { + environment_state_ = EnvironmentState::Indoor; + LOG(INFO) << "[GLIDER] Outdoor → Indoor"; + } +} + void GliderNode::interpolationCallback() { // if the state is not initialized we cannot interpolate @@ -100,7 +191,7 @@ void GliderNode::interpolationCallback() int64_t timestamp = getTime(this->now()); Glider::Odometry odom = glider_->interpolate(timestamp); - (publish_nsf_) ? publishNavSatFix(odom) : publishOdometry(odom); + publishOdometry(odom); } void GliderNode::imuCallback(const sensor_msgs::msg::Imu::ConstSharedPtr msg) @@ -109,85 +200,266 @@ void GliderNode::imuCallback(const sensor_msgs::msg::Imu::ConstSharedPtr msg) Eigen::Vector3d gyro = GliderROS::Conversions::rosToEigen(msg->angular_velocity); Eigen::Vector3d accel = GliderROS::Conversions::rosToEigen(msg->linear_acceleration); Eigen::Vector4d orient = GliderROS::Conversions::rosToEigen(msg->orientation); - int64_t timestamp = getTime(msg->header.stamp); + if (!gyro.allFinite() || !accel.allFinite() || !orient.allFinite() || orient.norm() < 1e-6) + { + LOG_FIRST_N(WARNING, 10) << "[GLIDER] IMU ignored: non-finite data or invalid quaternion"; + return; + } + orient.normalize(); + int64_t timestamp = getSynchronizedTime(msg->header.stamp, "IMU", imu_clock_offset_); + updateEnvironmentState(timestamp); glider_->addImu(timestamp, accel, gyro, orient); if (freq_ == 0 && current_state_.isInitialized()) { Glider::Odometry odom = glider_->interpolate(timestamp); - (publish_nsf_) ? publishNavSatFix(odom) : publishOdometry(odom); + publishOdometry(odom); } } -void GliderNode::dgpsCallback(const gps_msgs::msg::GPSFix::ConstSharedPtr msg) +void GliderNode::dgpsCallback(const dgps_msgs::msg::DifferentialNavSatFix::ConstSharedPtr msg) { + if (!use_dgps_) return; + const auto& fix = msg->nmea; + const double variance = std::max({fix.position_covariance[0], fix.position_covariance[4], + fix.position_covariance[8]}); + if (fix.status.status < sensor_msgs::msg::NavSatStatus::STATUS_FIX || + !std::isfinite(variance) || variance < 1e-6) + { + markGpsUnavailable(); + LOG_FIRST_N(WARNING, 5) << "[GLIDER] DGPS ignored: No fix or invalid covariance"; + return; + } + + if (variance > gps_rejection_variance_) + { + markGpsUnavailable(); + LOG_FIRST_N(INFO, 5) << "[GLIDER] DGPS rejected due to unsafe covariance (> " << gps_rejection_variance_ << ")"; + return; + } LOG_FIRST_N(INFO, 1) << "[GLIDER] Received DGPS measurement"; - std::pair dgps = GliderROS::Conversions::rosToEigen>(*msg); - int64_t timestamp = getTime(msg->header.stamp); + Eigen::Vector3d gps = GliderROS::Conversions::rosToEigen(fix); + if (!gps.allFinite()) + { + markGpsUnavailable(); + LOG_FIRST_N(WARNING, 10) << "[GLIDER] DGPS ignored: non-finite position"; + return; + } + int64_t timestamp = getSynchronizedTime(fix.header.stamp, "DGPS", dgps_clock_offset_); + markGpsAccepted(timestamp); + + // Include unmodeled frame, lever-arm, and synchronization uncertainty. + const double sigma = std::max(glider_->params().gps_noise, std::sqrt(variance)); + // The custom message stores ENU heading in radians and covariance in rad^2. + const double heading_covariance = static_cast(msg->heading_covariance); + if (!std::isfinite(static_cast(msg->heading)) || + !std::isfinite(heading_covariance) || heading_covariance < 0.0) + { + // Retain position-only fixes when heading is unavailable. + LOG_FIRST_N(WARNING, 10) << "[GLIDER] DGPS heading unavailable; fusing position only"; + glider_->addGps(timestamp, gps, sigma); + // LIO publishes the queued factor at the next odometry update. + if (!use_odom_) + { + current_state_ = glider_->optimize(timestamp); + if (publish_nsf_ && current_state_.isInitialized()) publishNavSatFix(current_state_); + } + return; + } + const double heading_sigma = std::max(0.1, std::sqrt(std::max(0.0, heading_covariance))); + Eigen::Vector2d heading(static_cast(msg->heading), heading_sigma); + glider_->addGpsWithHeading(timestamp, gps, heading, sigma); - glider_->addGpsWithHeading(timestamp, dgps.first, dgps.second); - - current_state_ = glider_->optimize(timestamp); + if (!use_odom_) + { + current_state_ = glider_->optimize(timestamp); + if (publish_nsf_ && current_state_.isInitialized()) publishNavSatFix(current_state_); + } } void GliderNode::gpsCallback(const sensor_msgs::msg::NavSatFix::ConstSharedPtr msg) { + if (!use_gps_) return; + if (msg->status.status < sensor_msgs::msg::NavSatStatus::STATUS_FIX || msg->position_covariance[0] < 1e-6) + { + markGpsUnavailable(); + LOG_FIRST_N(WARNING, 5) << "[GLIDER] GPS ignored: No fix or invalid covariance"; + return; + } + + const double variance = std::max({msg->position_covariance[0], msg->position_covariance[4], + msg->position_covariance[8]}); + if (!std::isfinite(variance) || variance > gps_rejection_variance_) + { + markGpsUnavailable(); + LOG_FIRST_N(INFO, 5) << "[GLIDER] GPS rejected due to unsafe covariance (> " << gps_rejection_variance_ << ")"; + return; + } LOG_FIRST_N(INFO, 1) << "[GLIDER] Recieved GPS measurement"; Eigen::Vector3d gps = GliderROS::Conversions::rosToEigen(*msg); + if (!gps.allFinite()) + { + markGpsUnavailable(); + LOG_FIRST_N(WARNING, 10) << "[GLIDER] GPS ignored: non-finite position"; + return; + } - int64_t timestamp = getTime(msg->header.stamp); + int64_t timestamp = getSynchronizedTime(msg->header.stamp, "GPS", gps_clock_offset_); + markGpsAccepted(timestamp); - glider_->addGps(timestamp, gps); + const double sigma = std::max(glider_->params().gps_noise, std::sqrt(variance)); + glider_->addGps(timestamp, gps, sigma); - current_state_ = glider_->optimize(timestamp); + if (!use_odom_) + { + current_state_ = glider_->optimize(timestamp); + if (publish_nsf_ && current_state_.isInitialized()) publishNavSatFix(current_state_); + } } void GliderNode::odomCallback(const nav_msgs::msg::Odometry::ConstSharedPtr msg) { - // TODO - //Eigen::Isometry3d pose = GliderROS::Conversions::rosToEigen(*msg); - //int64_t timestamp = getTime(msg->header.stamp); - //glider_->addOdom(timestamp, pose); + if (!use_odom_) return; + Eigen::Isometry3d pose = GliderROS::Conversions::rosToEigen(*msg); + if (!pose.matrix().allFinite()) + { + LOG_FIRST_N(WARNING, 10) << "[GLIDER] LIO odometry ignored: non-finite pose"; + return; + } + Eigen::Vector3d velocity_body(msg->twist.twist.linear.x, + msg->twist.twist.linear.y, + msg->twist.twist.linear.z); + if (!velocity_body.allFinite()) + { + LOG_FIRST_N(WARNING, 10) << "[GLIDER] LIO odometry ignored: non-finite velocity"; + return; + } + // nav_msgs/Odometry expresses twist in child_frame_id; the graph velocity + // is in the map frame. + Eigen::Vector3d velocity_map = pose.rotation() * velocity_body; + const double velocity_variance = std::max({msg->twist.covariance[0], + msg->twist.covariance[7], + msg->twist.covariance[14]}); + const double velocity_sigma = std::isfinite(velocity_variance) && velocity_variance > 1e-8 + ? std::sqrt(velocity_variance) + // Treat zero covariance as unknown. + : 0.1; + int64_t timestamp = getSynchronizedTime(msg->header.stamp, "LIO odometry", odom_clock_offset_); + if (glider_->addOdom(timestamp, pose, velocity_map, velocity_sigma)) + { + current_state_ = glider_->optimize(timestamp); + if (publish_nsf_ && current_state_.isInitialized()) publishNavSatFix(current_state_); + } +} + +void GliderNode::gpsGoalCallback(const sensor_msgs::msg::NavSatFix::ConstSharedPtr msg) +{ + if (!glider_->isGpsOffsetInitialized()) + { + LOG_FIRST_N(WARNING, 1) << "[GLIDER] GPS Goal ignored: System not initialized with global origin yet."; + return; + } + + LOG(INFO) << "[GLIDER] Received GPS Goal: " << msg->latitude << ", " << msg->longitude; + + double easting, northing; + char zone[10]; + Glider::geodetics::LLtoUTM(msg->latitude, msg->longitude, northing, easting, zone); + Eigen::Vector3d goal_utm(easting, northing, 0.0); + + Eigen::Vector3d gps_offset = glider_->getGpsOffset(); + Eigen::Vector3d goal_local = goal_utm - gps_offset; + + LOG(INFO) << "[GLIDER] GPS Goal in map frame: " << goal_local(0) << ", " << goal_local(1); + + geometry_msgs::msg::PoseStamped goal_msg; + goal_msg.header.stamp = this->now(); + goal_msg.header.frame_id = map_frame_; + goal_msg.pose.position.x = goal_local(0); + goal_msg.pose.position.y = goal_local(1); + goal_msg.pose.position.z = 0.0; + goal_msg.pose.orientation.w = 1.0; // Default orientation + + goal_pub_->publish(goal_msg); } void GliderNode::publishOdometry(Glider::OdometryWithCovariance& state) const { LOG_FIRST_N(INFO, 1) << "[GLIDER] Publishing Odometry from optimzation"; - nav_msgs::msg::Odometry msg = GliderROS::Conversions::odomToRos(state); + nav_msgs::msg::Odometry msg = GliderROS::Conversions::odomToRos(state, map_frame_); + msg.child_frame_id = base_link_frame_; odom_pub_->publish(msg); - + + geometry_msgs::msg::TransformStamped tf; + tf.header = msg.header; + tf.child_frame_id = msg.child_frame_id; + tf.transform.translation.x = msg.pose.pose.position.x; + tf.transform.translation.y = msg.pose.pose.position.y; + tf.transform.translation.z = msg.pose.pose.position.z; + tf.transform.rotation = msg.pose.pose.orientation; + tf_broadcaster_->sendTransform(tf); + if (viz_) publishOdometryViz(msg); } void GliderNode::publishOdometry(Glider::Odometry& odom) const { - LOG_FIRST_N(INFO, 1) << "[GLIDER] Publishing Odometry from prediction"; - nav_msgs::msg::Odometry msg = GliderROS::Conversions::odomToRos(odom); + nav_msgs::msg::Odometry msg = GliderROS::Conversions::odomToRos(odom, map_frame_); + msg.child_frame_id = base_link_frame_; GliderROS::Conversions::addCovariance(current_state_, msg); odom_pub_->publish(msg); + geometry_msgs::msg::TransformStamped tf; + tf.header = msg.header; + tf.child_frame_id = msg.child_frame_id; + tf.transform.translation.x = msg.pose.pose.position.x; + tf.transform.translation.y = msg.pose.pose.position.y; + tf.transform.translation.z = msg.pose.pose.position.z; + tf.transform.rotation = msg.pose.pose.orientation; + tf_broadcaster_->sendTransform(tf); + if (viz_) publishOdometryViz(msg); } void GliderNode::publishNavSatFix(Glider::OdometryWithCovariance& state) const { + if (!glider_->isGpsInitialized()) return; + // TODO add covariance LOG_FIRST_N(INFO, 1) << "[GLIDER] Publishing NavSatFix from optimization"; - sensor_msgs::msg::NavSatFix msg = GliderROS::Conversions::odomToRos(state); + Eigen::Vector3d offset = glider_->getGpsOffset(); + if (offset.norm() == 0.0 && origin_easting_ != 0.0) { + offset(0) = origin_easting_; + offset(1) = origin_northing_; + } + std::string zone = glider_->getUtmZone(); + if (zone.empty()) zone = utm_zone_; + sensor_msgs::msg::NavSatFix msg = GliderROS::Conversions::odomToRos(state, base_link_frame_, zone.c_str(), offset); + if (current_state_.isInitialized()) + GliderROS::Conversions::addCovariance(current_state_, msg); gps_pub_->publish(msg); } void GliderNode::publishNavSatFix(Glider::Odometry& odom) const { + if (!glider_->isGpsInitialized()) return; + // TODO add covariance LOG_FIRST_N(INFO, 1) << "[GLIDER] Publishing NavSatFix from prediction"; - sensor_msgs::msg::NavSatFix msg = GliderROS::Conversions::odomToRos(odom); + Eigen::Vector3d offset = glider_->getGpsOffset(); + if (offset.norm() == 0.0 && origin_easting_ != 0.0) { + offset(0) = origin_easting_; + offset(1) = origin_northing_; + } + sensor_msgs::msg::NavSatFix msg = GliderROS::Conversions::odomToRos(odom, base_link_frame_, utm_zone_.c_str(), offset); + if (current_state_.isInitialized()) + GliderROS::Conversions::addCovariance(current_state_, msg); gps_pub_->publish(msg); } void GliderNode::publishOdometryViz(nav_msgs::msg::Odometry viz_msg) const -{ +{ double x = viz_msg.pose.pose.position.x - origin_easting_; double y = viz_msg.pose.pose.position.y - origin_northing_; viz_msg.pose.pose.position.x = x; diff --git a/glider/src/factor_manager.cpp b/glider/src/factor_manager.cpp index 2a8964e..72cc1a8 100644 --- a/glider/src/factor_manager.cpp +++ b/glider/src/factor_manager.cpp @@ -14,15 +14,33 @@ using namespace Glider; std::mutex Glider::FactorManager::mutex_; FactorManager::FactorManager(const Parameters& params) +{ + initialize(params); +} + +void FactorManager::initialize(const Parameters& params) { // set initialization status imu_initialized_ = false; gps_initialized_ = false; sys_initialized_ = false; + odom_initialized_ = false; + using_local_origin_ = false; + gps_offset_initialized_ = false; + gps_offset_ = Eigen::Vector3d::Zero(); + last_node_time_ = 0.0; + accumulated_odom_delta_ = Eigen::Isometry3d::Identity(); // setup parameters params_ = params; imu_params_ = defaultImuParams(params.gravity); + const Eigen::Matrix3d identity = Eigen::Matrix3d::Identity(); + imu_params_->setAccelerometerCovariance(params.accel_cov * identity); + imu_params_->setGyroscopeCovariance(params.gyro_cov * identity); + imu_params_->setIntegrationCovariance(params.integration_cov * identity); + imu_params_->setBiasAccCovariance(params.bias_cov * identity); + imu_params_->setBiasOmegaCovariance(params.bias_cov * identity); + imu_params_->setBiasAccOmegaInit(params.bias_cov * Eigen::Matrix::Identity()); // imu initialization @@ -34,7 +52,8 @@ FactorManager::FactorManager(const Parameters& params) gps_noise_ = gtsam::noiseModel::Isotropic::Sigma(3, params.gps_noise); orient_noise_ = gtsam::noiseModel::Diagonal::Sigmas(gtsam::Vector3(params.roll_pitch_cov, params.roll_pitch_cov, params.heading_cov)); dgpsfm_noise_ = gtsam::noiseModel::Diagonal::Sigmas(gtsam::Vector3(M_PI/2, M_PI/2, params.dgpsfm_cov)); - + odom_noise_ = gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << params.odom_cov, params.odom_cov, params.odom_cov, params.odom_cov, params.odom_cov, params.odom_cov).finished()); + // set key index key_index_ = 0; @@ -46,6 +65,8 @@ FactorManager::FactorManager(const Parameters& params) isam_ = gtsam::ISAM2(isam_params_); smoother_ = gtsam::IncrementalFixedLagSmoother(params_.lag_time, isam_params_); + orient_ = Eigen::Vector4d(1.0, 0.0, 0.0, 0.0); + LOG(INFO) << "[GLIDER] Factor Manager initialzed"; } @@ -55,7 +76,7 @@ boost::shared_ptr FactorManager::defaultImu params = gtsam::PreintegrationCombinedParams::MakeSharedU(g); double gyro_sigma = (0.5 * M_PI / 180.0) / 60.0; double accel_sigma = 0.001; - + Eigen::Matrix3d I = Eigen::Matrix3d::Identity(); params->setGyroscopeCovariance(std::pow(gyro_sigma, 2) * I); @@ -65,12 +86,15 @@ boost::shared_ptr FactorManager::defaultImu return params; } -void FactorManager::initializeImu(const Eigen::Vector3d& accel_meas, const Eigen::Vector3d& gyro_meas, const Eigen::Vector4d& orient) +void FactorManager::initializeImu(const Eigen::Vector3d& accel_meas, const Eigen::Vector3d& gyro_meas, const Eigen::Vector4d& orient) { if (init_counter_ < params_.bias_num_measurements) { - // the measurement to bias matrix - bias_estimate_vec_.row(init_counter_).head(3) = accel_meas - gravity_vec_; + // Estimate bias in the IMU/body frame. + const gtsam::Rot3 body_to_enu = + gtsam::Rot3::Quaternion(orient(0), orient(1), orient(2), orient(3)); + const Eigen::Vector3d gravity_body = body_to_enu.unrotate(gravity_vec_); + bias_estimate_vec_.row(init_counter_).head(3) = accel_meas - gravity_body; bias_estimate_vec_.row(init_counter_).tail(3) = gyro_meas; init_counter_++; } @@ -88,31 +112,58 @@ void FactorManager::initializeImu(const Eigen::Vector3d& accel_meas, const Eigen initializeGraph(); imu_initialized_ = true; - LOG(INFO) << "[GLIDER] IMU initalized"; + LOG(INFO) << "[GLIDER] IMU initialized (bias calibration complete)"; } } -void FactorManager::initializeGraph() +void FactorManager::initializeGraph() { initials_ = gtsam::InitializePose3::initialize(graph_); } -void FactorManager::addGpsFactor(int64_t timestamp, const Eigen::Vector3d& gps) +void FactorManager::addGpsFactor(int64_t timestamp, const Eigen::Vector3d& gps, const double sigma) { - // wait until the imu is initialized - if (!imu_initialized_) return; - + std::lock_guard graph_lock(graph_mutex_); double time = nanosecIntToDouble(timestamp); + // wait until the imu is initialized + if (!imu_initialized_) + { + LOG_FIRST_N(WARNING, 5) << "[GLIDER] GPS received but IMU is not initialized yet. Skipping factor."; + return; + } + + if (!gps_offset_initialized_) + { + if (using_local_origin_) + { + // Align GPS to the measured local-odometry origin. + const Eigen::Vector3d local_position = odom_initialized_ + ? last_odom_meas_.translation() + : current_state_.getPose().translation(); + gps_offset_ = gps - local_position; + gps_offset_initialized_ = true; + gps_initialized_ = true; + LOG(INFO) << "[GLIDER] GPS offset initialized from outdoor transition at " << std::fixed << std::setprecision(2) << gps.transpose(); + } + else + { + gps_offset_ = Eigen::Vector3d::Zero(); + gps_offset_initialized_ = true; + gps_initialized_ = true; + LOG(INFO) << "[GLIDER] GPS offset initialized (GPS is origin)"; + } + } + if (key_index_ == 0) { - // set the initial NavState + // set the initial NavState // The initial orientation is the the initial orientation from the imu initialization - // The initial position is from the gps + // The initial position is from the gps // Initial velocity is set to zero gtsam::Pose3 initial_pose = gtsam::Pose3(initial_orientation_, gtsam::Point3(gps(0), gps(1), gps(2))); gtsam::NavState initial_navstate(initial_pose, gtsam::Point3(0.0, 0.0, 0.0)); // TODO why do I need this?? - + // save the initial values initials_.insert(X(key_index_), initial_pose); initials_.insert(V(key_index_), gtsam::Point3(0.0, 0.0, 0.0)); @@ -133,50 +184,123 @@ void FactorManager::addGpsFactor(int64_t timestamp, const Eigen::Vector3d& gps) LOG(INFO) << "[GLIDER] GPS Initialized"; return; } - + + // LIO owns graph-state creation when local odometry is enabled. + if (odom_initialized_) + { + Eigen::Vector3d aligned_gps = using_local_origin_ ? gps - gps_offset_ : gps; + gtsam::noiseModel::Base::shared_ptr noise = gps_noise_; + if (sigma > 0.0) noise = gtsam::noiseModel::Isotropic::Sigma(3, sigma); + graph_.add(gtsam::GPSFactor(X(key_index_ - 1), aligned_gps, noise)); + return; + } + + // Attach near-simultaneous GPS and LIO updates to the current state. + double imu_delta_time = 0.0; + { + std::lock_guard pim_lock(mutex_); + imu_delta_time = pim_ ? pim_->deltaTij() : 0.0; + } + if (time - last_node_time_ < 0.02 || imu_delta_time < 0.01) + { + Eigen::Vector3d aligned_gps = using_local_origin_ ? gps - gps_offset_ : gps; + gtsam::noiseModel::Base::shared_ptr noise = gps_noise_; + if (sigma > 0.0) noise = gtsam::noiseModel::Isotropic::Sigma(3, sigma); + graph_.add(gtsam::GPSFactor(X(key_index_ - 1), aligned_gps, noise)); + return; + } + // add the pim to the graph under a mutex - std::unique_lock lock(mutex_); + std::unique_lock pim_lock(mutex_); graph_.add(gtsam::CombinedImuFactor(X(key_index_-1), V(key_index_-1), X(key_index_), V(key_index_), B(key_index_-1), B(key_index_), *pim_)); - lock.unlock(); + pim_ = std::make_shared(imu_params_, bias_); + if (odom_initialized_) + { + gtsam::Pose3 odom_delta_gtsam(accumulated_odom_delta_.matrix()); + graph_.add(gtsam::BetweenFactor(X(key_index_-1), X(key_index_), odom_delta_gtsam, odom_noise_)); + } + accumulated_odom_delta_ = Eigen::Isometry3d::Identity(); + pim_lock.unlock(); // insert new initial values - initials_.insert(X(key_index_), current_state_.getPose()); - initials_.insert(V(key_index_), current_state_.getVelocity()); - initials_.insert(B(key_index_), bias_); + if (!initials_.exists(X(key_index_))) initials_.insert(X(key_index_), current_state_.getPose()); + if (!initials_.exists(V(key_index_))) initials_.insert(V(key_index_), current_state_.getVelocity()); + if (!initials_.exists(B(key_index_))) initials_.insert(B(key_index_), bias_); + graph_.add(gtsam::PriorFactor( + B(key_index_), bias_, gtsam::noiseModel::Isotropic::Sigma(6, 0.1))); + graph_.add(gtsam::PriorFactor( + V(key_index_), current_state_.getVelocity(), + gtsam::noiseModel::Isotropic::Sigma(3, 2.0))); // save the time for the smoother smoother_timestamps_[X(key_index_)] = time; smoother_timestamps_[V(key_index_)] = time; smoother_timestamps_[B(key_index_)] = time; - + // convert eigen to gtsam gtsam::Point3 meas(gps(0), gps(1), gps(2)); gtsam::Rot3 rot = gtsam::Rot3::Quaternion(orient_(0), orient_(1), orient_(2), orient_(3)); + + Eigen::Vector3d aligned_gps = gps; + if (using_local_origin_) aligned_gps = gps - gps_offset_; + + gtsam::noiseModel::Base::shared_ptr noise = gps_noise_; + if (sigma > 0.0) noise = gtsam::noiseModel::Isotropic::Sigma(3, sigma); + // add gps measurement to factor graph as gtsam object - graph_.add(gtsam::GPSFactor(X(key_index_), gps, gps_noise_)); + graph_.add(gtsam::GPSFactor(X(key_index_), aligned_gps, noise)); graph_.addExpressionFactor(gtsam::rotation(X(key_index_)), rot, orient_noise_); - + // increment key index key_index_++; + last_node_time_ = time; } -void FactorManager::addGpsFactor(int64_t timestamp, const Eigen::Vector3d& gps, const double& heading, const bool fuse) +void FactorManager::addGpsFactor(int64_t timestamp, const Eigen::Vector3d& gps, const double& heading, const bool fuse, + const double sigma, const double heading_sigma) { - // wait until the imu is initialized - if (!imu_initialized_) return; - + std::lock_guard graph_lock(graph_mutex_); double time = nanosecIntToDouble(timestamp); + // wait until the imu is initialized + if (!imu_initialized_) + { + LOG_FIRST_N(WARNING, 5) << "[GLIDER] GPS received but IMU is not initialized yet. Skipping factor."; + return; + } + + if (!gps_offset_initialized_) + { + if (using_local_origin_) + { + // Align GPS to the measured local-odometry origin. + const Eigen::Vector3d local_position = odom_initialized_ + ? last_odom_meas_.translation() + : current_state_.getPose().translation(); + gps_offset_ = gps - local_position; + gps_offset_initialized_ = true; + gps_initialized_ = true; + LOG(INFO) << "[GLIDER] GPS offset initialized from outdoor transition at " << std::fixed << std::setprecision(2) << gps.transpose(); + } + else + { + gps_offset_ = Eigen::Vector3d::Zero(); + gps_offset_initialized_ = true; + gps_initialized_ = true; + LOG(INFO) << "[GLIDER] GPS offset initialized (GPS is origin)"; + } + } + if (key_index_ == 0) { - // set the initial NavState + // set the initial NavState // The initial orientation is the the initial orientation from the imu initialization - // The initial position is from the gps + // The initial position is from the gps // Initial velocity is set to zero gtsam::Pose3 initial_pose = gtsam::Pose3(initial_orientation_, gtsam::Point3(gps(0), gps(1), gps(2))); gtsam::NavState initial_navstate(initial_pose, gtsam::Point3(0.0, 0.0, 0.0)); // TODO why do I need this?? - + // save the initial values initials_.insert(X(key_index_), initial_pose); initials_.insert(V(key_index_), gtsam::Point3(0.0, 0.0, 0.0)); @@ -197,35 +321,104 @@ void FactorManager::addGpsFactor(int64_t timestamp, const Eigen::Vector3d& gps, LOG(INFO) << "[GLIDER] GPS Initialized"; return; } - + + if (odom_initialized_) + { + Eigen::Vector3d aligned_gps = using_local_origin_ ? gps - gps_offset_ : gps; + gtsam::noiseModel::Base::shared_ptr noise = gps_noise_; + if (sigma > 0.0) noise = gtsam::noiseModel::Isotropic::Sigma(3, sigma); + graph_.add(gtsam::GPSFactor(X(key_index_ - 1), aligned_gps, noise)); + if (fuse) + { + auto heading_noise = dgpsfm_noise_; + if (heading_sigma > 0.0 && std::isfinite(heading_sigma)) + heading_noise = gtsam::noiseModel::Diagonal::Sigmas( + gtsam::Vector3(M_PI / 2.0, M_PI / 2.0, heading_sigma)); + graph_.addExpressionFactor(gtsam::rotation(X(key_index_ - 1)), + gtsam::Rot3::Ypr(heading, 0.0, 0.0), heading_noise); + } + return; + } + + // Attach near-simultaneous DGPS and LIO updates to the current state. + double imu_delta_time = 0.0; + { + std::lock_guard pim_lock(mutex_); + imu_delta_time = pim_ ? pim_->deltaTij() : 0.0; + } + if (time - last_node_time_ < 0.02 || imu_delta_time < 0.01) + { + Eigen::Vector3d aligned_gps = using_local_origin_ ? gps - gps_offset_ : gps; + gtsam::noiseModel::Base::shared_ptr noise = gps_noise_; + if (sigma > 0.0) noise = gtsam::noiseModel::Isotropic::Sigma(3, sigma); + graph_.add(gtsam::GPSFactor(X(key_index_ - 1), aligned_gps, noise)); + if (fuse) + { + auto heading_noise = dgpsfm_noise_; + if (heading_sigma > 0.0 && std::isfinite(heading_sigma)) + heading_noise = gtsam::noiseModel::Diagonal::Sigmas( + gtsam::Vector3(M_PI / 2.0, M_PI / 2.0, heading_sigma)); + graph_.addExpressionFactor(gtsam::rotation(X(key_index_ - 1)), + gtsam::Rot3::Ypr(heading, 0.0, 0.0), heading_noise); + } + return; + } + // add the pim to the graph under a mutex - std::unique_lock lock(mutex_); + std::unique_lock pim_lock(mutex_); graph_.add(gtsam::CombinedImuFactor(X(key_index_-1), V(key_index_-1), X(key_index_), V(key_index_), B(key_index_-1), B(key_index_), *pim_)); - lock.unlock(); + pim_ = std::make_shared(imu_params_, bias_); + if (odom_initialized_) + { + gtsam::Pose3 odom_delta_gtsam(accumulated_odom_delta_.matrix()); + graph_.add(gtsam::BetweenFactor(X(key_index_-1), X(key_index_), odom_delta_gtsam, odom_noise_)); + } + accumulated_odom_delta_ = Eigen::Isometry3d::Identity(); + pim_lock.unlock(); // insert new initial values - initials_.insert(X(key_index_), current_state_.getPose()); - initials_.insert(V(key_index_), current_state_.getVelocity()); - initials_.insert(B(key_index_), bias_); + if (!initials_.exists(X(key_index_))) initials_.insert(X(key_index_), current_state_.getPose()); + if (!initials_.exists(V(key_index_))) initials_.insert(V(key_index_), current_state_.getVelocity()); + if (!initials_.exists(B(key_index_))) initials_.insert(B(key_index_), bias_); + graph_.add(gtsam::PriorFactor( + B(key_index_), bias_, gtsam::noiseModel::Isotropic::Sigma(6, 0.1))); + graph_.add(gtsam::PriorFactor( + V(key_index_), current_state_.getVelocity(), + gtsam::noiseModel::Isotropic::Sigma(3, 2.0))); // save the time for the smoother smoother_timestamps_[X(key_index_)] = time; smoother_timestamps_[V(key_index_)] = time; smoother_timestamps_[B(key_index_)] = time; - + // add gps measurement to factor graph as gtsam object gtsam::Point3 meas(gps(0), gps(1), gps(2)); gtsam::Rot3 rot = gtsam::Rot3::Ypr(heading, 0.0, 0.0); - graph_.add(gtsam::GPSFactor(X(key_index_), gps, gps_noise_)); - if (fuse) graph_.addExpressionFactor(gtsam::rotation(X(key_index_)), rot, dgpsfm_noise_); + + Eigen::Vector3d aligned_gps = gps; + if (using_local_origin_) aligned_gps = gps - gps_offset_; + + gtsam::noiseModel::Base::shared_ptr noise = gps_noise_; + if (sigma > 0.0) noise = gtsam::noiseModel::Isotropic::Sigma(3, sigma); + + // add gps measurement to factor graph as gtsam object + graph_.add(gtsam::GPSFactor(X(key_index_), aligned_gps, noise)); + auto heading_noise = dgpsfm_noise_; + if (heading_sigma > 0.0 && std::isfinite(heading_sigma)) + { + heading_noise = gtsam::noiseModel::Diagonal::Sigmas( + gtsam::Vector3(M_PI / 2.0, M_PI / 2.0, heading_sigma)); + } + if (fuse) graph_.addExpressionFactor(gtsam::rotation(X(key_index_)), rot, heading_noise); // increment key index key_index_++; + last_node_time_ = time; } -void FactorManager::addImuFactor(int64_t timestamp, const Eigen::Vector3d& accel, const Eigen::Vector3d& gyro, const Eigen::Vector4d& orient) +void FactorManager::addImuFactor(int64_t timestamp, const Eigen::Vector3d& accel, const Eigen::Vector3d& gyro, const Eigen::Vector4d& orient) { // if the imu is not initialized, pass the meaurements to initialize it if (!imu_initialized_) @@ -238,22 +431,107 @@ void FactorManager::addImuFactor(int64_t timestamp, const Eigen::Vector3d& accel double current_time = nanosecIntToDouble(timestamp); double dt = current_time - last_imu_time_; if (dt <= 0.0) - { + { LOG(WARNING) << "[GLIDER] Recieved IMU measurement out of order, ignoring"; return; } // both the runner and the add imu access the pim in different threads // so we need to lock it when we manipulate it std::lock_guard lock(mutex_); - + pim_->integrateMeasurement(accel, gyro, dt); orient_ = orient; last_imu_time_ = current_time; } -void FactorManager::addLandmarkFactor(int64_t timestamp, size_t landmark_id, const Eigen::Vector3d& utm, const Eigen::Matrix3d& cov) +bool FactorManager::addOdomFactor(int64_t timestamp, const Eigen::Isometry3d& odom, + const Eigen::Vector3d& velocity, double velocity_sigma) +{ + std::lock_guard lock(graph_mutex_); + if (!imu_initialized_) return false; + + std::lock_guard pim_lock(mutex_); + if (!odom_initialized_) + { + last_odom_meas_ = odom; + odom_initialized_ = true; + + if (key_index_ == 0) + { + double time = nanosecIntToDouble(timestamp); + gtsam::Pose3 initial_pose(odom.matrix()); + if (!initials_.exists(X(key_index_))) initials_.insert(X(key_index_), initial_pose); + if (!initials_.exists(V(key_index_))) initials_.insert(V(key_index_), velocity); + if (!initials_.exists(B(key_index_))) initials_.insert(B(key_index_), bias_); + + smoother_timestamps_[X(key_index_)] = time; + smoother_timestamps_[V(key_index_)] = time; + smoother_timestamps_[B(key_index_)] = time; + + graph_.add(gtsam::PriorFactor(X(key_index_), initial_pose, gtsam::noiseModel::Isotropic::Sigma(6, 0.001))); + graph_.add(gtsam::PriorFactor( + V(key_index_), velocity, + gtsam::noiseModel::Isotropic::Sigma(3, std::max(0.01, velocity_sigma)))); + graph_.add(gtsam::PriorFactor(B(key_index_), bias_, gtsam::noiseModel::Isotropic::Sigma(6, 0.001))); + + key_index_++; + using_local_origin_ = true; + last_node_time_ = time; + LOG(INFO) << "[GLIDER] Odometry Initialized Graph Origin"; + return true; + } + + LOG(INFO) << "[GLIDER] Odometry Tracking Begun"; + return false; + } + + Eigen::Isometry3d delta = last_odom_meas_.inverse() * odom; + accumulated_odom_delta_ = accumulated_odom_delta_ * delta; + last_odom_meas_ = odom; + + double time = nanosecIntToDouble(timestamp); + double dt = time - last_node_time_; + + // Create a graph state for each time-valid LIO update. + if (dt >= 0.02 && pim_->deltaTij() >= 0.01) + { + // LIO supplies the graph transition; IMU remains the high-rate predictor. + pim_ = std::make_shared(imu_params_, bias_); + + gtsam::Pose3 odom_delta_gtsam(accumulated_odom_delta_.matrix()); + graph_.add(gtsam::BetweenFactor( + X(key_index_-1), X(key_index_), odom_delta_gtsam, odom_noise_)); + accumulated_odom_delta_ = Eigen::Isometry3d::Identity(); + + gtsam::Pose3 next_pose = current_state_.isInitialized() ? current_state_.getPose() : gtsam::Pose3(odom.matrix()); + gtsam::Vector3 next_vel = current_state_.isInitialized() ? current_state_.getVelocity() : gtsam::Vector3(0.0, 0.0, 0.0); + + if (!initials_.exists(X(key_index_))) initials_.insert(X(key_index_), next_pose); + if (!initials_.exists(V(key_index_))) initials_.insert(V(key_index_), next_vel); + if (!initials_.exists(B(key_index_))) initials_.insert(B(key_index_), bias_); + graph_.add(gtsam::PriorFactor( + B(key_index_), bias_, gtsam::noiseModel::Isotropic::Sigma(6, 0.1))); + graph_.add(gtsam::PriorFactor( + V(key_index_), velocity, + gtsam::noiseModel::Isotropic::Sigma(3, std::clamp(velocity_sigma, 0.01, 2.0)))); + // Absolute raw-LIO priors would conflict with accumulated GPS corrections. + + smoother_timestamps_[X(key_index_)] = time; + smoother_timestamps_[V(key_index_)] = time; + smoother_timestamps_[B(key_index_)] = time; + + key_index_++; + last_node_time_ = time; + return true; + } + + return false; +} + +void FactorManager::addLandmarkFactor(int64_t /*timestamp*/, size_t landmark_id, const Eigen::Vector3d& utm, const Eigen::Matrix3d& cov) { + std::lock_guard lock(graph_mutex_); Eigen::Matrix3d obs_info = cov.inverse(); auto it = landmark_info_.find(landmark_id); if (it == landmark_info_.end()) { @@ -269,7 +547,7 @@ PointWithCovariance FactorManager::getLandmarkPoint(size_t landmark_id) const { auto it = landmark_info_.find(landmark_id); if (it == landmark_info_.end()) return PointWithCovariance(); - + Eigen::Matrix3d cov = it->second.inverse(); Eigen::Vector3d point = cov * landmark_info_vec_.at(landmark_id); return PointWithCovariance(point, cov); @@ -277,23 +555,36 @@ PointWithCovariance FactorManager::getLandmarkPoint(size_t landmark_id) const Odometry FactorManager::predict(int64_t timestamp) { - // TODO update this. - //return Odometry::Uninitialized(); - if (sys_initialized_ && pim_) + std::lock_guard lock(graph_mutex_); + if (isSystemInitialized() && pim_) { + std::lock_guard lock(mutex_); gtsam::NavState result = pim_->predict(current_state_.getNavState(), bias_); - return Odometry(result, timestamp, true); - } - else - { - return Odometry::Uninitialized(); + const gtsam::Point3 predicted_position = result.pose().translation(); + const gtsam::Point3 optimized_position = current_state_.getPose().translation(); + const bool finite = predicted_position.allFinite() && result.velocity().allFinite() && + result.pose().rotation().matrix().allFinite(); + // Reject invalid short-horizon predictions. + if (!finite || (predicted_position - optimized_position).norm() > 2.0) + { + LOG_FIRST_N(WARNING, 10) + << "[GLIDER] IMU prediction rejected; publishing last optimized state"; + gtsam::NavState fallback_state = current_state_.getNavState(); + Odometry odom(fallback_state, timestamp, true); + odom.setGpsOffsetInitialized(gps_offset_initialized_); + return odom; + } + Odometry odom(result, timestamp, true); + odom.setGpsOffsetInitialized(gps_offset_initialized_); + return odom; } + + return Odometry::Uninitialized(); } -gtsam::Values FactorManager::optimize() +gtsam::Values FactorManager::optimize() { - isam_.update(graph_, initials_); gtsam::Values result; // call the specified optimizer if (params_.smooth) @@ -303,6 +594,7 @@ gtsam::Values FactorManager::optimize() } else { + isam_.update(graph_, initials_); result = isam_.calculateEstimate(); } optimized_count_++; @@ -316,20 +608,46 @@ gtsam::Values FactorManager::optimize() return result; } -OdometryWithCovariance FactorManager::runner(int64_t timestamp) +OdometryWithCovariance FactorManager::runner(int64_t timestamp) { + std::lock_guard lock(graph_mutex_); // if the graph or imu is not initialized we cannot optimize // so we return an uninitialized state - if (!imu_initialized_ || !gps_initialized_) return OdometryWithCovariance::Uninitialized(); + if (key_index_ == 0 || !imu_initialized_) + { + return OdometryWithCovariance::Uninitialized(); + } - gtsam::Values result = optimize(); + gtsam::Values result; + try + { + result = optimize(); + } + catch (const std::exception& e) + { + graph_.resize(0); + initials_.clear(); + smoother_timestamps_.clear(); + // Restore the next key after a failed update. + if (current_state_.isInitialized()) + { + key_index_ = current_state_.getKeyIndex() + 1; + LOG(WARNING) << "[GLIDER] Optimizer failed, rolling back key_index_ to " << key_index_; + } + else + { + key_index_ = 0; + LOG(WARNING) << "[GLIDER] Optimizer failed during init, resetting key_index_ to 0"; + } + throw; + } // get the covariance from isam or the smoother gtsam::Matrix pose_cov, vel_cov; if (params_.smooth) { pose_cov = smoother_.marginalCovariance(X(key_index_-1)); - vel_cov = smoother_.marginalCovariance(X(key_index_-1)); + vel_cov = smoother_.marginalCovariance(V(key_index_-1)); } else { @@ -338,18 +656,19 @@ OdometryWithCovariance FactorManager::runner(int64_t timestamp) } // save the current state we just optimized for current_state_ = OdometryWithCovariance(result, timestamp, key_index_-1, pose_cov, vel_cov, true); + current_state_.setGpsOffsetInitialized(gps_offset_initialized_); + + bias_ = current_state_.getBias(); - // reset the pim - pim_->resetIntegration(); // reset the graph + graph_.resize(0); initials_.clear(); smoother_timestamps_.clear(); - graph_.resize(0); // we want to optimize a few times before // publishing to allow convergence // otherwise we return an unitialized state - if (!sys_initialized_) return OdometryWithCovariance::Uninitialized(); + if (!isSystemInitialized()) return OdometryWithCovariance::Uninitialized(); return current_state_; } @@ -388,3 +707,13 @@ gtsam::Key FactorManager::getKeyIndex() const { return key_index_; } + +Eigen::Vector3d FactorManager::getGpsOffset() const +{ + return gps_offset_; +} + +bool FactorManager::isGpsOffsetInitialized() const +{ + return gps_offset_initialized_; +} diff --git a/glider/src/glider.cpp b/glider/src/glider.cpp index 9be26cc..2c8d1c1 100644 --- a/glider/src/glider.cpp +++ b/glider/src/glider.cpp @@ -10,14 +10,17 @@ namespace Glider { -Glider::Glider(const std::string& path) +Glider::Glider(const std::string& path) { Parameters params = Parameters::Load(path); initializeLogging(params); - factor_manager_ = FactorManager(params); - + factor_manager_.initialize(params); + frame_ = params.frame; - t_imu_gps_ = params.t_imu_gps; + t_body_imu_ = params.t_body_imu; + r_body_imu_ = params.r_body_imu; + t_body_gps_ = params.t_body_gps; + gps_heading_offset_ = params.gps_heading_offset; r_enu_ned_ = Eigen::Matrix3d::Zero(); r_enu_ned_ << 0.0, 1.0, 0.0, 1.0, 0.0, 0.0, @@ -25,11 +28,12 @@ Glider::Glider(const std::string& path) LOG(INFO) << "[GLIDER] Using IMU frame: " << frame_; LOG(INFO) << "[GLIDER] Using Fixed Lag Smoother: " << std::boolalpha << params.smooth; - LOG(INFO) << "[GLIDER] Logging to: " << params.log_dir; + LOG(INFO) << "[GLIDER] Logging to: " << params.log_dir; use_dgpsfm_ = params.use_dgpsfm; dgps_ = Geodetics::DifferentialGpsFromMotion(params.frame, params.dgpsfm_threshold); current_odom_ = OdometryWithCovariance::Uninitialized(); + utm_zone_ = ""; LOG(INFO) << "[GLIDER] Using IMU frame: " << frame_; LOG(INFO) << "[GLIDER] Using Fixed Lag Smoother: " << std::boolalpha << params.smooth; @@ -49,7 +53,7 @@ void Glider::initializeLogging(const Parameters& params) const } -void Glider::addGps(int64_t timestamp, Eigen::Vector3d& gps) +void Glider::addGps(int64_t timestamp, Eigen::Vector3d& gps, const double sigma) { // route the if (use_dgpsfm_) @@ -60,89 +64,117 @@ void Glider::addGps(int64_t timestamp, Eigen::Vector3d& gps) // transform from lat lon To UTM Eigen::Vector3d meas = Eigen::Vector3d::Zero(); - + double easting, northing; char zone[4]; geodetics::LLtoUTM(gps(0), gps(1), northing, easting, zone); - + utm_zone_ = std::string(zone); + // keep everything in the enu frame meas.head(2) << easting, northing; meas(2) = gps(2); - // TODO t_imu_gps_ needs to be rotated!! - meas = meas + t_imu_gps_; + // Convert the antenna position to the body origin. + if (current_odom_.isInitialized()) + meas -= current_odom_.getOrientation().toRotationMatrix() * t_body_gps_; - factor_manager_.addGpsFactor(timestamp, meas); + factor_manager_.addGpsFactor(timestamp, meas, sigma); } -void Glider::addGpsWithHeading(int64_t timestamp, Eigen::Vector3d& gps, Eigen::Vector2d& heading) +void Glider::addGpsWithHeading(int64_t timestamp, Eigen::Vector3d& gps, Eigen::Vector2d& heading, const double sigma) { // transform from lat lon To UTM Eigen::Vector3d meas = Eigen::Vector3d::Zero(); - + double easting, northing; char zone[4]; geodetics::LLtoUTM(gps(0), gps(1), northing, easting, zone); - + utm_zone_ = std::string(zone); + // keep everything in the enu frame meas.head(2) << easting, northing; meas(2) = gps(2); + const double body_heading = heading.x() + gps_heading_offset_; + const Eigen::Matrix3d r_enu_body = + Eigen::AngleAxisd(body_heading, Eigen::Vector3d::UnitZ()).toRotationMatrix(); + meas -= r_enu_body * t_body_gps_; + if (factor_manager_.isSystemInitialized()) { - factor_manager_.addGpsFactor(timestamp, meas, heading.x(), true); + factor_manager_.addGpsFactor(timestamp, meas, body_heading, true, sigma, heading.y()); } else { - factor_manager_.addGpsFactor(timestamp, meas, 0.0, false); + factor_manager_.addGpsFactor(timestamp, meas, 0.0, false, sigma); } } -void Glider::addGpsWithHeading(int64_t timestamp, Eigen::Vector3d& gps) +void Glider::addGpsWithHeading(int64_t timestamp, Eigen::Vector3d& gps, const double sigma) { // transform from lat lon To UTM Eigen::Vector3d meas = Eigen::Vector3d::Zero(); - + double easting, northing; char zone[4]; geodetics::LLtoUTM(gps(0), gps(1), northing, easting, zone); - + utm_zone_ = std::string(zone); + // keep everything in the enu frame meas.head(2) << easting, northing; meas(2) = gps(2); - // TODO t_imu_gps_ needs to be rotated!! - meas = meas + t_imu_gps_; - + if (current_odom_.isInitialized()) + meas -= current_odom_.getOrientation().toRotationMatrix() * t_body_gps_; + if(factor_manager_.isSystemInitialized() && current_odom_.isMovingFasterThan(dgps_.getVelocityThreshold())) { double heading = dgps_.getHeading(gps); - factor_manager_.addGpsFactor(timestamp, meas, heading, true); + factor_manager_.addGpsFactor(timestamp, meas, heading, true, sigma); } else { dgps_.setLastGps(gps); - factor_manager_.addGpsFactor(timestamp, meas, 0.0, false); + factor_manager_.addGpsFactor(timestamp, meas, 0.0, false, sigma); } } void Glider::addImu(int64_t timestamp, Eigen::Vector3d& accel, Eigen::Vector3d& gyro, Eigen::Vector4d& quat) { - if (frame_ == "ned") - { - Eigen::Vector3d accel_enu = r_enu_ned_ * accel; - Eigen::Vector3d gyro_enu = r_enu_ned_ * gyro; - Eigen::Vector4d quat_enu = rotateQuaternion(r_enu_ned_, quat); + Eigen::Vector3d accel_sensor = accel; + Eigen::Vector3d gyro_sensor = gyro; + Eigen::Vector4d quat_enu_sensor = quat; - factor_manager_.addImuFactor(timestamp, accel_enu, gyro_enu, quat_enu); - } - else if (frame_ == "enu") + if (frame_ == "ned") { - factor_manager_.addImuFactor(timestamp, accel, gyro, quat); + accel_sensor = r_enu_ned_ * accel; + gyro_sensor = r_enu_ned_ * gyro; + quat_enu_sensor = rotateQuaternion(r_enu_ned_, quat); } - else + else if (frame_ != "enu") { LOG(FATAL) << "[GLIDER] IMU Frame, not supported use ENU or NED"; } -} + + // Transform IMU measurements and orientation into the body frame. + Eigen::Vector3d accel_body = r_body_imu_ * accel_sensor; + Eigen::Vector3d gyro_body = r_body_imu_ * gyro_sensor; + const Eigen::Quaterniond q_enu_sensor(quat_enu_sensor(0), quat_enu_sensor(1), + quat_enu_sensor(2), quat_enu_sensor(3)); + const Eigen::Matrix3d r_enu_body = q_enu_sensor.normalized().toRotationMatrix() * r_body_imu_.transpose(); + const Eigen::Quaterniond q_enu_body(r_enu_body); + Eigen::Vector4d quat_enu_body(q_enu_body.w(), q_enu_body.x(), q_enu_body.y(), q_enu_body.z()); + factor_manager_.addImuFactor(timestamp, accel_body, gyro_body, quat_enu_body); +} + +bool Glider::addOdom(int64_t timestamp, const Eigen::Isometry3d& pose) +{ + return factor_manager_.addOdomFactor(timestamp, pose); +} + +bool Glider::addOdom(int64_t timestamp, const Eigen::Isometry3d& pose, + const Eigen::Vector3d& velocity, double velocity_sigma) +{ + return factor_manager_.addOdomFactor(timestamp, pose, velocity, velocity_sigma); +} void Glider::addLandmark(int64_t timestamp, size_t lid, const Eigen::Vector3d& utm, const Eigen::Matrix3d& cov) { @@ -154,6 +186,11 @@ PointWithCovariance Glider::getLandmark(size_t lid) return factor_manager_.getLandmarkPoint(lid); } +Eigen::Vector3d Glider::getGpsOffset() const +{ + return factor_manager_.getGpsOffset(); +} + Odometry Glider::interpolate(int64_t timestamp) { try @@ -171,7 +208,7 @@ Odometry Glider::interpolate(int64_t timestamp) OdometryWithCovariance Glider::optimize(int64_t timestamp) { try - { + { current_odom_ = factor_manager_.runner(timestamp); return current_odom_; } diff --git a/glider/src/odometry.cpp b/glider/src/odometry.cpp index f7fd31e..bd71b4a 100644 --- a/glider/src/odometry.cpp +++ b/glider/src/odometry.cpp @@ -2,7 +2,7 @@ * Jason Hughes * May 2025 * -* Struct to keep track of the robots odometry, +* Struct to keep track of the robots odometry, * not its full state. */ @@ -20,7 +20,7 @@ Odometry::Odometry(gtsam::Values& vals, int64_t timestamp, gtsam::Key key, bool altitude_ = pose_.translation().z(); heading_ = pose_.rotation().yaw(); - + timestamp_ = timestamp; initialized_ = init; } @@ -50,6 +50,11 @@ bool Odometry::isInitialized() const return initialized_; } +bool Odometry::isGpsOffsetInitialized() const +{ + return is_gps_offset_initialized_; +} + gtsam::NavState Odometry::getNavState() const { gtsam::NavState ns(pose_, velocity_); @@ -64,7 +69,7 @@ int64_t Odometry::getTimestamp() const template T Odometry::getPose() const { - if constexpr (std::is_same_v) + if constexpr (std::is_same_v) { return pose_; } @@ -128,7 +133,7 @@ T Odometry::getPosition() const return p; } else - { + { static_assert(std::is_same_v || std::is_same_v, "unsupported type"); } @@ -139,7 +144,7 @@ T Odometry::getVelocity() const { if constexpr (std::is_same_v) { - return velocity_; + return velocity_; } else if constexpr (std::is_same_v) { @@ -147,7 +152,7 @@ T Odometry::getVelocity() const return v; } else - { + { static_assert(std::is_same_v || std::is_same_v, "unsupported type"); } @@ -202,9 +207,9 @@ double Odometry::getLongitude(const char* zone) return longitude_; } -std::pair Odometry::getLatLon(const char* zone) +std::pair Odometry::getLatLon(const char* zone, const Eigen::Vector3d& offset) { - geodetics::UTMtoLL(position_.y(), position_.x(), zone, latitude_, longitude_); + geodetics::UTMtoLL(position_.y() + offset(1), position_.x() + offset(0), zone, latitude_, longitude_); return std::make_pair(latitude_, longitude_); } @@ -221,7 +226,7 @@ double Odometry::getAltitude() const double Odometry::getHeadingDegrees() const { double heading_deg = (heading_ * 180.0) / M_PI; - if (heading_deg < 0.0) + if (heading_deg < 0.0) { heading_deg += 360.0; } @@ -238,6 +243,11 @@ void Odometry::setInitializedStatus(bool init) initialized_ = init; } +void Odometry::setGpsOffsetInitialized(bool init) +{ + is_gps_offset_initialized_ = init; +} + template gtsam::Pose3 Odometry::getPose() const; template Eigen::Isometry3d Odometry::getPose() const; template std::pair Odometry::getPose>() const; diff --git a/glider/src/odometry_with_covariance.cpp b/glider/src/odometry_with_covariance.cpp index a128ae8..ed82403 100644 --- a/glider/src/odometry_with_covariance.cpp +++ b/glider/src/odometry_with_covariance.cpp @@ -14,18 +14,19 @@ using namespace Glider; OdometryWithCovariance::OdometryWithCovariance(gtsam::Values& vals, int64_t timestamp, gtsam::Key key, gtsam::Matrix& pose_cov, gtsam::Matrix& velocity_cov, bool init) : Odometry(vals, timestamp, key, init) { gtsam::imuBias::ConstantBias bias = vals.at(B(key)); - + key_index_ = key; accelerometer_bias_ = bias.accelerometer(); gyroscope_bias_ = bias.gyroscope(); pose_covariance_ = pose_cov; velocity_covariance_ = velocity_cov; - position_covariance_ = pose_cov.block<3,3>(0,0); + // GTSAM Pose3 tangent covariance is ordered [rotation, translation]. + position_covariance_ = pose_cov.block<3,3>(3,3); is_moving_ = (velocity_.norm() > 0.01) ? true : false; - initialized_ = init; + is_gps_offset_initialized_ = false; } OdometryWithCovariance OdometryWithCovariance::Uninitialized() @@ -62,7 +63,7 @@ T OdometryWithCovariance::getBias() const { Eigen::Vector3d ab = accelerometer_bias_; Eigen::Vector3d gb = gyroscope_bias_; - + return std::make_pair(ab, gb); } else @@ -79,9 +80,9 @@ T OdometryWithCovariance::getAccelerometerBias() const if constexpr (std::is_same_v) { return accelerometer_bias_; - } + } else if constexpr (std::is_same_v) - { + { Eigen::Vector3d bias = accelerometer_bias_; return bias; } @@ -101,7 +102,7 @@ T OdometryWithCovariance::getGyroscopeBias() const } else if constexpr (std::is_same_v) { - Eigen::Vector3d bias = gyroscope_bias_; + Eigen::Vector3d bias = gyroscope_bias_; return bias; } else diff --git a/glider/src/parameters.cpp b/glider/src/parameters.cpp index 6bad058..82f12fa 100644 --- a/glider/src/parameters.cpp +++ b/glider/src/parameters.cpp @@ -7,6 +7,25 @@ #include "glider/utils/parameters.hpp" +#include + +namespace +{ +Eigen::Vector3d loadTranslation(const YAML::Node& node) +{ + return {node["x"].as(), node["y"].as(), node["z"].as()}; +} + +Eigen::Matrix3d loadRpyDegrees(const YAML::Node& node) +{ + const double scale = M_PI / 180.0; + const Eigen::AngleAxisd roll(node["roll"].as() * scale, Eigen::Vector3d::UnitX()); + const Eigen::AngleAxisd pitch(node["pitch"].as() * scale, Eigen::Vector3d::UnitY()); + const Eigen::AngleAxisd yaw(node["yaw"].as() * scale, Eigen::Vector3d::UnitZ()); + return (yaw * pitch * roll).toRotationMatrix(); +} +} + Glider::Parameters::Parameters(const std::string& path) { @@ -22,15 +41,16 @@ Glider::Parameters::Parameters(const std::string& path) integration_cov = config["imu"]["covariances"]["integration"].as(); bias_cov = config["imu"]["covariances"]["bias"].as(); gps_noise = config["gps"]["covariance"].as(); - + odom_cov = config["odom"]["covariance"].as(); + // constants gravity = config["constants"]["gravity"].as(); bias_num_measurements = config["constants"]["bias_num_measurements"].as(); initial_num_measurements = config["constants"]["initial_num_measurements"].as(); frame = config["imu"]["frame"].as(); - - log = config["logging"]["stdout"].as(); + + log = config["logging"]["stdout"].as(); log_dir = config["logging"]["directory"].as(); smooth = config["optimizer"]["smooth"].as(); @@ -43,9 +63,21 @@ Glider::Parameters::Parameters(const std::string& path) use_dgps = config["dgps"]["enable"].as(); dgps_cov = config["dgps"]["covariance"].as(); - t_imu_gps(0) = config["gps_to_imu"]["x"].as(); - t_imu_gps(1) = config["gps_to_imu"]["y"].as(); - t_imu_gps(2) = config["gps_to_imu"]["z"].as(); + const YAML::Node extrinsics = config["extrinsics"]; + const Eigen::Vector3d t_reference_body = loadTranslation(extrinsics["body"]["translation"]); + const Eigen::Matrix3d r_reference_body = loadRpyDegrees(extrinsics["body"]["rotation_rpy_deg"]); + const auto toBodyTranslation = [&](const YAML::Node& sensor) { + return r_reference_body.transpose() * (loadTranslation(sensor["translation"]) - t_reference_body); + }; + const auto toBodyRotation = [&](const YAML::Node& sensor) { + return r_reference_body.transpose() * loadRpyDegrees(sensor["rotation_rpy_deg"]); + }; + + body_frame = extrinsics["body"]["frame"].as(); + t_body_imu = toBodyTranslation(extrinsics["imu"]); + r_body_imu = toBodyRotation(extrinsics["imu"]); + t_body_gps = toBodyTranslation(extrinsics["gps"]); + gps_heading_offset = extrinsics["gps"]["heading_offset_deg"].as() * M_PI / 180.0; } catch (const YAML::Exception& e) { @@ -54,7 +86,7 @@ Glider::Parameters::Parameters(const std::string& path) catch (const std::exception& e) { throw std::runtime_error("Error parsing YAML configuration at: " + path + " : " + std::string(e.what())); - } + } } Glider::Parameters Glider::Parameters::Load(const std::string& path) diff --git a/glider/test/test_factor_manager.cpp b/glider/test/test_factor_manager.cpp index 32f64c9..8197061 100644 --- a/glider/test/test_factor_manager.cpp +++ b/glider/test/test_factor_manager.cpp @@ -25,13 +25,13 @@ TEST(FactorManagerTestSuite, ImuInitialization) // assert its initialized ASSERT_TRUE(manager.isImuInitialized()); - + // get bias and compute mean Eigen::MatrixXd bias_est = manager.getBiasEstimate(); Eigen::Vector3d accel_bias(bias_est.colwise().mean().head(3)); Eigen::Vector3d gyro_bias(bias_est.colwise().mean().tail(3)); - + Eigen::Vector3d accel_gt(0.0, 0.0, 0.0); Eigen::Vector3d gyro_gt(0.0, 0.0, 0.0); @@ -40,8 +40,29 @@ TEST(FactorManagerTestSuite, ImuInitialization) ASSERT_EQ(gyro_bias, gyro_gt); } +TEST(FactorManagerTestSuite, ImuInitializationWithRotatedMount) +{ + Glider::Parameters params = Glider::Parameters::Load("../config/glider-params.yaml"); + Glider::FactorManager manager(params); + + // A +90 degree body-to-ENU rotation about X maps body +Y to ENU +Z. + const double s = std::sqrt(0.5); + Eigen::Vector4d orient(s, s, 0.0, 0.0); + for (int i = 0; i < params.bias_num_measurements + 1; ++i) + { + Eigen::Vector3d accel(0.0, params.gravity, 0.0); + Eigen::Vector3d gyro = Eigen::Vector3d::Zero(); + manager.addImuFactor(i, accel, gyro, orient); + } + + ASSERT_TRUE(manager.isImuInitialized()); + const Eigen::Vector3d accel_bias = + manager.getBiasEstimate().colwise().mean().head(3); + EXPECT_LT(accel_bias.norm(), 1e-9); +} + TEST(FactorManagerTestSuite, PimParameters) -{ +{ // initialized glider factor manager and params Glider::Parameters params = Glider::Parameters::Load("../config/glider-params.yaml"); Glider::FactorManager manager(params); @@ -55,20 +76,20 @@ TEST(FactorManagerTestSuite, PimParameters) int64_t timestamp = i; manager.addImuFactor(timestamp, accel, gyro, orient); } - // make sure the imu is initialized, + // make sure the imu is initialized, // otherwise nothing will get added to the pim ASSERT_TRUE(manager.isImuInitialized()); // add 10 basic measurements to the pim for (int i = 0; i < 10; ++i) - { + { Eigen::Vector3d accel(0.0, 0.0, params.gravity); Eigen::Vector3d gyro(0.0, 0.0, 0.0); Eigen::Vector4d orient(1.0, 0.0, 0.0, 0.0); int64_t timestamp = i; manager.addImuFactor(timestamp, accel, gyro, orient); } - + // get the pim from the factor manager gtsam::PreintegratedCombinedMeasurements pim = manager.getPim(); @@ -81,7 +102,7 @@ TEST(FactorManagerTestSuite, PimParameters) } TEST(FactorManagerTestSuite, KeyIndex) -{ +{ // set default lat lon double lat = 39.941279; double lon = -75.199197; @@ -89,7 +110,7 @@ TEST(FactorManagerTestSuite, KeyIndex) // initialized glider factor manager and params Glider::Parameters params = Glider::Parameters::Load("../config/glider-params.yaml"); Glider::FactorManager manager(params); - + ASSERT_EQ(manager.getKeyIndex(), 0); // provide measurements for initialization for (int i = 0; i < params.bias_num_measurements + 1; ++i) @@ -101,10 +122,10 @@ TEST(FactorManagerTestSuite, KeyIndex) manager.addImuFactor(timestamp, accel, gyro, orient); } ASSERT_EQ(manager.getKeyIndex(), 0); - + Eigen::Vector3d meas(lat, lon, 0.0); manager.addGpsFactor(1, meas); - + ASSERT_EQ(manager.getKeyIndex(), 1); } @@ -117,7 +138,7 @@ TEST(FactorManagerTestSuite, GPSInitialization) // initialized glider factor manager and params Glider::Parameters params = Glider::Parameters::Load("../config/glider-params.yaml"); Glider::FactorManager manager(params); - + // provide imu measurements for initialization for (int i = 0; i < params.bias_num_measurements + 1; ++i) { @@ -137,7 +158,7 @@ TEST(FactorManagerTestSuite, GPSInitialization) } TEST(FactorManagerTestSuite, SystemInitialization) -{ +{ // set default lat lon double lat = 39.941279; double lon = -75.199197; @@ -165,7 +186,7 @@ TEST(FactorManagerTestSuite, SystemInitialization) int64_t timestamp = 1; Glider::OdometryWithCovariance state = manager.runner(timestamp); } - + // After adding the specified amount of gps measurements // the system should initialize ASSERT_TRUE(manager.isSystemInitialized()); diff --git a/glider/test/test_odometry_w_cov.cpp b/glider/test/test_odometry_w_cov.cpp index ec6f9c0..946295b 100644 --- a/glider/test/test_odometry_w_cov.cpp +++ b/glider/test/test_odometry_w_cov.cpp @@ -7,6 +7,7 @@ */ #include +#include #include "glider/core/odometry_with_covariance.hpp" #include "glider/core/factor_manager.hpp" @@ -75,7 +76,8 @@ TEST(OdometryWithCovarainceTestSuite, TestCovariances) Glider::FactorManager manager(params); Glider::OdometryWithCovariance odom; - + int64_t timestamp = 0; + for (uint64_t i = 0; i < params.initial_num_measurements + 1; ++i) { // provide imu measurements for initialization @@ -84,42 +86,25 @@ TEST(OdometryWithCovarainceTestSuite, TestCovariances) Eigen::Vector3d accel(AX, AY, AZ); Eigen::Vector3d gyro(GX, GY, GZ); Eigen::Vector4d orient(QW, QX, QY, QZ); - int64_t timestamp = (i+1) * (j+1); + timestamp += 10000000; manager.addImuFactor(timestamp, accel, gyro, orient); } Eigen::Vector3d meas(LATITUDE, LONGITUDE, TZ); - manager.addGpsFactor(i+1, meas); - odom = manager.runner(1); + manager.addGpsFactor(timestamp, meas); + odom = manager.runner(timestamp); } - const double EPSILON = 1e-10; - // test pose covariance is greater than zero - Eigen::MatrixXd pose_cov = odom.getPoseCovariance(); - for (double& c : pose_cov.reshaped()) - { - if (std::abs(c) < EPSILON) c = 0.0; - ASSERT_GE(c, 0.0); - } - ASSERT_GT(pose_cov.sum(), 0.0); - - // test position covariance - Eigen::MatrixXd pos_cov = odom.getPositionCovariance(); - for (double& c : pos_cov.reshaped()) - { - if (std::abs(c) < EPSILON) c = 0.0; - - ASSERT_GE(c, 0.0); - } - ASSERT_GT(pos_cov.sum(), 0.0); + const auto expect_valid_covariance = [](const Eigen::MatrixXd& covariance) { + EXPECT_TRUE(covariance.isApprox(covariance.transpose(), 1e-9)); + Eigen::SelfAdjointEigenSolver solver(covariance); + ASSERT_EQ(solver.info(), Eigen::Success); + EXPECT_GE(solver.eigenvalues().minCoeff(), -1e-9); + EXPECT_GT(covariance.diagonal().sum(), 0.0); + }; - // test velocity covariance - Eigen::MatrixXd vel_cov = odom.getVelocityCovariance(); - for (double& c : vel_cov.reshaped()) - { - if (std::abs(c) < EPSILON) c = 0.0; - ASSERT_GE(c, 0.0); - } - ASSERT_GT(vel_cov.sum(), 0.0); + expect_valid_covariance(odom.getPoseCovariance()); + expect_valid_covariance(odom.getPositionCovariance()); + expect_valid_covariance(odom.getVelocityCovariance()); } TEST(OdometryWithCovarianceTestSuite, TestBiases) @@ -191,13 +176,14 @@ TEST(OdometryWithCovarianceTestSuite, TestBiases) } TEST(OdometryWithCovarianceTestSuite, TestKeyIndex) -{ +{ // initialized glider factor manager and params Glider::Parameters params = Glider::Parameters::Load("../config/glider-params.yaml"); Glider::FactorManager manager(params); Glider::OdometryWithCovariance odom; - + int64_t timestamp = 0; + for (uint64_t i = 0; i < params.initial_num_measurements + 1; ++i) { // provide imu measurements for initialization @@ -206,12 +192,12 @@ TEST(OdometryWithCovarianceTestSuite, TestKeyIndex) Eigen::Vector3d accel(AX, AY, AZ); Eigen::Vector3d gyro(GX, GY, GZ); Eigen::Vector4d orient(QW, QX, QY, QZ); - int64_t timestamp = (i+1) * (j+1); + timestamp += 10000000; manager.addImuFactor(timestamp, accel, gyro, orient); } Eigen::Vector3d meas(LATITUDE, LONGITUDE, TZ); - manager.addGpsFactor(i+1, meas); - odom = manager.runner(1); + manager.addGpsFactor(timestamp, meas); + odom = manager.runner(timestamp); } ASSERT_EQ(odom.getKeyIndex(), params.initial_num_measurements); diff --git a/run.bash b/run.bash new file mode 100755 index 0000000..8142819 --- /dev/null +++ b/run.bash @@ -0,0 +1,36 @@ +#!/bin/bash + +## Adding for convenience, needs to be removed later, allows the script to exit immediately on error +set -e + +xhost_enabled=false +if [ -n "${DISPLAY:-}" ] && command -v xhost >/dev/null 2>&1; then + xhost +SI:localuser:"$(whoami)" >/dev/null + xhost_enabled=true +fi + +cleanup_xhost() +{ + if [ "$xhost_enabled" = true ]; then + xhost -SI:localuser:"$(whoami)" >/dev/null 2>&1 || true + fi +} +trap cleanup_xhost EXIT INT TERM + +container="dtc-jackal-$(hostname)-glider" +if docker container inspect "$container" >/dev/null 2>&1; then + echo "[GLIDER] Removing existing container '$container'..." + docker rm -f "$container" >/dev/null +fi + +docker run --rm -it --gpus all \ + --privileged \ + --network=host \ + -u $UID \ + -e RUN=true \ + -e USE_SIM_TIME="${USE_SIM_TIME:-false}" \ + -e DISPLAY="${DISPLAY:-}" \ + -v /tmp/.X11-unix:/tmp/.X11-unix:rw \ + -v "$(cd .. && pwd):/data:ro" \ + --name "$container" \ + dtc-jackal-$(hostname):glider