Add distances lookup table max z

Lowering the max z of the lookup table helps reduce the impact of drop
ceilings.

Author: 12tbuchman

cfg/AMCL.cfg

@@ -103,6 +103,7 @@ gen.add("transform_frame_id", str_t, 0, "The name of the coordinate frame publis
 gen.add("off_object_penalty_factor", double_t, 0, "Penalty factor for points that miss an object on the static map.", 1000.0, 0.0, 100000.0)
 
 gen.add("publish_distances_lut", bool_t, 0, "Whether to publish the Distances Lookup Table as a point cloud for debugging.", False)
+gen.add("lut_max_z", double_t, 0, "Add octopmap objects to the lookup table only if they have a z height below this value.", 10.0, 0.0, 10.0)
 
 gen.add("restore_defaults", bool_t, 0, "Restore the default configuration", False)
 

include/amcl/map/octomap.h

@@ -59,7 +59,7 @@ class OctoMap : public Map
 {
 public:
   OctoMap(double resolution, std::string global_frame_id);
-  OctoMap(double resolution, std::string global_frame_id, bool publish_distances_lut);
+  OctoMap(double resolution, std::string global_frame_id, bool publish_distances_lut, double lut_max_z);
   virtual ~OctoMap() = default;
   virtual void initFromOctree(std::shared_ptr<octomap::OcTree> octree, double max_distance_to_object);
   // Convert from voxel indices to point in map frame
@@ -116,6 +116,7 @@ class OctoMap : public Map
   ros::NodeHandle nh_;
   ros::Publisher distances_lut_pub_;
   bool publish_distances_lut_;
+  double lut_max_z_;
   std::string global_frame_id_;
 };
 }  // namespace amcl

include/amcl/node/node_3d.h

@@ -134,6 +134,7 @@ class Node3D : public NodeND
   double non_free_space_radius_;
   double z_hit_, z_short_, z_max_, z_rand_, sigma_hit_, lambda_short_;
   bool publish_distances_lut_;
+  double lut_max_z_;
   double global_localization_off_map_factor_;
   double global_localization_non_free_space_factor_;
   bool global_localization_active_;

src/amcl/map/octomap.cpp

@@ -35,11 +35,12 @@ namespace badger_amcl
 {
 
 OctoMap::OctoMap(double resolution, std::string global_frame_id)
-    : OctoMap(resolution, global_frame_id, false) {}
+    : OctoMap(resolution, global_frame_id, false, 10.0) {}
 
-OctoMap::OctoMap(double resolution, std::string global_frame_id, bool publish_distances_lut)
+OctoMap::OctoMap(double resolution, std::string global_frame_id, bool publish_distances_lut, double lut_max_z)
     : Map(resolution),
       publish_distances_lut_(publish_distances_lut),
+      lut_max_z_(lut_max_z),
       cdm_(resolution, 0.0),
       global_frame_id_(global_frame_id)
 {
@@ -207,7 +208,11 @@ void OctoMap::updateDistancesLUT()
   if (publish_distances_lut_)
   {
     publishDistancesLUT();
-    ROS_INFO("Octree published");
+    ROS_INFO("Distances lookup table published");
+  }
+  else
+  {
+    ROS_INFO("Distances lookup table not published");
   }
   distances_lut_created_ = true;
 }
@@ -231,6 +236,8 @@ void OctoMap::iterateObstacleCells(CellDataQueue& q)
       point[0] = it.getX();
       point[1] = it.getY();
       point[2] = it.getZ();
+      if (point[2] > lut_max_z_)
+        continue;
       rasterize(point, &voxel);
       i = voxel[0];
       j = voxel[1];

src/amcl/node/node_3d.cpp

@@ -78,6 +78,8 @@ Node3D::Node3D(Node* node, std::mutex& configuration_mutex, std::string global_f
   private_nh_.param("global_localization_scanner_non_free_space_factor",
                     global_localization_non_free_space_factor_, 1.0);
   private_nh_.param("map_scale_up_factor", occupancy_map_scale_up_factor_, 1);
+  private_nh_.param("publish_distances_lut", publish_distances_lut_, false);
+  private_nh_.param("lut_max_z", lut_max_z_, 10.0);
 
   cloud_topic_ = "cloud";
   cloud_sub_ = std::unique_ptr<message_filters::Subscriber<sensor_msgs::PointCloud2>>(
@@ -208,7 +210,7 @@ std::shared_ptr<OctoMap> Node3D::convertMap(const octomap_msgs::Octomap& map_msg
   }
   double resolution = map_msg.resolution;
   std::shared_ptr<OctoMap> octomap = std::make_shared<OctoMap>(
-      resolution, global_frame_id_, publish_distances_lut_);
+      resolution, global_frame_id_, publish_distances_lut_, lut_max_z_);
   ROS_ASSERT(octomap);
   octomap->initFromOctree(octree_, max_distance_to_object_);
   octree_.reset();