Merge pull request #63 from cmrobotics/feat/spread-laser-sample

Feat/spread laser samples

nav2_amcl/include/nav2_amcl/amcl_node.hpp

@@ -439,6 +439,7 @@ class AmclNode : public nav2_util::LifecycleNode
   std::string global_frame_id_;
   double lambda_short_;
   double laser_likelihood_max_dist_;
+  double min_laser_hit_sample_dist_;
   double laser_max_range_;
   double laser_min_range_;
   std::string sensor_model_type_;

nav2_amcl/include/nav2_amcl/sensors/laser/laser.hpp

@@ -131,7 +131,8 @@ class BeamModel : public Laser
    */
   BeamModel(
     double z_hit, double z_short, double z_max, double z_rand, double sigma_hit,
-    double lambda_short, double chi_outlier, size_t max_beams, map_t * map, double laser_importance_factor);
+    double lambda_short, double chi_outlier, size_t max_beams, map_t * map, double laser_importance_factor,
+    const double min_laser_hit_sample_dist);
 
   /*
    * @brief Run a sensor update on laser
@@ -161,7 +162,8 @@ class LikelihoodFieldModel : public Laser
    */
   LikelihoodFieldModel(
     double z_hit, double z_rand, double sigma_hit, double max_occ_dist,
-    size_t max_beams, map_t * map, double laser_importance_factor);
+    size_t max_beams, map_t * map, double laser_importance_factor,
+    const double min_laser_hit_sample_dist);
 
   /*
    * @brief Run a sensor update on laser
@@ -179,6 +181,8 @@ class LikelihoodFieldModel : public Laser
    * @return total weight of the particle set
    */
   static double sensorFunction(LaserData * data, pf_sample_set_t * set);
+
+  double min_laser_hit_sample_dist_sq_;
 };
 
 /*
@@ -195,7 +199,8 @@ class LikelihoodFieldModelProb : public Laser
     double z_hit, double z_rand, double sigma_hit, double max_occ_dist,
     bool do_beamskip, double beam_skip_distance,
     double beam_skip_threshold, double beam_skip_error_threshold,
-    size_t max_beams, map_t * map, double laser_importance_factor);
+    size_t max_beams, map_t * map, double laser_importance_factor,
+    const double min_laser_hit_sample_dist);
 
   /*
    * @brief Run a sensor update on laser

nav2_amcl/src/amcl_node.cpp

@@ -255,6 +255,10 @@ AmclNode::AmclNode(const rclcpp::NodeOptions & options)
     "laser_importance_factor", rclcpp::ParameterValue(1.0f),
     "Importance factor for laser scan, [0.0, 1.0]");
 
+  add_parameter(
+    "min_laser_hit_sample_dist", rclcpp::ParameterValue(0.0f),
+    "Minimum distance between two laser points for the laser mesurement model.");
+
     add_parameter(
       "std_warn_level_x", rclcpp::ParameterValue(0.2),
       "Limit threshold of x value. Monitors the estimated standard deviation of the filter.");
@@ -1295,19 +1299,22 @@ AmclNode::createLaserObject()
   if (sensor_model_type_ == "beam") {
     return new nav2_amcl::BeamModel(
       z_hit_, z_short_, z_max_, z_rand_, sigma_hit_, lambda_short_,
-      0.0, max_beams_, map_, laser_importance_factor_);
+      0.0, max_beams_, map_, laser_importance_factor_,
+      min_laser_hit_sample_dist_);
   }
 
   if (sensor_model_type_ == "likelihood_field_prob") {
     return new nav2_amcl::LikelihoodFieldModelProb(
       z_hit_, z_rand_, sigma_hit_,
       laser_likelihood_max_dist_, do_beamskip_, beam_skip_distance_, beam_skip_threshold_,
-      beam_skip_error_threshold_, max_beams_, map_, laser_importance_factor_);
+      beam_skip_error_threshold_, max_beams_, map_, laser_importance_factor_,
+      min_laser_hit_sample_dist_);
   }
 
   return new nav2_amcl::LikelihoodFieldModel(
     z_hit_, z_rand_, sigma_hit_,
-    laser_likelihood_max_dist_, max_beams_, map_, laser_importance_factor_);
+    laser_likelihood_max_dist_, max_beams_, map_, laser_importance_factor_,
+    min_laser_hit_sample_dist_);
 }
 
 void
@@ -1368,6 +1375,7 @@ AmclNode::initParameters()
   get_parameter("scan_topic", scan_topic_);
   get_parameter("map_topic", map_topic_);
   get_parameter("laser_importance_factor", laser_importance_factor_);
+  get_parameter("min_laser_hit_sample_dist", min_laser_hit_sample_dist_);
   get_parameter("std_warn_level_x", std_warn_level_x_);
   get_parameter("std_warn_level_y", std_warn_level_y_);
   get_parameter("std_warn_level_yaw", std_warn_level_yaw_);
@@ -1524,6 +1532,9 @@ AmclNode::dynamicParametersCallback(
       } else if (param_name == "laser_likelihood_max_dist") {
         laser_likelihood_max_dist_ = parameter.as_double();
         reinit_laser = true;
+      } else if (param_name == "min_laser_hit_sample_dist") {
+        min_laser_hit_sample_dist_ = parameter.as_double();
+        reinit_laser = true;
       } else if (param_name == "laser_max_range") {
         laser_max_range_ = parameter.as_double();
         reinit_laser = true;

nav2_amcl/src/sensors/laser/beam_model.cpp

@@ -29,7 +29,8 @@ namespace nav2_amcl
 
 BeamModel::BeamModel(
   double z_hit, double z_short, double z_max, double z_rand, double sigma_hit,
-  double lambda_short, double chi_outlier, size_t max_beams, map_t * map, double importance_factor)
+  double lambda_short, double chi_outlier, size_t max_beams, map_t * map, double importance_factor,
+  const double /*min_laser_hit_sample_dist*/)
 : Laser(max_beams, map)
 {
   z_hit_ = z_hit;

nav2_amcl/src/sensors/laser/likelihood_field_model.cpp

@@ -29,14 +29,16 @@ namespace nav2_amcl
 
 LikelihoodFieldModel::LikelihoodFieldModel(
   double z_hit, double z_rand, double sigma_hit,
-  double max_occ_dist, size_t max_beams, map_t * map, double importance_factor)
+  double max_occ_dist, size_t max_beams, map_t * map, double importance_factor,
+  const double min_laser_hit_sample_dist)
 : Laser(max_beams, map)
 {
   z_hit_ = z_hit;
   z_rand_ = z_rand;
   sigma_hit_ = sigma_hit;
   importance_factor_ = importance_factor;
   map_update_cspace(map, max_occ_dist);
+  min_laser_hit_sample_dist_sq_ = min_laser_hit_sample_dist * min_laser_hit_sample_dist;
 }
 
 double
