diff --git a/Dockerfile b/Dockerfile new file mode 100644 index 0000000..0a5b1df --- /dev/null +++ b/Dockerfile @@ -0,0 +1,39 @@ +# Start from ROS Jazzy + CUDA base +FROM dtcpronto/ros-jazzy:cuda + +USER root + +RUN apt-get update && apt-get install -y --no-install-recommends \ + vim \ + tmux \ + cmake \ + gcc \ + g++ \ + git \ + build-essential \ + sudo \ + wget \ + curl \ + zip \ + unzip \ + ros-jazzy-gtsam \ + ros-jazzy-gps-msgs \ + python3-colcon-common-extensions \ + libgoogle-glog-dev \ + && rm -rf /var/lib/apt/lists/* + +# Switch to dtc user +USER dtc +WORKDIR /home/dtc/ws + +# Clone glider +COPY --chown=dtc:dtc ./glider /home/dtc/ws/src/glider + +# Build workspace +RUN /bin/bash -c "source /opt/ros/jazzy/setup.bash && \ + colcon build --symlink-install" + +COPY --chown=dtc:dtc ./entrypoint.bash /home/dtc/entrypoint.bash +RUN chmod +x /home/dtc/entrypoint.bash + +ENTRYPOINT ["/home/dtc/entrypoint.bash"] \ No newline at end of file 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/entrypoint.bash b/entrypoint.bash new file mode 100755 index 0000000..328f8fc --- /dev/null +++ b/entrypoint.bash @@ -0,0 +1,22 @@ +#!/bin/bash + +source /opt/ros/jazzy/setup.bash +source /home/dtc/ws/install/setup.bash + +if [ "$RMW_IMPLEMENTATION" = "rmw_zenoh_cpp" ]; then + echo "[GLIDER] Starting Zenoh router..." + ros2 run rmw_zenoh_cpp rmw_zenohd > /tmp/zenoh_router.log 2>&1 & + sleep 2 +fi + +if [ "$RUN" = "true" ]; then + echo "[GLIDER] Starting foxglove_bridge..." + nohup ros2 run foxglove_bridge foxglove_bridge --ros-args -p address:='0.0.0.0' -p port:=8765 > /dev/null 2>&1 & + sleep 3 + echo "[GLIDER] Launching glider..." + ros2 launch glider glider-node.launch.py +else + echo "[GLIDER] RUN=false, keeping container alive..." +fi + +exec "$@" \ No newline at end of file diff --git a/glider/CMakeLists.txt b/glider/CMakeLists.txt index f0e1550..9f49616 100644 --- a/glider/CMakeLists.txt +++ b/glider/CMakeLists.txt @@ -44,6 +44,8 @@ if (BUILD_ROS) find_package(std_msgs REQUIRED) find_package(nav_msgs REQUIRED) find_package(gps_msgs REQUIRED) + find_package(geometry_msgs REQUIRED) + find_package(tf2_ros REQUIRED) set(node_plugins "") endif() @@ -97,6 +99,7 @@ if (BUILD_ROS) geometry_msgs nav_msgs gps_msgs + tf2_ros ) add_executable(${PROJECT_NAME}_node diff --git a/glider/config/glider-params.yaml b/glider/config/glider-params.yaml index 1e77d8a..a65b69b 100644 --- a/glider/config/glider-params.yaml +++ b/glider/config/glider-params.yaml @@ -9,9 +9,12 @@ imu: frame: "enu" gps: covariance: 2.0 +odom: + covariance: 0.1 dgps: enable: true covariance: 0.03 + rejection_limit: 1.7 dgpsfm: enable: false integration_threshold: 1.0 diff --git a/glider/config/ros-params.yaml b/glider/config/ros-params.yaml index 0a06d4c..695e268 100644 --- a/glider/config/ros-params.yaml +++ b/glider/config/ros-params.yaml @@ -2,10 +2,15 @@ glider_node: ros__parameters: publishers: rate: 0.0 - nav_sat_fix: false + nav_sat_fix: true + utm_zone: "18S" # Pennovation : 18S, College station : 14R + map_frame: "map" + odom_frame: "rko_odom" + base_link_frame: "base_link" viz: use: true - origin_easting: 753912.0063845584 + origin_easting: 753912.0063845584 # These needs to be changed based on the location origin_northing: 3385461.6073698294 subscribers: - use_odom: false + use_odom: true + dgps_topic: "/dgps/antenna1/fix" diff --git a/glider/include/glider/core/factor_manager.hpp b/glider/include/glider/core/factor_manager.hpp index 521aa58..54ef79d 100644 --- a/glider/include/glider/core/factor_manager.hpp +++ b/glider/include/glider/core/factor_manager.hpp @@ -62,6 +62,9 @@ class FactorManager * factor manager * @param params: the parameters loaded from the yaml file*/ FactorManager(const Parameters& params); + /*! @brief initializes all parameters in the factor manager + * @param params: the parameters loaded from the yaml file*/ + void initialize(const Parameters& params); // state predictors /*! @brief calls the pim predict method @@ -79,20 +82,26 @@ class FactorManager /*! @brief adds the gps measurement and pim to the factor graph * @param timestamp: time of the gps measurement * @param gps: GPS measurement in the UTM frame */ - void addGpsFactor(int64_t timestamp, const Eigen::Vector3d& gps); + void addGpsFactor(int64_t timestamp, const Eigen::Vector3d& gps, const double sigma = 0.0); /*! @brief adds the gps measurement and a heading from dgps * @param timestamp: time of the gps measurement * @param gps: GPS measurement in the UTM frame * @param heading: heading from dgpsfm in the ENU frame * @param fuse: whether or not to add the heading measurement - * to the factor graph */ - void addGpsFactor(int64_t timestamp, const Eigen::Vector3d& gps, const double& heading, const bool fuse); + * to the factor graph + * @param sigma: the standard deviation of the gps measurement, if 0 use param */ + void addGpsFactor(int64_t timestamp, const Eigen::Vector3d& gps, const double& heading, const bool fuse, const double sigma = 0.0); /*! @brief adds the imu measurements to the pim and saves the orientation * @param timestamp: time of the imu measurement * @param accel: the accelerometer reading * @param gyro: gyroscopre reading * @param orient: orientation in quaternion (w,x,y,z) format */ void addImuFactor(int64_t timestamp, const Eigen::Vector3d& accel, const Eigen::Vector3d& gyro, const Eigen::Vector4d& orient); + /*! @brief adds an odometry measurement + * @param timestamp: time of the odometry + * @param odom: estimated odometry pose + * @return true if a graph node was created */ + bool addOdomFactor(int64_t timestamp, const Eigen::Isometry3d& odom); /*! @brief adds a landmark factor for an estimated utm point and covariance * @param timestamp: time of the landmark measurements * @param landmark_id: a unique id for the landmark @@ -127,6 +136,16 @@ class FactorManager /*! @brief gets the key index * @return the current key index */ gtsam::Key getKeyIndex() const; + /*! @brief gets the gps offset + * @return the 3D gps offset */ + Eigen::Vector3d getGpsOffset() const; + /*! @brief checks if the gps offset has been initialized + * @return true if the gps offset has been initialized else false */ + bool isGpsOffsetInitialized() const; + + /*! @brief gets the current parameters + * @return the current parameters */ + const Parameters& params() const { return params_; } private: /*! @brief handles the optimization call with the specified @@ -151,6 +170,8 @@ class FactorManager // @brief a mutex to use accross function that access the pim // as the pim could be accessd by multiple threads static std::mutex mutex_; + // @brief a mutex to protect the factor graph and its variables + mutable std::mutex graph_mutex_; // parameters // @brief parameters for the isam2 optimizer @@ -186,6 +207,8 @@ class FactorManager gtsam::noiseModel::Base::shared_ptr orient_noise_; // @brief noise in the heading estimate of differential gps gtsam::noiseModel::Base::shared_ptr dgpsfm_noise_; + // @brief noise for the odometry constraints + gtsam::noiseModel::Diagonal::shared_ptr odom_noise_; // factor graph // @brief tracks the number of times the optimizer has been called @@ -222,6 +245,17 @@ class FactorManager bool imu_initialized_; // @param tracks if a gps measurement has been received bool gps_initialized_; + // @param tracks if an odom measurement has been received + bool odom_initialized_; + // @param true if the first graph node originated from Odometry (LIO), making it a local origin + bool using_local_origin_; + bool gps_offset_initialized_; + Eigen::Vector3d gps_offset_; + double last_node_time_; + // @param previous odometry measurement for relative constraints + Eigen::Isometry3d last_odom_meas_; + // @param accumulated odometry transform between nodes + Eigen::Isometry3d accumulated_odom_delta_; // landmark variables std::unordered_map landmark_info_; diff --git a/glider/include/glider/core/glider.hpp b/glider/include/glider/core/glider.hpp index deea476..3d057c8 100644 --- a/glider/include/glider/core/glider.hpp +++ b/glider/include/glider/core/glider.hpp @@ -34,9 +34,20 @@ class Glider * @param gps: gps measurement in (lat, lon, alt) format, * should be in degree decimal and altitude in meters. Altitude * frame does not matter */ - void addGps(int64_t timestamp, Eigen::Vector3d& gps); - void addGpsWithHeading(int64_t timestamp, Eigen::Vector3d& gps); - void addGpsWithHeading(int64_t timestamp, Eigen::Vector3d& gps, Eigen::Vector2d& heading); + void addGps(int64_t timestamp, Eigen::Vector3d& gps, const double sigma = 0.0); + /*! @brief adds the gps measurement and heading info to the factor + * graph + * @param timestamp: time of measurement + * @param gps: lat, lon, alt coordinates + * @param heading: track, error track + * @param sigma: standard deviation of the gps position measurement */ + void addGpsWithHeading(int64_t timestamp, Eigen::Vector3d& gps, Eigen::Vector2d& heading, const double sigma = 0.0); + /*! @brief adds the gps measurement and calculates a heading based on previous + * GPS measurements + * @param timestamp: time of measurement + * @param gps: lat, lon, alt coordinates + * @param sigma: standard deviation of the gps position measurement */ + void addGpsWithHeading(int64_t timestamp, Eigen::Vector3d& gps, const double sigma = 0.0); /*! @brief converts the imu measurements into the ENU frame if * they are not in that frame already. * @param timestamp: time the imu measurement was taken @@ -44,8 +55,19 @@ 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); void addLandmark(int64_t timestamp, size_t lid, const Eigen::Vector3d& utm, const Eigen::Matrix3d& cov); PointWithCovariance getLandmark(size_t lid); + Eigen::Vector3d getGpsOffset() const; + /*! @brief gets the auto-detected UTM zone + * @return the UTM zone string */ + std::string getUtmZone() const { return utm_zone_; } + + const Parameters& params() const { return factor_manager_.params(); } + + bool isGpsInitialized() const { return factor_manager_.isGpsInitialized(); } + bool isGpsOffsetInitialized() const { return factor_manager_.isGpsOffsetInitialized(); } + bool isSystemInitialized() const { return factor_manager_.isSystemInitialized(); } /*! @brief calls the factor manager to interpolate between GPS @@ -92,5 +114,6 @@ class Glider // @brief save the state estimate from // the optimizer OdometryWithCovariance current_odom_; + std::string utm_zone_; }; } diff --git a/glider/include/glider/core/odometry.hpp b/glider/include/glider/core/odometry.hpp index d3d3d84..9cd8559 100644 --- a/glider/include/glider/core/odometry.hpp +++ b/glider/include/glider/core/odometry.hpp @@ -102,6 +102,10 @@ class Odometry * @return true if odometry is initialized otherwise false */ bool isInitialized() const; + /*! @brief check if the gps offset is initialized + * @return true if it is otherwise false */ + bool isGpsOffsetInitialized() const; + /*! @brief get the latitude of the current position * @param zone: the utm zone ex "18S" * @return the latitude in degrees decimal from the UTM position */ @@ -114,7 +118,7 @@ class Odometry * @param zone: the utm zone, ex "18S" * @return latitude and longitude in degrees decimal as a pair * where lat is first and lon is second */ - std::pair 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,6 +128,11 @@ class Odometry * otherwise false */ void setInitializedStatus(bool init); + /*! @brief set the gps offset initalization status + * @param init: true if you want the gps offset to be initialized + * otherwise false */ + void setGpsOffsetInitialized(bool init); + protected: /*! @brief a helper function to convert gtsam Pose3 to a pair * of Eigen objects @@ -157,5 +166,7 @@ class Odometry int64_t timestamp_; // @brief is this initialized, default to false bool initialized_{false}; + // @brief is the gps offset initialized, default to false + bool is_gps_offset_initialized_{false}; }; } // namespace glider diff --git a/glider/include/glider/utils/parameters.hpp b/glider/include/glider/utils/parameters.hpp index 3160331..5bb0ebd 100644 --- a/glider/include/glider/utils/parameters.hpp +++ b/glider/include/glider/utils/parameters.hpp @@ -46,6 +46,8 @@ struct Parameters // @brief covariance of the GPS position estimate // TODO make this gps_cov to match double gps_noise; + // @brief covariance of the odometry position estimate + double odom_cov; // @brief gravity as read from your IMU double gravity; @@ -83,6 +85,7 @@ struct Parameters bool use_dgps; double dgps_cov; + double dgps_rejection_limit; // @brief translation from the GPS to the IMU Eigen::Vector3d t_imu_gps; diff --git a/glider/include/ros/conversions.hpp b/glider/include/ros/conversions.hpp index 26a1262..15bbd22 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, 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, const char* zone = nullptr, const Eigen::Vector3d& offset = Eigen::Vector3d::Zero()); template static void addCovariance(const Glider::OdometryWithCovariance& odom_wc, T& msg); diff --git a/glider/include/ros/glider_node.hpp b/glider/include/ros/glider_node.hpp index 121d5f1..6c7fa91 100644 --- a/glider/include/ros/glider_node.hpp +++ b/glider/include/ros/glider_node.hpp @@ -14,7 +14,9 @@ #include #include #include +#include #include +#include #include "glider/core/glider.hpp" #include "glider/core/odometry.hpp" @@ -37,12 +39,13 @@ class GliderNode : public rclcpp::Node void interpolationCallback(); // subscriber callbacks - void dgpsCallback(const gps_msgs::msg::GPSFix::ConstSharedPtr msg); + void dgpsCallback(const sensor_msgs::msg::NavSatFix::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; @@ -53,12 +56,13 @@ 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_; @@ -68,6 +72,9 @@ class GliderNode : public rclcpp::Node 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_; @@ -76,12 +83,18 @@ class GliderNode : public rclcpp::Node bool initialized_; bool publish_nsf_; bool viz_; + bool use_odom_; std::string utm_zone_; + std::string map_frame_; + std::string odom_frame_; + std::string base_link_frame_; double origin_easting_; double origin_northing_; double freq_; // tracker Glider::OdometryWithCovariance current_state_; + Eigen::Isometry3d last_odom_pose_; + bool has_odom_{false}; }; } diff --git a/glider/launch/glider-node.launch.py b/glider/launch/glider-node.launch.py index 7e05987..0d7b509 100644 --- a/glider/launch/glider-node.launch.py +++ b/glider/launch/glider-node.launch.py @@ -59,12 +59,12 @@ def generate_launch_description(): ros_params_file, {'path': graph_params_file, 'use_sim_time': use_sim_time, - 'use_odom': False} + 'use_odom': True} ], remappings=[ - ('/dgps', '/dgps/fix'), + ('/dgps', '/dgps/converted'), ('/imu', '/vectornav/imu'), - ('/odom', '/Odometry'), + ('/odom', '/rko_lio/odometry'), ] ) diff --git a/glider/package.xml b/glider/package.xml index 849bb37..a35c725 100644 --- a/glider/package.xml +++ b/glider/package.xml @@ -17,6 +17,7 @@ sensor_msgs nav_msgs gps_msgs + tf2_ros tf2_eigen message_filters diff --git a/glider/ros/conversions.cpp b/glider/ros/conversions.cpp index 0034c04..8cd20f5 100644 --- a/glider/ros/conversions.cpp +++ b/glider/ros/conversions.cpp @@ -175,24 +175,28 @@ 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) + 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; } @@ -216,7 +220,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,22 +232,26 @@ 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) @@ -253,7 +261,7 @@ Output Conversions::odomToRos(Glider::OdometryWithCovariance& odom_wc, const cha 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; } @@ -295,7 +303,7 @@ Output Conversions::odomToRos(Glider::OdometryWithCovariance& odom_wc, const cha 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 @@ -371,11 +379,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..91096f3 100644 --- a/glider/ros/glider_node.cpp +++ b/glider/ros/glider_node.cpp @@ -15,7 +15,11 @@ 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.use_odom", false); declare_parameter("path", ""); @@ -27,12 +31,17 @@ 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(); - bool use_odom = this->get_parameter("subscribers.use_odom").as_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); @@ -51,28 +60,29 @@ GliderNode::GliderNode(const rclcpp::NodeOptions& options) : rclcpp::Node("glide 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, 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 +90,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_); @@ -100,7 +113,8 @@ void GliderNode::interpolationCallback() int64_t timestamp = getTime(this->now()); Glider::Odometry odom = glider_->interpolate(timestamp); - (publish_nsf_) ? publishNavSatFix(odom) : publishOdometry(odom); + if (publish_nsf_) publishNavSatFix(odom); + publishOdometry(odom); } void GliderNode::imuCallback(const sensor_msgs::msg::Imu::ConstSharedPtr msg) @@ -116,73 +130,174 @@ void GliderNode::imuCallback(const sensor_msgs::msg::Imu::ConstSharedPtr msg) if (freq_ == 0 && current_state_.isInitialized()) { Glider::Odometry odom = glider_->interpolate(timestamp); - (publish_nsf_) ? publishNavSatFix(odom) : publishOdometry(odom); + if (publish_nsf_) publishNavSatFix(odom); + publishOdometry(odom); } } -void GliderNode::dgpsCallback(const gps_msgs::msg::GPSFix::ConstSharedPtr msg) +void GliderNode::dgpsCallback(const sensor_msgs::msg::NavSatFix::ConstSharedPtr msg) { + if (msg->status.status < sensor_msgs::msg::NavSatStatus::STATUS_FIX || msg->position_covariance[0] < 1e-6) + { + LOG_FIRST_N(WARNING, 5) << "[GLIDER] DGPS ignored: No fix or invalid covariance"; + return; + } + + if (msg->position_covariance[0] > glider_->params().dgps_rejection_limit) + { + LOG_FIRST_N(INFO, 1) << "[GLIDER] DGPS rejected due to high covariance (> " << glider_->params().dgps_rejection_limit << ")"; + return; + } LOG_FIRST_N(INFO, 1) << "[GLIDER] Received DGPS measurement"; - std::pair dgps = GliderROS::Conversions::rosToEigen>(*msg); + Eigen::Vector3d gps = GliderROS::Conversions::rosToEigen(*msg); int64_t timestamp = getTime(msg->header.stamp); - glider_->addGpsWithHeading(timestamp, dgps.first, dgps.second); + double sigma = std::sqrt(msg->position_covariance[0]); + glider_->addGps(timestamp, gps, sigma); current_state_ = glider_->optimize(timestamp); } void GliderNode::gpsCallback(const sensor_msgs::msg::NavSatFix::ConstSharedPtr msg) { + if (msg->status.status < sensor_msgs::msg::NavSatStatus::STATUS_FIX || msg->position_covariance[0] < 1e-6) + { + LOG_FIRST_N(WARNING, 5) << "[GLIDER] GPS ignored: No fix or invalid covariance"; + return; + } + + if (msg->position_covariance[0] > glider_->params().dgps_rejection_limit) + { + LOG_FIRST_N(INFO, 1) << "[GLIDER] GPS rejected due to high covariance (> " << glider_->params().dgps_rejection_limit << ")"; + return; + } LOG_FIRST_N(INFO, 1) << "[GLIDER] Recieved GPS measurement"; Eigen::Vector3d gps = GliderROS::Conversions::rosToEigen(*msg); int64_t timestamp = getTime(msg->header.stamp); - glider_->addGps(timestamp, gps); + double sigma = std::sqrt(msg->position_covariance[0]); + glider_->addGps(timestamp, gps, sigma); current_state_ = glider_->optimize(timestamp); } 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); + int64_t timestamp = getTime(msg->header.stamp); + if (glider_->addOdom(timestamp, pose)) + { + current_state_ = glider_->optimize(timestamp); + } +} + +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; + + // Convert GPS Goal (lat, lon, alt) to UTM map coordinates + double easting, northing; + char zone[10]; + Glider::geodetics::LLtoUTM(msg->latitude, msg->longitude, northing, easting, zone); + Eigen::Vector3d goal_utm(easting, northing, 0.0); + + // Subtract the GPS offset so the goal is in the same frame as the optimizer output. + // Outdoor: offset is (0,0,0) so this is a no-op. + // Indoor (odom-seeded): offset bridges UTM → local frame. + 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); + + // Publish as a local map frame goal pose + 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); + 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); + GliderROS::Conversions::addCovariance(current_state_, msg); gps_pub_->publish(msg); } diff --git a/glider/src/factor_manager.cpp b/glider/src/factor_manager.cpp index 2a8964e..85c526f 100644 --- a/glider/src/factor_manager.cpp +++ b/glider/src/factor_manager.cpp @@ -14,11 +14,22 @@ 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; @@ -34,6 +45,7 @@ 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; @@ -45,6 +57,8 @@ FactorManager::FactorManager(const Parameters& params) isam_params_.relinearizeSkip = 1; 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"; } @@ -88,7 +102,7 @@ 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)"; } } @@ -97,13 +111,36 @@ 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_) + { + gps_offset_ = gps - current_state_.getPose().translation(); + 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 @@ -135,14 +172,21 @@ void FactorManager::addGpsFactor(int64_t timestamp, const Eigen::Vector3d& gps) } // 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_); // save the time for the smoother smoother_timestamps_[X(key_index_)] = time; @@ -153,21 +197,52 @@ void FactorManager::addGpsFactor(int64_t timestamp, const Eigen::Vector3d& gps) 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::Isotropic::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) { - // 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_) + { + gps_offset_ = gps - current_state_.getPose().translation(); + 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 @@ -199,14 +274,21 @@ void FactorManager::addGpsFactor(int64_t timestamp, const Eigen::Vector3d& gps, } // 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_); // save the time for the smoother smoother_timestamps_[X(key_index_)] = time; @@ -215,13 +297,23 @@ void FactorManager::addGpsFactor(int64_t timestamp, const Eigen::Vector3d& gps, // 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); + double heading_rad = heading * M_PI / 180.0; + gtsam::Rot3 rot = gtsam::Rot3::Ypr(heading_rad, 0.0, 0.0); - graph_.add(gtsam::GPSFactor(X(key_index_), gps, gps_noise_)); + + Eigen::Vector3d aligned_gps = gps; + if (using_local_origin_) aligned_gps = gps - gps_offset_; + + gtsam::noiseModel::Isotropic::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)); if (fuse) graph_.addExpressionFactor(gtsam::rotation(X(key_index_)), rot, dgpsfm_noise_); // increment key index key_index_++; + last_node_time_ = time; } @@ -252,8 +344,88 @@ void FactorManager::addImuFactor(int64_t timestamp, const Eigen::Vector3d& accel 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) +{ + 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_), gtsam::Point3(0.0, 0.0, 0.0)); + 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_), gtsam::Point3(0.0, 0.0, 0.0), gtsam::noiseModel::Isotropic::Sigma(3, 0.001))); + 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; + + // only create a new node if we moved > 0.3 meters or rotated > 0.15 rad or if we are trying to initialize the system + double trans_dist = accumulated_odom_delta_.translation().norm(); + Eigen::AngleAxisd angle_axis(accumulated_odom_delta_.rotation()); + double rot_dist = std::abs(angle_axis.angle()); + + double time = nanosecIntToDouble(timestamp); + double dt = time - last_node_time_; + + if (trans_dist > 0.3 || rot_dist > 0.15 || (!sys_initialized_ && key_index_ < params_.initial_num_measurements + 2) || dt > 1.0) + { + 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_)); + 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_); + + 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()) { @@ -277,23 +449,22 @@ 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(); + Odometry odom(result, timestamp, true); + odom.setGpsOffsetInitialized(gps_offset_initialized_); + return odom; } + + return Odometry::Uninitialized(); } gtsam::Values FactorManager::optimize() { - isam_.update(graph_, initials_); gtsam::Values result; // call the specified optimizer if (params_.smooth) @@ -303,6 +474,7 @@ gtsam::Values FactorManager::optimize() } else { + isam_.update(graph_, initials_); result = isam_.calculateEstimate(); } optimized_count_++; @@ -318,18 +490,46 @@ gtsam::Values FactorManager::optimize() 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 (!isSystemInitialized() || !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(); + // Roll back key_index_ to the last successfully optimized key + // so the next GPS/odom measurement creates factors referencing + // keys that actually exist in ISAM2/smoother + 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 +538,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_; } @@ -359,9 +560,9 @@ gtsam::ExpressionFactorGraph FactorManager::getGraph() return graph_; } -bool FactorManager::isSystemInitialized() const -{ - return sys_initialized_; +bool FactorManager::isSystemInitialized() const +{ + return key_index_ > 0; } bool FactorManager::isImuInitialized() const @@ -388,3 +589,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..a2994f7 100644 --- a/glider/src/glider.cpp +++ b/glider/src/glider.cpp @@ -14,7 +14,7 @@ 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; @@ -30,6 +30,7 @@ Glider::Glider(const std::string& path) 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 +50,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_) @@ -64,6 +65,7 @@ void Glider::addGps(int64_t timestamp, Eigen::Vector3d& gps) 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; @@ -72,10 +74,10 @@ void Glider::addGps(int64_t timestamp, Eigen::Vector3d& gps) // TODO t_imu_gps_ needs to be rotated!! meas = meas + t_imu_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(); @@ -83,6 +85,7 @@ void Glider::addGpsWithHeading(int64_t timestamp, Eigen::Vector3d& gps, Eigen::V 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; @@ -90,13 +93,13 @@ void Glider::addGpsWithHeading(int64_t timestamp, Eigen::Vector3d& gps, Eigen::V if (factor_manager_.isSystemInitialized()) { - factor_manager_.addGpsFactor(timestamp, meas, heading.x(), true); + factor_manager_.addGpsFactor(timestamp, meas, heading.x(), true, sigma); } 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(); @@ -104,6 +107,7 @@ void Glider::addGpsWithHeading(int64_t timestamp, Eigen::Vector3d& gps) 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; @@ -115,12 +119,12 @@ void Glider::addGpsWithHeading(int64_t timestamp, Eigen::Vector3d& 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); } } @@ -144,6 +148,11 @@ void Glider::addImu(int64_t timestamp, Eigen::Vector3d& accel, Eigen::Vector3d& } } +bool Glider::addOdom(int64_t timestamp, const Eigen::Isometry3d& pose) +{ + return factor_manager_.addOdomFactor(timestamp, pose); +} + void Glider::addLandmark(int64_t timestamp, size_t lid, const Eigen::Vector3d& utm, const Eigen::Matrix3d& cov) { factor_manager_.addLandmarkFactor(timestamp, lid, utm, cov); @@ -154,6 +163,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 diff --git a/glider/src/odometry.cpp b/glider/src/odometry.cpp index f7fd31e..9a43fdc 100644 --- a/glider/src/odometry.cpp +++ b/glider/src/odometry.cpp @@ -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_); @@ -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_); } @@ -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..0b57a76 100644 --- a/glider/src/odometry_with_covariance.cpp +++ b/glider/src/odometry_with_covariance.cpp @@ -24,8 +24,8 @@ OdometryWithCovariance::OdometryWithCovariance(gtsam::Values& vals, int64_t time position_covariance_ = pose_cov.block<3,3>(0,0); is_moving_ = (velocity_.norm() > 0.01) ? true : false; - initialized_ = init; + is_gps_offset_initialized_ = false; } OdometryWithCovariance OdometryWithCovariance::Uninitialized() diff --git a/glider/src/parameters.cpp b/glider/src/parameters.cpp index 6bad058..7522468 100644 --- a/glider/src/parameters.cpp +++ b/glider/src/parameters.cpp @@ -22,6 +22,7 @@ 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(); @@ -42,6 +43,7 @@ Glider::Parameters::Parameters(const std::string& path) use_dgps = config["dgps"]["enable"].as(); dgps_cov = config["dgps"]["covariance"].as(); + dgps_rejection_limit = config["dgps"]["rejection_limit"].as(); t_imu_gps(0) = config["gps_to_imu"]["x"].as(); t_imu_gps(1) = config["gps_to_imu"]["y"].as(); diff --git a/run.bash b/run.bash new file mode 100755 index 0000000..f0e7428 --- /dev/null +++ b/run.bash @@ -0,0 +1,11 @@ +#!/bin/bash + +docker run --rm -it --gpus all \ + --privileged \ + --network=host \ + -u $UID \ + -e RUN=true \ + -e DISPLAY=$DISPLAY \ + -v /tmp/.X11-unix:/tmp/.X11-unix:rw \ + --name dtc-jackal-$(hostname)-glider \ + dtc-jackal-$(hostname):glider \ No newline at end of file