Merge pull request #60 from rosflight/46-seeding-estimator

46 seeding estimator

rosplane/CMakeLists.txt

@@ -98,6 +98,9 @@ add_executable(rosplane_estimator_node
               src/estimator_base.cpp
               src/estimator_example.cpp
               src/param_manager.cpp)
+target_link_libraries(rosplane_estimator_node
+  ${YAML_CPP_LIBRARIES}
+)
 ament_target_dependencies(rosplane_estimator_node rosplane_msgs rosflight_msgs rclcpp Eigen3)
 install(TARGETS
         rosplane_estimator_node

rosplane/include/controller_base.hpp

@@ -12,8 +12,7 @@
 
 #include <chrono>
 #include <cstring>
-#include <iostream>
-#include <variant>
+#include <ament_index_cpp/get_package_share_directory.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <rosflight_msgs/msg/command.hpp>
 #include <rosplane_msgs/msg/controller_commands.hpp>

rosplane/include/estimator_base.hpp

@@ -13,7 +13,8 @@
 #include <chrono>
 #include <geometry_msgs/msg/twist_stamped.hpp>
 #include <math.h>
-#include <numeric>
+#include <yaml-cpp/yaml.h>
+#include <ament_index_cpp/get_package_share_directory.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <rosflight_msgs/msg/airspeed.hpp>
 #include <rosflight_msgs/msg/barometer.hpp>
@@ -88,6 +89,7 @@ class estimator_base : public rclcpp::Node
   double init_lat_ = 0.0;                 /**< Initial latitude in degrees */
   double init_lon_ = 0.0;                 /**< Initial longitude in degrees */
   float init_alt_ = 0.0;                  /**< Initial altitude in meters above MSL  */
+  float init_static_;                     /**< Initial static pressure (mbar)  */
 
 private:
   rclcpp::Publisher<rosplane_msgs::msg::State>::SharedPtr vehicle_state_pub_;
@@ -99,11 +101,20 @@ class estimator_base : public rclcpp::Node
   rclcpp::Subscription<rosflight_msgs::msg::Airspeed>::SharedPtr airspeed_sub_;
   rclcpp::Subscription<rosflight_msgs::msg::Status>::SharedPtr status_sub_;
 
+  std::string param_filepath_ = "estimator_params.yaml";
+
   void update();
   void gnssFixCallback(const sensor_msgs::msg::NavSatFix::SharedPtr msg);
   void gnssVelCallback(const geometry_msgs::msg::TwistStamped::SharedPtr msg);
   void imuCallback(const sensor_msgs::msg::Imu::SharedPtr msg);
   void baroAltCallback(const rosflight_msgs::msg::Barometer::SharedPtr msg);
+  /**
+   * @brief This saves parameters to the param file for later use.
+   *
+   * @param param_name The name of the parameter.
+   * @param param_val The value of the parameter.
+   */
+  void saveParameter(std::string param_name, double param_val);
   void airspeedCallback(const rosflight_msgs::msg::Airspeed::SharedPtr msg);
   void statusCallback(const rosflight_msgs::msg::Status::SharedPtr msg);
 
@@ -119,7 +130,6 @@ class estimator_base : public rclcpp::Node
 
   bool gps_new_;
   bool armed_first_time_;                 /**< Arm before starting estimation  */
-  float init_static_;                     /**< Initial static pressure (mbar)  */
   int baro_count_;                        /**< Used to grab the first set of baro measurements */
   std::vector<float> init_static_vector_; /**< Used to grab the first set of baro measurements */
 

rosplane/include/estimator_example.hpp

@@ -5,6 +5,7 @@
 
 #include <Eigen/Geometry>
 #include <math.h>
+#include <yaml-cpp/yaml.h>
 
 namespace rosplane
 {
@@ -13,7 +14,7 @@ class estimator_example : public estimator_base
 {
 public:
   estimator_example();
-  estimator_example(float init_lat, float init_long, float init_alt);
+  estimator_example(bool use_params);
 
 private:
   virtual void estimate(const input_s & input, output_s & output);

rosplane/launch/rosplane.launch.py

@@ -11,10 +11,7 @@ def generate_launch_description():
     # Determine the appropriate control scheme.
     control_type = "default"
     aircraft = "skyhunter" # Default aircraft 
-    init_lat = "0.0" # init lat if seeding the estimator (typically not done)
-    init_long = "0.0"
-    init_alt = "0.0"
-    init_baro_alt = "0.0"
+    use_params = 'false'
 
     for arg in sys.argv:
         if arg.startswith("control_type:="):
@@ -23,20 +20,8 @@ def generate_launch_description():
         if arg.startswith("aircraft:="):
             aircraft = arg.split(":=")[1]
 
-        if arg.startswith("init_lat:="):
-            init_lat = float(arg.split(":=")[1])
-            assert isinstance(init_lat, float)
-            init_lat = str(init_lat)
-
-        if arg.startswith("init_long:="):
-            init_long = float(arg.split(":=")[1])
-            assert isinstance(init_long, float)
-            init_long = str(init_long)
-
-        if arg.startswith("init_alt:="):
-            init_alt = float(arg.split(":=")[1])
-            assert isinstance(init_alt, float)
-            init_alt = str(init_alt)
+        if arg.startswith("seed_estimator:="):
+            use_params = arg.split(":=")[1].lower()
 
     autopilot_params = os.path.join(
         rosplane_dir,
@@ -76,6 +61,6 @@ def generate_launch_description():
             name='estimator',
             output='screen',
             parameters = [autopilot_params],
-            arguments = [init_lat, init_long, init_alt, init_baro_alt]
+            arguments = [use_params]
         )
     ])

rosplane/params/anaconda_autopilot_params.yaml

@@ -1,4 +1,4 @@
-autopilot: # node name.
+autopilot:
   ros__parameters:
     alt_hz: 10.0
     alt_toz: 5.0
@@ -31,7 +31,7 @@ autopilot: # node name.
     trim_t: 0.5
     max_e: 0.61
     max_a: 0.60
-    max_r: 0.523  # TODO: Was hardcoded as 1.0 in code...
+    max_r: 0.523
     max_t: 1.0
     pwm_rad_e: 1.0
     pwm_rad_a: 1.0
@@ -41,23 +41,20 @@ autopilot: # node name.
     gravity: 9.8
     max_roll: 35.0
     controller_output_frequency: 100.0
-
 path_manager:
   ros__parameters:
     R_min: 100.0
     orbit_last: False
     default_altitude: 50.0
     default_airspeed: 25.0
     current_path_pub_frequency: 100.0
-
 path_follower:
   ros__parameters:
     controller_commands_pub_frequency: 10.0
     chi_infty: 0.5
     k_orbit: 4.0
     k_path: 0.05
     gravity: 9.81
-
 estimator:
   ros__parameters:
     rho: 1.225
@@ -71,4 +68,9 @@ estimator:
     lpf_a1: 8.0
     gps_n_lim: 10000.
     gps_e_lim: 10000.
-    frequency: 100
+    frequency: 100
+    # These will be overridden on each boot the workspace is symlink installed.
+    baro_calibration_val: 0.0
+    init_lat: 0.0
+    init_lon: 0.0
+    init_alt: 0.0

rosplane/src/estimator_base.cpp

@@ -1,7 +1,12 @@
 #include "estimator_base.hpp"
 #include "estimator_example.hpp"
 #include <cstdlib>
-//#include <sensor_msgs/nav_sat_status.hpp>
+#include <ament_index_cpp/get_package_share_directory.hpp>
+#include <cstring>
+#include <filesystem>
+#include <fstream>
+#include <iostream>
+#include <rclcpp/logging.hpp>
 
 namespace rosplane
 {
@@ -40,6 +45,14 @@ estimator_base::estimator_base()
 
   params_initialized_ = true;
 
+  std::filesystem::path rosplane_dir = ament_index_cpp::get_package_share_directory("rosplane");
+
+  std::filesystem::path params_dir = "params";
+  std::filesystem::path params_file = "anaconda_autopilot_params.yaml";
+  std::filesystem::path full_path = rosplane_dir/params_dir/params_file;
+
+  param_filepath_ = full_path.string();
+
   set_timer();
 }
 
@@ -52,6 +65,10 @@ void estimator_base::declare_parameters()
   params.declare_double("baro_measurement_gate", 1.35);  // TODO: this is a magic number. What is it determined from?
   params.declare_double("airspeed_measurement_gate", 5.0);  // TODO: this is a magic number. What is it determined from?
   params.declare_int("baro_calibration_count", 100);  // TODO: this is a magic number. What is it determined from?
+  params.declare_double("baro_calibration_val", 0.0);
+  params.declare_double("init_lat", 0.0);
+  params.declare_double("init_lon", 0.0);
+  params.declare_double("init_alt", 0.0);
 }
 
 void estimator_base::set_timer() {
@@ -166,11 +183,13 @@ 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;
     init_lon_ = msg->longitude;
+    saveParameter("init_lat", init_lat_);
+    saveParameter("init_lon", init_lon_);
+    saveParameter("init_alt", init_alt_);
   } else {
     input_.gps_n = EARTH_RADIUS * (msg->latitude - init_lat_) * M_PI / 180.0;
     input_.gps_e =
@@ -225,6 +244,7 @@ void estimator_base::baroAltCallback(const rosflight_msgs::msg::Barometer::Share
       init_static_ = std::accumulate(init_static_vector_.begin(), init_static_vector_.end(), 0.0)
         / init_static_vector_.size();
       baro_init_ = true;
+      saveParameter("baro_calibration_val", init_static_);
 
       //Check that it got a good calibration.
       std::sort(init_static_vector_.begin(), init_static_vector_.end());
@@ -284,24 +304,48 @@ void estimator_base::statusCallback(const rosflight_msgs::msg::Status::SharedPtr
   if (!armed_first_time_ && msg->armed) armed_first_time_ = true;
 }
 
+void estimator_base::saveParameter(std::string param_name, double param_val)
+{
+
+  YAML::Node param_yaml_file = YAML::LoadFile(param_filepath_);
+
+  if (param_yaml_file["estimator"]["ros__parameters"][param_name])
+  {
+    param_yaml_file["estimator"]["ros__parameters"][param_name] = param_val;
+  }
+  else 
+  {
+    RCLCPP_ERROR_STREAM(this->get_logger(), "Parameter [" << param_name << "] is not in parameter file.");
+  }
+
+  std::ofstream fout(param_filepath_);
+  fout << param_yaml_file;
+}
+
 } // namespace rosplane
 
 int main(int argc, char ** argv)
 {
 
   rclcpp::init(argc, argv);
 
-  double init_lat = std::atof(argv[1]);
-  double init_long = std::atof(argv[2]);
-  double init_alt = std::atof(argv[3]);
-
+  char* use_params = argv[1];
 
-  if (init_lat != 0.0 || init_long != 0.0 || init_alt != 0.0)
+  if (!strcmp(use_params, "true"))
+  {
+    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>(init_lat, init_long, init_alt));
+    rclcpp::spin(std::make_shared<rosplane::estimator_example>());
   }
 
-  rclcpp::spin(std::make_shared<rosplane::estimator_example>());
 
   return 0;
 }

rosplane/src/estimator_example.cpp

@@ -1,6 +1,5 @@
 #include "estimator_example.hpp"
 #include "estimator_base.hpp"
-#include <rclcpp/logging.hpp>
 
 namespace rosplane
 {
@@ -55,17 +54,26 @@ estimator_example::estimator_example()
   N_ = params.get_int("num_propagation_steps");
 }
 
-estimator_example::estimator_example(float init_lat, float init_long, float init_alt) : estimator_example()
+estimator_example::estimator_example(bool use_params) : estimator_example()
 {
-  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);
+  double init_lat = params.get_double("init_lat");
+  double init_long = params.get_double("init_lon");
+  double init_alt = params.get_double("init_alt");
+  double init_static = params.get_double("baro_calibration_val");
+
+  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;
   init_lon_ = init_long;
   init_alt_ = init_alt;
 
+  baro_init_ = true;
+  init_static_ = init_static;
 }
 
 void estimator_example::initialize_state_covariances() {