From ccd7b0489ec38b42a41f37f3f968d59911839649 Mon Sep 17 00:00:00 2001 From: antonioc76 Date: Tue, 21 May 2024 20:39:59 -0500 Subject: [PATCH] adding configuration options to the c++ particle filter --- .../include/autonav_filters/filters.hpp | 19 +++++-- .../autonav_filters/particle_filter.hpp | 39 +++++++++----- .../src/autonav_filters/src/filters.cpp | 52 +++++++++++++------ .../tests/particle_filter_test.cpp | 21 ++++++-- display/scripts/globals.js | 11 ++++ 5 files changed, 106 insertions(+), 36 deletions(-) diff --git a/autonav_ws/src/autonav_filters/include/autonav_filters/filters.hpp b/autonav_ws/src/autonav_filters/include/autonav_filters/filters.hpp index 03e439c2..3903cf8c 100644 --- a/autonav_ws/src/autonav_filters/include/autonav_filters/filters.hpp +++ b/autonav_ws/src/autonav_filters/include/autonav_filters/filters.hpp @@ -15,6 +15,19 @@ using namespace std::chrono_literals; +// struct ParticleFilterConfig { +// int num_particles; +// double latitudeLength; +// double longitudeLength; +// double gps_noise; +// double odom_noise_x; +// double odom_noise_y; +// double odom_noise_theta; + +// NLOHMANN_DEFINE_TYPE_INTRUSIVE(ParticleFilterConfig, num_particles, latitudeLength, longitudeLength, gps_noise, odom_noise); +// }; + + class FiltersNode : public SCR::Node { public: FiltersNode() : SCR::Node("autonav_filters_pf"), count_(0) @@ -34,10 +47,10 @@ class FiltersNode : public SCR::Node { private: //particle filter - double latitudeLength = 111086.2; - double longitudeLength = 81978.2; + ParticleFilter particle_filter; + double latitudeLength; + double longitudeLength; double degreeOffset = 107.0; - ParticleFilter particle_filter{latitudeLength, longitudeLength}; autonav_msgs::msg::GPSFeedback first_gps; autonav_msgs::msg::GPSFeedback last_gps; bool first_gps_received = false; diff --git a/autonav_ws/src/autonav_filters/include/autonav_filters/particle_filter.hpp b/autonav_ws/src/autonav_filters/include/autonav_filters/particle_filter.hpp index 689e97cd..3182afd5 100644 --- a/autonav_ws/src/autonav_filters/include/autonav_filters/particle_filter.hpp +++ b/autonav_ws/src/autonav_filters/include/autonav_filters/particle_filter.hpp @@ -45,23 +45,33 @@ class Particle { class ParticleFilter { public: // constructor - ParticleFilter(double latitudeLength, double longitudeLength) { + ParticleFilter(int num_particles, double latitudeLength, double longitudeLength, double gps_noise, double odom_noise_x, double odom_noise_y, double odom_noise_theta) { + this->num_particles = num_particles; + printf("num partices: %d\n", this->num_particles); this->latitudeLength = latitudeLength; + printf("lat len: %f\n", this->latitudeLength); this->longitudeLength = longitudeLength; + printf("long len %f\n", this->longitudeLength); + this->gps_noise = {gps_noise}; + this->odom_noise = {odom_noise_x, odom_noise_y, odom_noise_theta}; + std::random_device rd; //std::mt19937 generator(std::chrono::high_resolution_clock::now().time_since_epoch().count()); std::mt19937 generator(rd()); std::vector range = {1, 5, 10, 15, 20, 25}; - std::discrete_distribution discrete(range.begin(), range.end()); - for (int i = 0; i < 10; i++) { - int index = discrete(generator); - int value = range[index]; - //printf("index: %d\n", index); - //printf("value: %d\n", value); - } this->generator = generator; }; + // empty constructor + ParticleFilter() { + this->num_particles = 0; + this->latitudeLength = 0; + this->longitudeLength = 0; + std::random_device rd; + std::mt19937 generator(rd()); + this->generator = generator; + } + void init_particles() { particles.clear(); for (int i=0; inum_particles; i++) { @@ -243,11 +253,11 @@ class ParticleFilter { return this->num_particles; } - double * get_gps_noise() { + std::array get_gps_noise() { return this->gps_noise; } - double * get_odom_noise() { + std::array get_odom_noise() { return this->odom_noise; } @@ -280,9 +290,12 @@ class ParticleFilter { private: std::mt19937 generator; - static const int num_particles = 750; - double gps_noise[1] = {0.8}; - double odom_noise[3] = {0.05, 0.05, 0.01}; + //int num_particles = 750; + int num_particles = 0; + //double gps_noise[1] = {0.8}; + std::array gps_noise; + //double odom_noise[3] = {0.05, 0.05, 0.01}; + std::array odom_noise; std::vector particles; double latitudeLength; double longitudeLength; diff --git a/autonav_ws/src/autonav_filters/src/filters.cpp b/autonav_ws/src/autonav_filters/src/filters.cpp index 9850beac..34bb1231 100644 --- a/autonav_ws/src/autonav_filters/src/filters.cpp +++ b/autonav_ws/src/autonav_filters/src/filters.cpp @@ -6,19 +6,33 @@ #include "autonav_msgs/msg/gps_feedback.hpp" #include "autonav_msgs/msg/position.hpp" #include "autonav_msgs/msg/imu_data.hpp" -// #include "scr/states.hpp" -// #include "scr_msgs/msg/system_state.hpp" #define _USE_MATH_DEFINES using namespace std::chrono_literals; -//particle filter +struct ParticleFilterConfig { + int num_particles; + double latitudeLength; + double longitudeLength; + double gps_noise; + double odom_noise_x; + double odom_noise_y; + double odom_noise_theta; + + NLOHMANN_DEFINE_TYPE_INTRUSIVE(ParticleFilterConfig, num_particles, latitudeLength, longitudeLength, gps_noise, odom_noise_x, odom_noise_y, odom_noise_theta); +}; +ParticleFilterConfig config; + +//particle filter void FiltersNode::init() { - double latitudeLength = 111086.2; - double longitudeLength = 81978.2; - ParticleFilter particle_filter{latitudeLength, longitudeLength}; + //double latitudeLength = 111086.2; + //double longitudeLength = 81978.2; + this->latitudeLength = config.latitudeLength; + this->longitudeLength = config.longitudeLength; + ParticleFilter particle_filter{config.num_particles, config.latitudeLength, config.longitudeLength, config.gps_noise, config.odom_noise_x, config.odom_noise_y, config.odom_noise_theta}; + this->particle_filter = particle_filter; autonav_msgs::msg::GPSFeedback first_gps; autonav_msgs::msg::GPSFeedback last_gps; bool first_gps_received = false; @@ -30,15 +44,6 @@ void FiltersNode::system_state_transition(scr_msgs::msg::SystemState old, scr_ms }; -void FiltersNode::config_updated(json config) { - -}; - -json FiltersNode::get_default_config() { - -}; - - // void FiltersNode::system_state_transition(scr_msgs::msg::SystemState old, scr_msgs::msg::SystemState updated) { // if ((old.state != SCR::SystemState::AUTONOMOUS) && (updated.state == SCR::SystemState::AUTONOMOUS)) { // this->on_reset(); @@ -54,6 +59,23 @@ void FiltersNode::on_reset() { this->particle_filter.init_particles(); } +void FiltersNode::config_updated(json newConfig) { + config = newConfig.template get(); +} + +json FiltersNode::get_default_config() { + ParticleFilterConfig newConfig; + newConfig.latitudeLength = 111086.2; + newConfig.longitudeLength = 81978.2; + newConfig.num_particles = 750; + newConfig.gps_noise = 0.8; + newConfig.odom_noise_x = 0.05; + newConfig.odom_noise_y = 0.05; + newConfig.odom_noise_theta = 0.01; + + return newConfig; +} + void FiltersNode::on_GPS_received(const autonav_msgs::msg::GPSFeedback gps_message) { RCLCPP_INFO(this->get_logger(), "RECEIVED GPS"); RCLCPP_INFO(this->get_logger(), "gps_message.latitude %f\n", gps_message.latitude); diff --git a/autonav_ws/src/autonav_filters/tests/particle_filter_test.cpp b/autonav_ws/src/autonav_filters/tests/particle_filter_test.cpp index 5a75cbe3..4d1af85c 100644 --- a/autonav_ws/src/autonav_filters/tests/particle_filter_test.cpp +++ b/autonav_ws/src/autonav_filters/tests/particle_filter_test.cpp @@ -11,9 +11,12 @@ TEST(ParticleFilterTests, initialization_test) { //GTEST_SKIP() << "skipping init test"; + int num_particles = 750; double latitudeLength = 111086.2; double longitudeLength = 81978.2; - ParticleFilter particle_filter = ParticleFilter(latitudeLength, longitudeLength); + double gps_noise = 0.8; + double odom_noise[3] = {0.5, 0.5, 0.1}; + ParticleFilter particle_filter = ParticleFilter(num_particles, latitudeLength, longitudeLength, gps_noise, odom_noise[0], odom_noise[1], odom_noise[2]); ASSERT_EQ(particle_filter.get_latitudeLength(), latitudeLength); ASSERT_EQ(particle_filter.get_longitudeLength(), longitudeLength); @@ -49,11 +52,13 @@ TEST(ParticleFilterTests, feedback_test) { // printf("sliced_delta_xs size: %ld\n", sliced_delta_xs.size()); // printf("sliced_delta_xs size: %ld\n", sliced_delta_xs.size()); // printf("sliced_delta_xs size: %ld\n", sliced_delta_xs.size()); - + int num_particles = 750; double latitudeLength = 111086.2; double longitudeLength = 81978.2; - ParticleFilter particle_filter = ParticleFilter(latitudeLength, longitudeLength); + double gps_noise = 0.8; + double odom_noise[3] = {0.5, 0.5, 0.1}; + ParticleFilter particle_filter = ParticleFilter(num_particles, latitudeLength, longitudeLength, gps_noise, odom_noise[0], odom_noise[1], odom_noise[2]); particle_filter.init_particles(); // construct feedback messages @@ -80,9 +85,12 @@ TEST(ParticleFilterTests, feedback_test) { TEST(ParticleFilterTests, gps_test) { //GTEST_SKIP() << "Skipping gps_test"; + int num_particles = 750; double latitudeLength = 111086.2; double longitudeLength = 81978.2; - ParticleFilter particle_filter = ParticleFilter(latitudeLength, longitudeLength); + double gps_noise = 0.8; + double odom_noise[3] = {0.5, 0.5, 0.1}; + ParticleFilter particle_filter = ParticleFilter(num_particles, latitudeLength, longitudeLength, gps_noise, odom_noise[0], odom_noise[1], odom_noise[2]); particle_filter.init_particles(); @@ -103,9 +111,12 @@ TEST(ParticleFilterTests, gps_test) { TEST(ParticleFilterTests, complete_test) { // This test will fail sometimes because the particle filter is non-deterministic + int num_particles = 750; double latitudeLength = 111086.2; double longitudeLength = 81978.2; - ParticleFilter particle_filter = ParticleFilter(latitudeLength, longitudeLength); + double gps_noise = 0.8; + double odom_noise[3] = {0.5, 0.5, 0.1}; + ParticleFilter particle_filter = ParticleFilter(num_particles, latitudeLength, longitudeLength, gps_noise, odom_noise[0], odom_noise[1], odom_noise[2]); particle_filter.init_particles(); diff --git a/display/scripts/globals.js b/display/scripts/globals.js index a5d5cdae..52f0708d 100644 --- a/display/scripts/globals.js +++ b/display/scripts/globals.js @@ -63,6 +63,17 @@ var addressKeys = { "seed_heading": "bool", }, + "autonav_filters_pf": { + "internal_title": "[Localization] Filters", + "num_particles": "int", + "latitudeLength": "float", + "longitudeLength": "float", + "gps_noise": "float", + "odom_noise_x": "float", + "odom_noise_y": "float", + "odom_noise_theta": "float", + }, + "autonav_manual_steamcontroller": { "internal_title": "[Manual] Steam Controller", "forward_speed": "float",