Moved error checking and added useful debug statements.

rosflight · Jun 26, 2024 · ef5a1cf · ef5a1cf
1 parent f6f9d3f
commit ef5a1cf
Show file tree

Hide file tree

Showing 3 changed files with 16 additions and 12 deletions.
diff --git 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
@@ -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
@@ -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;