Check for range_max of laserscan in updateFilter to avoid a implicit overflow crash.

[Cryst4L9527]

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses
Primary OS tested on	Ubuntu20.04
Robotic platform tested on	(gazebo simulation of turtlebot3

---

Description of contribution in a few bullet points

There is a possibility that the laser scan can be malformed or forged to be negative or zero, or, with a malfunction, the sensor may output a message with all the content zeros.

with a new laserscan received, the function UpdateFilter may be called:

AmclNode::laserReceived(sensor_msgs::msg::LaserScan::ConstSharedPtr laser_scan)
{
  ......
  bool resampled = false;

  // If the robot has moved, update the filter
  if (lasers_update_[laser_index]) {
    updateFilter(laser_index, laser_scan, pose);

    // Resample the particles
    if (!(++resample_count_ % resample_interval_)) {
      pf_update_resample(pf_);
      resampled = true;
    }
    ......
  }
  ......
}

go to updateFilter():

bool AmclNode::updateFilter(
  const int & laser_index,
  const sensor_msgs::msg::LaserScan::ConstSharedPtr & laser_scan,
  const pf_vector_t & pose)
{
  nav2_amcl::LaserData ldata;
  ldata.laser = lasers_[laser_index];
  ldata.range_count = laser_scan->ranges.size();
  ......
  // Apply range min/max thresholds, if the user supplied them
  if (laser_max_range_ > 0.0) {
    ldata.range_max = std::min(laser_scan->range_max, static_cast<float>(laser_max_range_));
  } else {
    ldata.range_max = laser_scan->range_max;
  }
 ......
  lasers_[laser_index]->sensorUpdate(pf_, reinterpret_cast<nav2_amcl::LaserData *>(&ldata));
  lasers_update_[laser_index] = false;
  pf_odom_pose_ = pose;
  return true;
}

if the laser_scan->range_max is zero, then the ldata->range_max will be zero, then in sensorUpdate->pf_update_sensor:

void pf_update_sensor(pf_t * pf, pf_sensor_model_fn_t sensor_fn, void * sensor_data)
{
  ......
  set = pf->sets + pf->current_set;
  // Compute the sample weights
  total = (*sensor_fn)(sensor_data, set);
  if (total > 0.0) {
    // Normalize weights
    double w_avg = 0.0;
    for (i = 0; i < set->sample_count; i++) {
      sample = set->samples + i;
      w_avg += sample->weight;
      sample->weight /= total;
    }
    // Update running averages of likelihood of samples (Prob Rob p258)
   ......
    }
  } else {
  ......
}

here in the sensor_fn(), eg. in likelihood_field_model:

double z_rand_mult = 1.0 / data->range_max;
pz += self->z_rand_ * z_rand_mult;
p += pz * pz * pz;
sample->weight *= p;
total_weight += sample->weight;

Therefore, z_rand_mult will be inf and the sample->weight will be inf, so will total_weight. sample->weight /= total will make the weight to be nan.

And in pf_update_resample() after updateFilter():

void pf_update_resample(pf_t * pf)
{
  ......
  set_a = pf->sets + pf->current_set;
  set_b = pf->sets + (pf->current_set + 1) % 2;

  // Build up cumulative probability table for resampling.
  c = (double *)malloc(sizeof(double) * (set_a->sample_count + 1));
  c[0] = 0.0;
  for (i = 0; i < set_a->sample_count; i++) {
    c[i + 1] = c[i] + set_a->samples[i].weight;
  }

  while (set_b->sample_count < pf->max_samples) {
    sample_b = set_b->samples + set_b->sample_count++;

    if (drand48() < w_diff) {
      sample_b->pose = (pf->random_pose_fn)(pf->random_pose_data);
    } else {
       .....
      // Naive discrete event sampler
      double r;
      r = drand48();
      for (i = 0; i < set_a->sample_count; i++) {
        if ((c[i] <= r) && (r < c[i + 1])) {
          break;
        }
      } //c[i]s are all nan, no break, until i = set_a->sample_count
      sample_a = set_a->samples + i; //overflow
      // Add sample to list
      sample_b->pose = sample_a->pose;
    }
......
}

Since it's a overflow bug more than a single crash, it may be exploitd maliciously o(╥﹏╥)o.
So I fixed it by: check if the range_max of the message is <=0 (although <0 won't cause the overflow crash, but it can be a semantic mistake), if so, just stop updating and return false, like the solution of other exceptions.

Description of documentation updates required from your changes

---

Future work that may be required in bullet points

No requirements.

For Maintainers:

• Check that any new parameters added are updated in navigation.ros.org
• Check that any significant change is added to the migration guide
• Check that any new features OR changes to existing behaviors are reflected in the tuning guide
• Check that any new functions have Doxygen added
• Check that any new features have test coverage
• Check that any new plugins is added to the plugins page
• If BT Node, Additionally: add to BT's XML index of nodes for groot, BT package's readme table, and BT library lists

[mergify]

@Cryst4L9527, please properly fill in PR template in the future. @SteveMacenski, use this instead.

• Check that any new parameters added are updated in navigation.ros.org
• Check that any significant change is added to the migration guide
• Check that any new features OR changes to existing behaviors are reflected in the tuning guide
• Check that any new functions have Doxygen added
• Check that any new features have test coverage
• Check that any new plugins is added to the plugins page
• If BT Node, Additionally: add to BT's XML index of nodes for groot, BT package's readme table, and BT library lists

[codecov]

Codecov Report

Base: 89.00% // Head: 88.90% // Decreases project coverage by -0.09% ⚠️

Coverage data is based on head (0f1c613) compared to base (a03cb63).
Patch coverage: 33.33% of modified lines in pull request are covered.

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #3276      +/-   ##
==========================================
- Coverage   89.00%   88.90%   -0.10%     
==========================================
  Files         351      351              
  Lines       15658    15661       +3     
==========================================
- Hits        13936    13924      -12     
- Misses       1722     1737      +15     

Impacted Files	Coverage Δ
nav2_amcl/src/amcl_node.cpp	70.01% <33.33%> (-0.30%)	⬇️
...clude/nav2_behavior_tree/bt_cancel_action_node.hpp	92.30% <0.00%> (-7.70%)	⬇️
..._planner/include/nav2_smac_planner/node_hybrid.hpp	92.85% <0.00%> (-7.15%)	⬇️
...ehaviors/include/nav2_behaviors/timed_behavior.hpp	84.93% <0.00%> (-6.85%)	⬇️
...stmap_2d/plugins/costmap_filters/binary_filter.cpp	94.80% <0.00%> (-2.60%)	⬇️
nav2_planner/src/planner_server.cpp	75.17% <0.00%> (-1.42%)	⬇️
nav2_navfn_planner/src/navfn_planner.cpp	95.36% <0.00%> (-1.04%)	⬇️
..._dwb_controller/dwb_critics/src/rotate_to_goal.cpp	91.11% <0.00%> (+8.88%)	⬆️

Help us with your feedback. Take ten seconds to tell us how you rate us. Have a feature suggestion? Share it here.

☔ View full report at Codecov.
📢 Do you have feedback about the report comment? Let us know in this issue.