From 0e3bb735fee24f2adaacd02ec37b29403f3db94e Mon Sep 17 00:00:00 2001 From: Tyler Buchman Date: Thu, 27 Mar 2025 17:59:13 -0400 Subject: [PATCH 1/2] Clean up dynamic reconfigure Dynamic reconfigure has become overly complex despite being unused. To simplify, remove any parameters that would require re-initializing an object to use. If these parameters need to be changed it should be done through salt. --- cfg/AMCL.cfg | 99 +++------------------------------------ src/amcl/node/node.cpp | 59 ++--------------------- src/amcl/node/node_2d.cpp | 43 ----------------- src/amcl/node/node_3d.cpp | 36 -------------- 4 files changed, 10 insertions(+), 227 deletions(-) diff --git a/cfg/AMCL.cfg b/cfg/AMCL.cfg index 7505f43..63af4a3 100755 --- a/cfg/AMCL.cfg +++ b/cfg/AMCL.cfg @@ -7,106 +7,19 @@ from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, i gen = ParameterGenerator() -map_type_enum = gen.enum([ gen.const("OccupancyMap", int_t, 2, "Use a static occupancy map"), - gen.const("OctoMap", int_t, 3, "Use a static OctoMap") - ], "Type of static map to use for localization") - -gen.add("map_type", int_t, 0, "Type of static map to use", 3, edit_method=map_type_enum) - -gen.add("min_particles", int_t, 0, "Minimum allowed number of particles.", 100, 0, 1000) -gen.add("max_particles", int_t, 0, "Mamimum allowed number of particles.", 5000, 0, 10000) -gen.add("pose_estimate_max_particles", int_t, 0, "Maximum number of particles used to estimate the pose.", 100, 1, 10000) -gen.add("num_best_fit_particles", int_t, 0, "Number of particles to use in the best fit message used for debugging.", 100, 1, 10000) -gen.add("particle_cluster_size_x", double_t, 0, - "Cell size used for clustering in the X-dimension (meters).", - 0.5, 0.001, 10) -gen.add("particle_cluster_size_y", double_t, 0, - "Cell size used for clustering in the Y-dimension (meters).", - 0.5, 0.001, 10) -gen.add("particle_cluster_size_yaw", double_t, 0, - "Cell size used for clustering yaw angles (radians).", - 10*pi/180, 0.001, 7) - -gen.add("kld_err", double_t, 0, "Maximum error between the true distribution and the estimated distribution.", .01, 0, 1) -gen.add("kld_z", double_t, 0, "Upper standard normal quantile for (1 - p), where p is the probability that the error on the estimated distrubition will be less than kld_err.", .99, 0, 1) - +gen.add("restore_defaults", bool_t, 0, "Restore the default configuration", False) gen.add("update_min_d", double_t, 0, "Translational movement required before performing a filter update.", .2, 0, 5) gen.add("update_min_a", double_t, 0, "Rotational movement required before performing a filter update.", pi/6, 0, 2*pi) - -gen.add("resample_interval", int_t, 0, "Number of filter updates required before resampling.", 2, 0, 20) - -gen.add("transform_tolerance", double_t, 0, "Time with which to post-date the transform that is published, to indicate that this transform is valid into the future.", .1, 0, 2) - -gen.add("recovery_alpha_slow", double_t, 0, "Exponential decay rate for the slow average weight filter, used in deciding when to recover by adding random poses. A good value might be 0.001.", 0, 0, .5) -gen.add("recovery_alpha_fast", double_t, 0, "Exponential decay rate for the fast average weight filter, used in deciding when to recover by adding random poses. A good value might be 0.1.", 0, 0, 1) - -gen.add("uniform_pose_starting_weight_threshold", double_t, 0, "When adding uniform poses, attempt to pick a pose with at least this sample weight according to the sensor model.", 0.0, 0.0, 10.0) -gen.add("uniform_pose_deweight_multiplier", double_t, 0, "When adding uniform poses, deweight uniform_pose_starting_weight_threshold by this multiplier for each try. This guarantees that we will eventually find a pose.", 0.0, 0.0, 1.0) - -gen.add("global_localization_alpha_slow", double_t, 0, "During global localization, override recovery alpha_slow to this value. A good value might be 0.001.", 0, 0, .5) -gen.add("global_localization_alpha_fast", double_t, 0, "During global localization, override recovery alpha_fast to this value. A good value might be 0.1.", 0, 0, 1) - -gen.add("do_beamskip", bool_t, 0, "When true skips scans when a scan doesnt work for a majority of particles", False) -gen.add("beam_skip_distance", double_t, 0, "Distance from a valid map point before scan is considered invalid", 0, 2, 0.5) -gen.add("beam_skip_threshold", double_t, 0, "Ratio of samples for which the scans are valid to consider as valid scan", 0, 1, 0.3) - -gen.add("tf_broadcast", bool_t, 0, "When true (the default), publish results via TF. When false, do not.", True) -gen.add("tf_reverse", bool_t, 0, "When set to true, reverse published TF.", False) -gen.add("gui_publish_rate", double_t, 0, "Maximum rate (Hz) at which scans and paths are published for visualization, -1.0 to disable.", -1, -1, 100) gen.add("transform_publish_rate", double_t, 0, "Rate (Hz) at which to publish the transform between map and odom to tf.", 50.0, 0.1, 100.0) gen.add("save_pose_to_file_rate", double_t, 0, "Maximum rate (Hz) at which to store the last estimated pose and covariance to file. This saved pose will be used on subsequent runs to initialize the filter if the param server does not have the parameters stored. 0.0 to disable.", 0.1, 0.0, 10.0) +gen.add("transform_tolerance", double_t, 0, "Time with which to post-date the transform that is published, to indicate that this transform is valid into the future.", .1, 0, 2) +gen.add("save_pose", bool_t, 0, "If the node should save the pose to the param server and to a persistent file.", True) -gen.add("use_map_topic", bool_t, 0, "When set to true, AMCL will subscribe to the map topic rather than making a service call to receive its map.", False) -gen.add("first_map_only", bool_t, 0, "When set to true, AMCL will only use the first map it subscribes to, rather than updating each time a new one is received.", False) -gen.add("wait_for_occupancy_map", bool_t, 0, "When set to true and map type is OctoMap, AMCL will wait for the occupancy map message before creating the OctoMap", True) - -# Model Parameters +# 2D parameters gen.add("laser_min_range", double_t, 0, "Minimum scan range to be considered; -1.0 will cause the scanner's reported minimum range to be used.", -1, -1, 1000) gen.add("laser_max_range", double_t, 0, "Maximum scan range to be considered; -1.0 will cause the scanner's reported maximum range to be used.", -1, -1, 1000) -gen.add("laser_max_beams", int_t, 0, "How many evenly-spaced beams in each scan to be used when updating the filter.", 30, 0, 100) - -gen.add("laser_z_hit", double_t, 0, "Mixture weight for the z_hit part of the model.", .95, 0, 10) -gen.add("laser_z_short", double_t, 0, "Mixture weight for the z_short part of the model.", .1, 0, 10) -gen.add("laser_z_max", double_t, 0, "Mixture weight for the z_max part of the model.", .05, 0, 10) -gen.add("laser_z_rand", double_t, 0, "Mixture weight for the z_rand part of the model.", .05, 0, 10) - -gen.add("laser_gompertz_a", double_t, 0, "Gompertz a coefficient for gompertz sample weight function", 1.0, 0.0, 10.0) -gen.add("laser_gompertz_b", double_t, 0, "Gompertz b coefficient for gompertz sample weight function", 1.0, 0.0, 10.0) -gen.add("laser_gompertz_c", double_t, 0, "Gompertz c coefficient for gompertz sample weight function", 1.0, 0.0, 10.0) -gen.add("laser_gompertz_input_shift", double_t, 0, "Shift input value to gompertz function (after input scaling)", 0.0, -10.0, 10.0) -gen.add("laser_gompertz_input_scale", double_t, 0, "Scale input value to gompertz function (before input shifting)", 1.0, 0.0, 10.0) -gen.add("laser_gompertz_output_shift", double_t, 0, "Shift output value of gompertz function", 0.0, -10.0, 10.0) - -# There is no option for output scale since the output will just be normalized by the particle filter - -gen.add("laser_sigma_hit", double_t, 0, "Standard deviation for Gaussian model used in z_hit part of the model.", .2, 0, 10) -gen.add("laser_lambda_short", double_t, 0, "Exponential decay parameter for z_short part of model.", .1, 0, 10) -gen.add("laser_likelihood_max_dist", double_t, 0, "Maximum distance to do obstacle inflation on map, for use in likelihood_field model.", 2, 0, 20) -gen.add("laser_off_map_factor", double_t, 0, "Factor applied to particle weights out of the map bounds.", 1.0, 0.0, 1.0) -gen.add("laser_non_free_space_factor", double_t, 0, "Factor applied ot particle weights not in free space.", 1.0, 0.0, 1.0) -gen.add("laser_non_free_space_radius", double_t, 0, "Radius used to interpolate laser_non_free_space_factor near non free space.", 0.0, 0.0, 10.0) -gen.add("global_localization_laser_off_map_factor", double_t, 0, "During global localization, Factor applied to particle weights out of the map bounds.", 1.0, 0.0, 1.0) -gen.add("global_localization_laser_non_free_space_factor", double_t, 0, "During global localization, override factor applied ot particle weights not in free space.", 1.0, 0.0, 1.0) - -gen.add("odom_alpha1", double_t, 0, "Specifies the expected noise in odometry's rotation estimate from the rotational component of the robot's motion.", .2, 0, 10) -gen.add("odom_alpha2", double_t, 0, "Specifies the expected noise in odometry's rotation estimate from the translational component of the robot's motion.", .2, 0, 10) -gen.add("odom_alpha3", double_t, 0, "Specifies the expected noise in odometry's translation estimate from the translational component of the robot's motion.", .2, 0, 10) -gen.add("odom_alpha4", double_t, 0, "Specifies the expected noise in odometry's translation estimate from the rotational component of the robot's motion.", .2, 0, 10) -gen.add("odom_alpha5", double_t, 0, "Specified the expected noise in odometry's sideways translation estimate from the sideways translational component of the robot's motion.", .2, 0, 10) - -gen.add("odom_frame_id", str_t, 0, "Which frame to use for odometry.", "odom") -gen.add("base_frame_id", str_t, 0, "Which frame to use for the robot base.", "base_link") -gen.add("global_frame_id", str_t, 0, "The name of the common coordinate frame.", "map") -gen.add("transform_frame_id", str_t, 0, "The name of the coordinate frame published by the localization system.", "map") - -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("restore_defaults", bool_t, 0, "Restore the default configuration", False) - -gen.add("save_pose", bool_t, 0, "If the node should save the pose to the param server and to a persistent file.", True) -gen.add("saved_pose_filepath", str_t, 0, "Path of file to store saved poses.", "badger_amcl_saved_pose.yaml") +# 3D parameters +gen.add("resample_interval", int_t, 0, "Number of filter updates required before resampling.", 2, 0, 20) exit(gen.generate(PACKAGE, "badger_amcl", "AMCL")) diff --git a/src/amcl/node/node.cpp b/src/amcl/node/node.cpp index 7816d06..fbe1dbe 100644 --- a/src/amcl/node/node.cpp +++ b/src/amcl/node/node.cpp @@ -176,67 +176,16 @@ void Node::reconfigureCB(AMCLConfig& config, uint32_t level) a_thresh_ = config.update_min_a; transform_publish_period_ = ros::Duration(1.0 / config.transform_publish_rate); + publish_transform_timer_.setPeriod(transform_publish_period_); save_pose_to_file_period_ = ros::Duration(1.0 / config.save_pose_to_file_rate); + save_pose_to_file_timer_.setPeriod(save_pose_to_file_period_); transform_tolerance_.fromSec(config.transform_tolerance); - alpha1_ = config.odom_alpha1; - alpha2_ = config.odom_alpha2; - alpha3_ = config.odom_alpha3; - alpha4_ = config.odom_alpha4; - alpha5_ = config.odom_alpha5; - - if (config.min_particles > config.max_particles) - { - ROS_WARN("You've set min_particles to be greater than max particles, " - "this isn't allowed so they'll be set to be equal."); - config.max_particles = config.min_particles; - } - - min_particles_ = config.min_particles; - max_particles_ = config.max_particles; - alpha_slow_ = config.recovery_alpha_slow; - alpha_fast_ = config.recovery_alpha_fast; - uniform_pose_starting_weight_threshold_ = config.uniform_pose_starting_weight_threshold; - uniform_pose_deweight_multiplier_ = config.uniform_pose_deweight_multiplier; - global_localization_alpha_slow_ = config.global_localization_alpha_slow; - global_localization_alpha_fast_ = config.global_localization_alpha_fast; - tf_broadcast_ = config.tf_broadcast; - tf_reverse_ = config.tf_reverse; - - uniform_pose_generator_fn_ = std::bind(&Node::uniformPoseGenerator, this); - particle_cluster_size_ = Eigen::Vector3d( - config.particle_cluster_size_x, config.particle_cluster_size_y, config.particle_cluster_size_yaw); - pf_ = std::make_shared(particle_cluster_size_, global_frame_id_, - min_particles_, max_particles_, pose_estimate_max_particles_, - alpha_slow_, alpha_fast_, - global_localization_convergence_threshold_, - uniform_pose_generator_fn_); - pf_err_ = config.kld_err; - pf_z_ = config.kld_z; - pf_->setPopulationSizeParameters(pf_err_, pf_z_); - - // Initialize the filter - Eigen::Vector3d pf_init_pose_mean; - pf_init_pose_mean[0] = last_published_pose_->pose.pose.position.x; - pf_init_pose_mean[1] = last_published_pose_->pose.pose.position.y; - pf_init_pose_mean[2] = tf2::getYaw(last_published_pose_->pose.pose.orientation); - Eigen::Matrix3d pf_init_pose_cov; - pf_init_pose_cov(0, 0) = last_published_pose_->pose.covariance[COVARIANCE_XX]; - pf_init_pose_cov(1, 1) = last_published_pose_->pose.covariance[COVARIANCE_YY]; - pf_init_pose_cov(2, 2) = last_published_pose_->pose.covariance[COVARIANCE_AA]; - pf_->initWithGaussian(pf_init_pose_mean, pf_init_pose_cov); - odom_initialized_ = false; - odom_.initModel(alpha1_, alpha2_, alpha3_, alpha4_, alpha5_); - odom_frame_id_ = config.odom_frame_id; - base_frame_id_ = config.base_frame_id; - global_frame_id_ = config.global_frame_id; - transform_frame_id_ = config.transform_frame_id; node_->reconfigure(config); save_pose_ = config.save_pose; - saved_pose_filepath_ = config.saved_pose_filepath; - publish_transform_timer_.setPeriod(transform_publish_period_); - save_pose_to_file_timer_.setPeriod(save_pose_to_file_period_); + + ROS_INFO("Finished AMCL dynamic reconfigure"); } void Node::setPfDecayRateNormal() diff --git a/src/amcl/node/node_2d.cpp b/src/amcl/node/node_2d.cpp index 89bf2cb..9c90860 100644 --- a/src/amcl/node/node_2d.cpp +++ b/src/amcl/node/node_2d.cpp @@ -108,49 +108,6 @@ void Node2D::reconfigure(AMCLConfig& config) { sensor_min_range_ = config.laser_min_range; sensor_max_range_ = config.laser_max_range; - z_hit_ = config.laser_z_hit; - z_short_ = config.laser_z_short; - z_max_ = config.laser_z_max; - z_rand_ = config.laser_z_rand; - sigma_hit_ = config.laser_sigma_hit; - lambda_short_ = config.laser_lambda_short; - sensor_likelihood_max_dist_ = config.laser_likelihood_max_dist; - off_map_factor_ = config.laser_off_map_factor; - non_free_space_factor_ = config.laser_non_free_space_factor; - non_free_space_radius_ = config.laser_non_free_space_radius; - global_localization_off_map_factor_ = config.global_localization_laser_off_map_factor; - global_localization_non_free_space_factor_ = config.global_localization_laser_non_free_space_factor; - resample_interval_ = config.resample_interval; - do_beamskip_ = config.do_beamskip; - beam_skip_distance_ = config.beam_skip_distance; - beam_skip_threshold_ = config.beam_skip_threshold; - gompertz_a_ = config.laser_gompertz_a; - gompertz_b_ = config.laser_gompertz_b; - gompertz_c_ = config.laser_gompertz_c; - gompertz_input_shift_ = config.laser_gompertz_input_shift; - gompertz_input_scale_ = config.laser_gompertz_input_scale; - gompertz_output_shift_ = config.laser_gompertz_output_shift; - max_beams_ = config.laser_max_beams; - scanner_.init( - max_beams_, map_, z_hit_, z_rand_, sigma_hit_, sensor_likelihood_max_dist_, gompertz_a_, gompertz_b_, - gompertz_c_, gompertz_input_shift_, gompertz_input_scale_, gompertz_output_shift_); - ROS_INFO("Gompertz key points by total planar scan match: 0.0: %f, 0.25: %f, 0.5: %f, 0.75: %f, 1.0: %f", - scanner_.applyGompertz(z_rand_), - scanner_.applyGompertz(z_rand_ + z_hit_ * .25), - scanner_.applyGompertz(z_rand_ + z_hit_ * .5), - scanner_.applyGompertz(z_rand_ + z_hit_ * .75), - scanner_.applyGompertz(z_rand_ + z_hit_)); - scanner_.setMapFactors(off_map_factor_, non_free_space_factor_, non_free_space_radius_); - - scan_filter_.reset(); - scan_sub_.reset(); - - scan_sub_.reset(new message_filters::Subscriber(nh_, scan_topic_, 1)); - scan_filter_.reset(new tf2_ros::MessageFilter(*scan_sub_.get(), tf_buffer_, - node_->getOdomFrameId(), 1, nh_)); - - scan_filter_->registerCallback(std::bind(&Node2D::scanReceived, this, std::placeholders::_1)); - pf_ = node_->getPfPtr(); } void Node2D::mapMsgReceived(const nav_msgs::OccupancyGridConstPtr& msg) diff --git a/src/amcl/node/node_3d.cpp b/src/amcl/node/node_3d.cpp index 678b0bd..843db58 100644 --- a/src/amcl/node/node_3d.cpp +++ b/src/amcl/node/node_3d.cpp @@ -106,42 +106,6 @@ Node3D::~Node3D() void Node3D::reconfigure(AMCLConfig& config) { resample_interval_ = config.resample_interval; - max_beams_ = config.laser_max_beams; - z_hit_ = config.laser_z_hit; - z_short_ = config.laser_z_short; - z_max_ = config.laser_z_max; - z_rand_ = config.laser_z_rand; - sigma_hit_ = config.laser_sigma_hit; - max_distance_to_object_ = config.laser_likelihood_max_dist; - off_map_factor_ = config.laser_off_map_factor; - non_free_space_factor_ = config.laser_non_free_space_factor; - non_free_space_radius_ = config.laser_non_free_space_radius; - global_localization_off_map_factor_ = config.global_localization_laser_off_map_factor; - global_localization_non_free_space_factor_ = config.global_localization_laser_non_free_space_factor; - num_best_fit_particles_ = config.num_best_fit_particles; - scanner_.init( - max_beams_, map_, global_frame_id_, z_hit_, z_rand_, sigma_hit_, gompertz_a_, gompertz_b_, gompertz_c_, - gompertz_input_shift_, gompertz_input_scale_, gompertz_output_shift_, num_best_fit_particles_); - ROS_INFO("Gompertz key points by total planar scan match: " - "0.0: %f, 0.25: %f, 0.5: %f, 0.75: %f, 1.0: %f", - scanner_.applyGompertz(z_rand_), - scanner_.applyGompertz(z_rand_ + z_hit_ * .25), - scanner_.applyGompertz(z_rand_ + z_hit_ * .5), - scanner_.applyGompertz(z_rand_ + z_hit_ * .75), - scanner_.applyGompertz(z_rand_ + z_hit_)); - - scanner_.setMapFactors(off_map_factor_, non_free_space_factor_, non_free_space_radius_); - - cloud_filter_.reset(); - cloud_sub_.reset(); - - cloud_sub_.reset(new message_filters::Subscriber(nh_, cloud_topic_, 1)); - cloud_filter_.reset(new tf2_ros::MessageFilter(*cloud_sub_, tf_buffer_, - node_->getOdomFrameId(), 1, nh_)); - - cloud_filter_->registerCallback(std::bind(&Node3D::scanReceived, this, std::placeholders::_1)); - pf_ = node_->getPfPtr(); - publish_distances_lut_ = config.publish_distances_lut; } void Node3D::occupancyMapMsgReceived(const nav_msgs::OccupancyGridConstPtr& msg) From 428275001ac32a757c1fe19adcef7e1cefa0aafe Mon Sep 17 00:00:00 2001 From: Tyler Buchman Date: Thu, 27 Mar 2025 17:22:25 -0400 Subject: [PATCH 2/2] Add distances lookup table max z Lowering the max z of the lookup table helps reduce the impact of drop ceilings. --- include/amcl/map/octomap.h | 3 ++- include/amcl/node/node_3d.h | 1 + src/amcl/map/octomap.cpp | 13 ++++++++++--- src/amcl/node/node_3d.cpp | 4 +++- 4 files changed, 16 insertions(+), 5 deletions(-) diff --git a/include/amcl/map/octomap.h b/include/amcl/map/octomap.h index 01cd7c7..dc0be3b 100644 --- a/include/amcl/map/octomap.h +++ b/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 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 diff --git a/include/amcl/node/node_3d.h b/include/amcl/node/node_3d.h index 952e7a2..b5fe62e 100644 --- a/include/amcl/node/node_3d.h +++ b/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_; diff --git a/src/amcl/map/octomap.cpp b/src/amcl/map/octomap.cpp index d94792d..bf3365f 100644 --- a/src/amcl/map/octomap.cpp +++ b/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]; diff --git a/src/amcl/node/node_3d.cpp b/src/amcl/node/node_3d.cpp index 843db58..616d9eb 100644 --- a/src/amcl/node/node_3d.cpp +++ b/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>( @@ -208,7 +210,7 @@ std::shared_ptr Node3D::convertMap(const octomap_msgs::Octomap& map_msg } double resolution = map_msg.resolution; std::shared_ptr octomap = std::make_shared( - 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();