From 99b2083653c35edebe8cf6fc4568079e2069dbcb Mon Sep 17 00:00:00 2001 From: Ian Reid Date: Tue, 25 Jun 2024 15:56:45 -0600 Subject: [PATCH] Added the override to the launch script and node. You can now override the calibration for GPS and baro with whatever value is in the parameters. The parameters are update on every calibration that does occur. This means that in flight you can immeadiatley restart as long as the seed_estimator flag is passed to the launch or run command. --- rosplane/include/estimator_base.hpp | 2 +- rosplane/include/estimator_example.hpp | 2 +- rosplane/launch/rosplane.launch.py | 26 +++++++++----------------- rosplane/src/estimator_base.cpp | 10 +++------- rosplane/src/estimator_example.cpp | 10 +++++++++- 5 files changed, 23 insertions(+), 27 deletions(-) diff --git a/rosplane/include/estimator_base.hpp b/rosplane/include/estimator_base.hpp index 1052811..f510cd6 100644 --- a/rosplane/include/estimator_base.hpp +++ b/rosplane/include/estimator_base.hpp @@ -89,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::SharedPtr vehicle_state_pub_; @@ -129,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 init_static_vector_; /**< Used to grab the first set of baro measurements */ diff --git a/rosplane/include/estimator_example.hpp b/rosplane/include/estimator_example.hpp index b55af4d..5fbe226 100644 --- a/rosplane/include/estimator_example.hpp +++ b/rosplane/include/estimator_example.hpp @@ -14,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); diff --git a/rosplane/launch/rosplane.launch.py b/rosplane/launch/rosplane.launch.py index fd97e7d..bd9a1ac 100644 --- a/rosplane/launch/rosplane.launch.py +++ b/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,15 @@ 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("seed_estimator:="): + use_params = arg.split(":=")[1].lower() - if arg.startswith("init_long:="): - init_long = float(arg.split(":=")[1]) - assert isinstance(init_long, float) - init_long = str(init_long) + print(use_params) - if arg.startswith("init_alt:="): - init_alt = float(arg.split(":=")[1]) - assert isinstance(init_alt, float) - init_alt = str(init_alt) + 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, @@ -76,6 +68,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] ) ]) diff --git a/rosplane/src/estimator_base.cpp b/rosplane/src/estimator_base.cpp index 0e7ae13..6efe074 100644 --- a/rosplane/src/estimator_base.cpp +++ b/rosplane/src/estimator_base.cpp @@ -6,7 +6,6 @@ #include #include #include -//#include namespace rosplane { @@ -330,14 +329,11 @@ 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(init_lat, init_long, init_alt)); + rclcpp::spin(std::make_shared(use_params)); } rclcpp::spin(std::make_shared()); diff --git a/rosplane/src/estimator_example.cpp b/rosplane/src/estimator_example.cpp index 94550d6..59e2e5b 100644 --- a/rosplane/src/estimator_example.cpp +++ b/rosplane/src/estimator_example.cpp @@ -54,8 +54,13 @@ 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() { + 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(), "init_lat: " << init_lat); RCLCPP_INFO_STREAM(this->get_logger(), "init_long: " << init_long); RCLCPP_INFO_STREAM(this->get_logger(), "init_alt: " << init_alt); @@ -64,6 +69,9 @@ estimator_example::estimator_example(float init_lat, float init_long, float init 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() {