Amcl improvements

[12tbuchman]

Various improvements to AMCL including a design change in how we estimate the pose

---

This change is 

[bobhenz-jabil]

Reviewed 3 of 3 files at r1, 9 of 9 files at r2, 2 of 2 files at r3, 5 of 5 files at r4, 9 of 9 files at r5, 5 of 5 files at r6, 4 of 4 files at r7, 4 of 4 files at r8, 10 of 10 files at r9, 2 of 2 files at r10, 3 of 3 files at r11, 6 of 6 files at r12, 6 of 6 files at r13, 1 of 1 files at r14, 2 of 2 files at r15, 2 of 2 files at r16, 5 of 5 files at r17, 7 of 7 files at r18, 5 of 5 files at r19, 3 of 3 files at r20, all commit messages.
Reviewable status: all files reviewed, 15 unresolved discussions (waiting on @12tbuchman)

---

-- commits line 59 at r9:
typo: ranamed => renamed

---

-- commits line 5 at r10:
typo: updateDistnacesLUT => updateDistancesLUT

---

include/amcl/node/node_3d.h line 123 at r1 (raw file):

  int resample_count_;
  bool first_occupancy_map_received_;
  bool first_octomap_received_, new_octomap_received_;

nit: Add this on its own line. We don't stack declarations anywhere else in this list and it can make diffs more confusing to remove/add partial lines rather than full lines.

---

include/amcl/sensors/point_cloud_scanner.h line 119 at r17 (raw file):

  ros::NodeHandle nh_;
  ros::Publisher best_fit_cloud_pub_, best_fit_particles_pub_;

2 lines please don't stack declarations

---

include/amcl/node/node.h line 195 at r19 (raw file):

  double d_thresh_, a_thresh_;
  Eigen::Vector3d particle_cluster_size_;
  int min_particles_, max_particles_, pose_estimate_max_particles_;

don't stack declarations

---

src/amcl/sensors/point_cloud_scanner.cpp line 125 at r17 (raw file):

  Eigen::Vector3d pose;
  double z_hit_denom = 2 * sigma_hit_ * sigma_hit_;
  PointCloud::iterator it;

Move this declaration down inside the for-loop below...

for (PointCloud::iterator it = pose_cloud.begin(); it != pose_cloud.end(); ++it)

---

src/amcl/sensors/point_cloud_scanner.cpp line 127 at r17 (raw file):

  PointCloud::iterator it;
  PointCloud pose_cloud;
  std::vector<PValueIndex> p_values;

For efficiency you can call p_values.resize(set->sample_count); This will avoid causing the vector to reallocate during the loop and we know what the size is here, so there's no reason not to do it.

---

src/amcl/sensors/point_cloud_scanner.cpp line 155 at r17 (raw file):

  std::sort(p_values.begin(), p_values.end(), [] (const PValueIndex &a, const PValueIndex &b)
                                                  {return a.first > b.first;});
  best_fit_cloud_pub_ = nh_.advertise<PointCloud>("best_fit_cloud", 1);

Um, are you creating a new publisher every time you run through this code? 😬

---

src/amcl/sensors/point_cloud_scanner.cpp line 166 at r17 (raw file):

  pcl_conversions::toPCL(ros::Time::now(), pub_cloud->header.stamp);
  best_fit_cloud_pub_.publish(*pub_cloud);
  best_fit_particles_pub_ = nh_.advertise<geometry_msgs::PoseArray>("best_fit_particles", 1);

Um, are you creating a new publisher every time you run through this code? 😬

---

src/amcl/node/node_2d.cpp line 503 at r17 (raw file):

  {
    ROS_ERROR("No pose!");
  }

Rather than removing curly-braces, please add them to the "if" statement above (or don't change anything here at all).

---

src/amcl/node/node_3d.cpp line 460 at r17 (raw file):

  {
    ROS_ERROR("No pose!");
  }

Same comment here, either don't change anything or if you want to change something add them to the "if" above. (I would actually prefer no change.)

---

src/amcl/node/node_2d.cpp line 533 at r11 (raw file):

    {
      ROS_ERROR_STREAM("Couldn't get stats on cluster " << cluster_index);
      break;

You are changing this function in a subtle way. If we ever get this error, then it is possible that this function will return a non-zero (possibly incorrect) weight. I think hitting this error is pretty much 0% chance, but one way to avoid stopping part-way-through like that would just be to change the ROS_ERROR_STREAM to a ROS_WARN_STREAM and this "break" into a "continue". To be clear, that will still be changing the behavior of this function, but at least it is in a more-complete way if we run into this error (i.e. it will just ignore the one troublesome cluster instead of all clusters after the troublesome cluster)

---

src/amcl/node/node_3d.cpp line 250 at r1 (raw file):

  }
  scanner_.setMapFactors(off_map_factor_, non_free_space_factor_, non_free_space_radius_);
  node_->initFromNewMap(map_, not new_octomap_received_);

I don't understand this change. I think this will create a brand new particle filter and reset the pose etc. every time the code gets here and it has not received a new octomap.

---

src/amcl/node/node_3d.cpp line 484 at r11 (raw file):

    {
      ROS_ERROR_STREAM("Couldn't get stats on cluster " << cluster_index);
      break;

Same comment here. Change ROS_ERROR => ROS_WARN and break => continue.

---

src/amcl/pf/particle_filter.cpp line 504 at r20 (raw file):

  }

  cluster_particles_pub_ = nh_.advertise<geometry_msgs::PoseArray>("cluster_particles", 1);

Again, create this once, not over-and-over