From ef5a1cfd8d8ed05d8c0c8cacc61fb0bed5ab9443 Mon Sep 17 00:00:00 2001
From: Ian Reid <ian.young.reid@gmail.com>
Date: Wed, 26 Jun 2024 16:10:37 -0600
Subject: [PATCH] Moved error checking and added useful debug statements.

---
 rosplane/launch/rosplane.launch.py |  7 -------
 rosplane/src/estimator_base.cpp    | 13 +++++++++++--
 rosplane/src/estimator_example.cpp |  8 +++++---
 3 files changed, 16 insertions(+), 12 deletions(-)

diff --git a/rosplane/launch/rosplane.launch.py b/rosplane/launch/rosplane.launch.py
index bd9a1ac..6820316 100644
--- a/rosplane/launch/rosplane.launch.py
+++ b/rosplane/launch/rosplane.launch.py
@@ -22,13 +22,6 @@ def generate_launch_description():
         
         if arg.startswith("seed_estimator:="):
             use_params = arg.split(":=")[1].lower()
-
-            print(use_params)
-
-            if use_params != 'true' and use_params != 'false':
-                print('Not a valid value for seed_estimator. Enter true or false.')
-                print('Defaulting to false.')
-                use_params = 'false'
     
     autopilot_params = os.path.join(
         rosplane_dir,
diff --git a/rosplane/src/estimator_base.cpp b/rosplane/src/estimator_base.cpp
index 6efe074..cf43a0a 100644
--- a/rosplane/src/estimator_base.cpp
+++ b/rosplane/src/estimator_base.cpp
@@ -2,6 +2,7 @@
 #include "estimator_example.hpp"
 #include <cstdlib>
 #include <ament_index_cpp/get_package_share_directory.hpp>
+#include <cstring>
 #include <filesystem>
 #include <fstream>
 #include <iostream>
@@ -182,7 +183,6 @@ void estimator_base::gnssFixCallback(const sensor_msgs::msg::NavSatFix::SharedPt
     return;
   }
   if (!gps_init_ && has_fix) {
-    RCLCPP_INFO_STREAM(this->get_logger(), "init_lat: " << msg->latitude);
     gps_init_ = true;
     init_alt_ = msg->altitude;
     init_lat_ = msg->latitude;
@@ -335,8 +335,17 @@ int main(int argc, char ** argv)
   {
     rclcpp::spin(std::make_shared<rosplane::estimator_example>(use_params));
   }
+  else if(strcmp(use_params, "false")) // If the string is not true or false print error.
+  {
+    auto estimator_node = std::make_shared<rosplane::estimator_example>();
+    RCLCPP_WARN(estimator_node->get_logger(), "Invalid option for seeding estimator, defaulting to unseeded.");
+    rclcpp::spin(estimator_node);
+  }
+  else 
+  {
+    rclcpp::spin(std::make_shared<rosplane::estimator_example>());
+  }
 
-  rclcpp::spin(std::make_shared<rosplane::estimator_example>());
 
   return 0;
 }
diff --git a/rosplane/src/estimator_example.cpp b/rosplane/src/estimator_example.cpp
index 59e2e5b..6088a7b 100644
--- a/rosplane/src/estimator_example.cpp
+++ b/rosplane/src/estimator_example.cpp
@@ -61,9 +61,11 @@ estimator_example::estimator_example(bool use_params) : estimator_example()
   double init_alt = params.get_double("init_alt");
   double init_static = params.get_double("baro_calibration_val");
 
-  RCLCPP_INFO_STREAM(this->get_logger(), "init_lat: " << init_lat);
-  RCLCPP_INFO_STREAM(this->get_logger(), "init_long: " << init_long);
-  RCLCPP_INFO_STREAM(this->get_logger(), "init_alt: " << init_alt);
+  RCLCPP_INFO_STREAM(this->get_logger(), "Using seeded estimator values.");
+  RCLCPP_INFO_STREAM(this->get_logger(), "Seeded initial latitude: " << init_lat);
+  RCLCPP_INFO_STREAM(this->get_logger(), "Seeded initial longitude: " << init_long);
+  RCLCPP_INFO_STREAM(this->get_logger(), "Seeded initial altitude: " << init_alt);
+  RCLCPP_INFO_STREAM(this->get_logger(), "Seeded barometer calibration value: " << init_static);
 
   gps_init_ = true;
   init_lat_ = init_lat;