diff --git a/nav2_amcl/include/nav2_amcl/pf/pf.hpp b/nav2_amcl/include/nav2_amcl/pf/pf.hpp index afcb9daafbb..c4759d06b48 100644 --- a/nav2_amcl/include/nav2_amcl/pf/pf.hpp +++ b/nav2_amcl/include/nav2_amcl/pf/pf.hpp @@ -129,7 +129,6 @@ typedef struct _pf_t // Function used to draw random pose samples pf_init_model_fn_t random_pose_fn; - void * random_pose_data; double dist_threshold; // distance threshold in each axis over which the pf is considered to not // be converged @@ -141,7 +140,7 @@ typedef struct _pf_t pf_t * pf_alloc( int min_samples, int max_samples, double alpha_slow, double alpha_fast, - pf_init_model_fn_t random_pose_fn, void * random_pose_data); + pf_init_model_fn_t random_pose_fn); // Free an existing filter void pf_free(pf_t * pf); @@ -159,7 +158,7 @@ void pf_init_model(pf_t * pf, pf_init_model_fn_t init_fn, void * init_data); void pf_update_sensor(pf_t * pf, pf_sensor_model_fn_t sensor_fn, void * sensor_data); // Resample the distribution -void pf_update_resample(pf_t * pf); +void pf_update_resample(pf_t * pf, void * random_pose_data); // Compute the CEP statistics (mean and variance). // void pf_get_cep_stats(pf_t * pf, pf_vector_t * mean, double * var); diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index 303a753b73b..5620ae5edd5 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -679,7 +679,7 @@ AmclNode::laserReceived(sensor_msgs::msg::LaserScan::ConstSharedPtr laser_scan) // Resample the particles if (!(++resample_count_ % resample_interval_)) { - pf_update_resample(pf_); + pf_update_resample(pf_, reinterpret_cast(map_)); resampled = true; } @@ -1580,8 +1580,7 @@ AmclNode::initParticleFilter() // Create the particle filter pf_ = pf_alloc( min_particles_, max_particles_, alpha_slow_, alpha_fast_, - (pf_init_model_fn_t)AmclNode::uniformPoseGenerator, - reinterpret_cast(map_)); + (pf_init_model_fn_t)AmclNode::uniformPoseGenerator); pf_->pop_err = pf_err_; pf_->pop_z = pf_z_; diff --git a/nav2_amcl/src/pf/pf.c b/nav2_amcl/src/pf/pf.c index 63d71b99bd0..de4e3247d10 100644 --- a/nav2_amcl/src/pf/pf.c +++ b/nav2_amcl/src/pf/pf.c @@ -47,7 +47,7 @@ static int pf_resample_limit(pf_t * pf, int k); pf_t * pf_alloc( int min_samples, int max_samples, double alpha_slow, double alpha_fast, - pf_init_model_fn_t random_pose_fn, void * random_pose_data) + pf_init_model_fn_t random_pose_fn) { int i, j; pf_t * pf; @@ -59,7 +59,6 @@ pf_t * pf_alloc( pf = calloc(1, sizeof(pf_t)); pf->random_pose_fn = random_pose_fn; - pf->random_pose_data = random_pose_data; pf->min_samples = min_samples; pf->max_samples = max_samples; @@ -291,7 +290,7 @@ void pf_update_sensor(pf_t * pf, pf_sensor_model_fn_t sensor_fn, void * sensor_d // Resample the distribution -void pf_update_resample(pf_t * pf) +void pf_update_resample(pf_t * pf, void * random_pose_data) { int i; double total; @@ -344,7 +343,7 @@ void pf_update_resample(pf_t * pf) sample_b = set_b->samples + set_b->sample_count++; if (drand48() < w_diff) { - sample_b->pose = (pf->random_pose_fn)(pf->random_pose_data); + sample_b->pose = (pf->random_pose_fn)(random_pose_data); } else { // Can't (easily) combine low-variance sampler with KLD adaptive // sampling, so we'll take the more traditional route.