@@ -77,6 +79,7 @@ LikelihoodFieldModel::sensorFunction(LaserData * data, pf_sample_set_t * set)
       step = 1;
     }
 
+    double last_hit_x = 0, last_hit_y = 0;
     for (i = 0; i < data->range_count; i += step) {
       obs_range = data->ranges[i][0];
       obs_bearing = data->ranges[i][1];
@@ -97,6 +100,18 @@ LikelihoodFieldModel::sensorFunction(LaserData * data, pf_sample_set_t * set)
       hit.v[0] = pose.v[0] + obs_range * cos(pose.v[2] + obs_bearing);
       hit.v[1] = pose.v[1] + obs_range * sin(pose.v[2] + obs_bearing);
 
+      if(i > 0)
+      {
+        const double dist_to_last_sample_sq = 
+          (hit.v[0] - last_hit_x) * (hit.v[0] - last_hit_x) +
+          (hit.v[1] - last_hit_y) * (hit.v[1] - last_hit_y);
+
+        if(dist_to_last_sample_sq < self->min_laser_hit_sample_dist_sq_)
+          continue;
+      }
+      last_hit_x = hit.v[0];
+      last_hit_y = hit.v[1];
+
       // Convert to map grid coords.
       int mi, mj;
       mi = MAP_GXWX(self->map_, hit.v[0]);

nav2_amcl/src/sensors/laser/likelihood_field_model_prob.cpp

@@ -34,7 +34,8 @@ LikelihoodFieldModelProb::LikelihoodFieldModelProb(
   double beam_skip_distance,
   double beam_skip_threshold,
   double beam_skip_error_threshold,
-  size_t max_beams, map_t * map, double importance_factor)
+  size_t max_beams, map_t * map, double importance_factor,
+  const double /*min_laser_hit_sample_dist*/)
 : Laser(max_beams, map)
 {
   z_hit_ = z_hit;