Added gps init count

Author: ongdexter

glider/config/ros-params.yaml

@@ -8,3 +8,4 @@ glider_node:
     declination: 11.82
     origin_easting: 482933.2819920078
     origin_northing: 4421345.135559781
+    gps_init_count: 10

glider/include/ros/glider_node.hpp

@@ -73,6 +73,8 @@ class GliderNode : public rclcpp::Node
         double declination_;
         double origin_easting_;
         double origin_northing_;
+        int gps_counter_;
+        int gps_init_count_;
 
         // tracker
         Glider::State current_state_;

glider/ros/glider_node.cpp

@@ -19,6 +19,7 @@ GliderNode::GliderNode(const rclcpp::NodeOptions& options) : rclcpp::Node("glide
     declare_parameter("origin_easting", 0.0);
     declare_parameter("origin_northing", 0.0);
     declare_parameter("viz", false);
+    declare_parameter("gps_init_count", 10);
 
     // Get parameters
     double freq = this->get_parameter("rate").as_double();
@@ -45,6 +46,9 @@ GliderNode::GliderNode(const rclcpp::NodeOptions& options) : rclcpp::Node("glide
     origin_easting_ = this->get_parameter("origin_easting").as_double();
     origin_northing_ = this->get_parameter("origin_northing").as_double();
 
+    gps_init_count_ = this->get_parameter("gps_init_count").as_int();
+    gps_counter_ = 0;
+
     glider_ = std::make_unique<Glider::Glider>(path);
 
     imu_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
@@ -175,7 +179,10 @@ void GliderNode::gpsCallback(const sensor_msgs::msg::NavSatFix::ConstSharedPtr m
     glider_->addGPS(timestamp, gps);
 
     current_state_ = glider_->optimize();
-    if (!initialized_) initialized_ = true;
+    if (gps_counter_++ > gps_init_count_)
+    {
+        if (!initialized_) initialized_ = true;
+    }
 }
 
 void GliderNode::odomCallback(const nav_msgs::msg::Odometry::ConstSharedPtr msg)