Expose model_unknown_space parameter to ROS2

Signed-off-by: Diego Palma <dpalma@symbotic.com>
Ekumen-OS · Oct 20, 2024 · 27fd135 · 27fd135
1 parent d547b8a
commit 27fd135
Show file tree

Hide file tree

Showing 5 changed files with 18 additions and 0 deletions.
diff --git a/beluga_amcl/config/Amcl.cfg b/beluga_amcl/config/Amcl.cfg
@@ -263,6 +263,12 @@ gen.add(
     default=0.05, min=0., max=1.
 )
 
+gen.add(
+    "model_unknown_space", bool_t, 0,
+    "Whether to model unknown space or assume it free.",
+    default=False,
+)
+
 gen.add(
     "laser_z_max", double_t, 0,
     "Mixture weight for the probability of getting max range measurements.",

diff --git a/beluga_amcl/src/amcl_node.cpp b/beluga_amcl/src/amcl_node.cpp
@@ -120,6 +120,12 @@ AmclNode::AmclNode(const rclcpp::NodeOptions& options) : BaseAMCLNode{"amcl", ""
     declare_parameter("z_rand", rclcpp::ParameterValue(0.5), descriptor);
   }
 
+  {
+    auto descriptor = rcl_interfaces::msg::ParameterDescriptor();
+    descriptor.description = "If false, Likelihood Field sensor model won't model unknown space.";
+    declare_parameter("model_unknown_space", false, descriptor);
+  }
+
   {
     auto descriptor = rcl_interfaces::msg::ParameterDescriptor();
     descriptor.description = "Mixture weight for the probability of getting max range measurements.";
@@ -304,6 +310,7 @@ auto AmclNode::get_sensor_model(std::string_view name, nav_msgs::msg::OccupancyG
     params.z_hit = get_parameter("z_hit").as_double();
     params.z_random = get_parameter("z_rand").as_double();
     params.sigma_hit = get_parameter("sigma_hit").as_double();
+    params.model_unknown_space = get_parameter("model_unknown_space").as_bool();
     return beluga::LikelihoodFieldModel{params, beluga_ros::OccupancyGrid{map}};
   }
   if (name == kBeamSensorModelName) {

diff --git a/beluga_amcl/src/amcl_nodelet.cpp b/beluga_amcl/src/amcl_nodelet.cpp
@@ -194,6 +194,7 @@ auto AmclNodelet::get_sensor_model(std::string_view name, const nav_msgs::Occupa
     params.z_hit = config_.laser_z_hit;
     params.z_random = config_.laser_z_rand;
     params.sigma_hit = config_.laser_sigma_hit;
+    params.model_unknown_space = config_.model_unknown_space;
     return beluga::LikelihoodFieldModel{params, beluga_ros::OccupancyGrid{map}};
   }
   if (name == kBeamSensorModelName) {

diff --git a/beluga_amcl/test/test_amcl_node.cpp b/beluga_amcl/test/test_amcl_node.cpp
@@ -733,6 +733,8 @@ INSTANTIATE_TEST_SUITE_P(
         rclcpp::Parameter("z_hit", 2.0),
         rclcpp::Parameter("z_rand", -1.0),
         rclcpp::Parameter("z_rand", 2.0),
+        rclcpp::Parameter("model_unknown_space", false),
+        rclcpp::Parameter("model_unknown_space", true),
         rclcpp::Parameter("z_max", -1.0),
         rclcpp::Parameter("z_max", 2.0),
         rclcpp::Parameter("z_short", -1.0),

diff --git a/beluga_example/params/default.ros2.yaml b/beluga_example/params/default.ros2.yaml
@@ -61,6 +61,8 @@ amcl:
     z_hit: 0.5
     # Weight used to combine the probability of random noise in perception.
     z_rand: 0.5
+    # Whether to model unknown space or assume it free.
+    model_unknown_space : false
     # Weight used to combine the probability of getting short readings.
     z_short: 0.05
     # Weight used to combine the probability of getting max range readings.