Skip to content

Commit

Permalink
adding configuration options to the c++ particle filter
Browse files Browse the repository at this point in the history
  • Loading branch information
@antonioc76
antonioc76 committed May 22, 2024
1 parent f1738c7 commit ccd7b04
Show file tree
Hide file tree
Showing 5 changed files with 106 additions and 36 deletions.
19 changes: 16 additions & 3 deletions autonav_ws/src/autonav_filters/include/autonav_filters/filters.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand All @@ -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;
Expand Down
39 changes: 26 additions & 13 deletions autonav_ws/src/autonav_filters/include/autonav_filters/particle_filter.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<int> range = {1, 5, 10, 15, 20, 25};
std::discrete_distribution<int> 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; i<this->num_particles; i++) {
Expand Down Expand Up @@ -243,11 +253,11 @@ class ParticleFilter {
return this->num_particles;
}

double * get_gps_noise() {
std::array<double, 1> get_gps_noise() {
return this->gps_noise;
}

double * get_odom_noise() {
std::array<double, 3> get_odom_noise() {
return this->odom_noise;
}

Expand Down Expand Up @@ -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<double, 1> gps_noise;
//double odom_noise[3] = {0.05, 0.05, 0.01};
std::array<double, 3> odom_noise;
std::vector<Particle> particles;
double latitudeLength;
double longitudeLength;
Expand Down
52 changes: 37 additions & 15 deletions autonav_ws/src/autonav_filters/src/filters.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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();
Expand All @@ -54,6 +59,23 @@ void FiltersNode::on_reset() {
this->particle_filter.init_particles();
}

void FiltersNode::config_updated(json newConfig) {
config = newConfig.template get<ParticleFilterConfig>();
}

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);
Expand Down
21 changes: 16 additions & 5 deletions autonav_ws/src/autonav_filters/tests/particle_filter_test.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);

Expand Down Expand Up @@ -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
Expand All @@ -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();

Expand All @@ -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();

Expand Down
11 changes: 11 additions & 0 deletions display/scripts/globals.js
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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",
Expand Down

0 comments on commit ccd7b04

Please sign in to comment.