Fix Wrong Map Pointer (ros-navigation#3311) (ros-navigation#3315)

Not assigning fixed map pointer to particle filter, using latest when resample. (cherry picked from commit cc6f205) Signed-off-by: Borong Yuan <yuanborong@hotmail.com> Signed-off-by: Borong Yuan <yuanborong@hotmail.com>
EnjoyRobotics · Dec 6, 2022 · 85735ea · 85735ea
1 parent a1fd077
commit 85735ea
Show file tree

Hide file tree

Showing 3 changed files with 7 additions and 10 deletions.
diff --git 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
@@ -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<void *>(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<void *>(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
@@ -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.