Keepout Filter cleanup

nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp

@@ -63,11 +63,11 @@ void KeepoutFilter::initializeFilter(
   filter_info_topic_ = filter_info_topic;
   // Setting new costmap filter info subscriber
   RCLCPP_INFO(
-    node->get_logger(),
-    "Subscribing to \"%s\" topic for filter info...",
-    filter_info_topic.c_str());
+    logger_,
+    "KeepoutFilter: Subscribing to \"%s\" topic for filter info...",
+    filter_info_topic_.c_str());
   filter_info_sub_ = node->create_subscription<nav2_msgs::msg::CostmapFilterInfo>(
-    filter_info_topic, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(),
+    filter_info_topic_, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(),
     std::bind(&KeepoutFilter::filterInfoCallback, this, std::placeholders::_1));
 
   global_frame_ = layered_costmap_->getGlobalFrameID();
@@ -83,31 +83,35 @@ void KeepoutFilter::filterInfoCallback(
     throw std::runtime_error{"Failed to lock node"};
   }
 
-  // Resetting previous subscriber each time when new costmap filter information arrives
-  if (mask_sub_) {
+  if (!mask_sub_) {
+    RCLCPP_INFO(
+      logger_,
+      "KeepoutFilter: Received filter info from %s topic.", filter_info_topic_.c_str());
+  } else {
     RCLCPP_WARN(
-      node->get_logger(),
-      "New costmap filter info arrived from %s topic. Updating old filter info.",
+      logger_,
+      "KeepoutFilter: New costmap filter info arrived from %s topic. Updating old filter info.",
       filter_info_topic_.c_str());
+    // Resetting previous subscriber each time when new costmap filter information arrives
     mask_sub_.reset();
   }
 
   // Checking that base and multiplier are set to their default values
   if (msg->base != BASE_DEFAULT or msg->multiplier != MULTIPLIER_DEFAULT) {
     RCLCPP_ERROR(
-      node->get_logger(),
-      "For proper use of keepout filter base and multiplier in CostmapFilterInfo message "
-      "should be set to their default values (%f and %f)",
+      logger_,
+      "KeepoutFilter: For proper use of keepout filter base and multiplier"
+      " in CostmapFilterInfo message should be set to their default values (%f and %f)",
       BASE_DEFAULT, MULTIPLIER_DEFAULT);
   }
 
   mask_topic_ = msg->filter_mask_topic;
 
   // Setting new filter mask subscriber
   RCLCPP_INFO(
-    node->get_logger(),
-    "Subscribing to \"%s\" topic for filter mask...",
-    msg->filter_mask_topic.c_str());
+    logger_,
+    "KeepoutFilter: Subscribing to \"%s\" topic for filter mask...",
+    mask_topic_.c_str());
   mask_sub_ = node->create_subscription<nav_msgs::msg::OccupancyGrid>(
     mask_topic_, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(),
     std::bind(&KeepoutFilter::maskCallback, this, std::placeholders::_1));
@@ -123,10 +127,14 @@ void KeepoutFilter::maskCallback(
     throw std::runtime_error{"Failed to lock node"};
   }
 
-  if (mask_costmap_) {
+  if (!mask_costmap_) {
+    RCLCPP_INFO(
+      logger_,
+      "KeepoutFilter: Received filter mask from %s topic.", mask_topic_.c_str());
+  } else {
     RCLCPP_WARN(
-      node->get_logger(),
-      "New filter mask arrived from %s topic. Updating old filter mask.",
+      logger_,
+      "KeepoutFilter: New filter mask arrived from %s topic. Updating old filter mask.",
       mask_topic_.c_str());
     mask_costmap_.reset();
   }
@@ -143,16 +151,11 @@ void KeepoutFilter::process(
 {
   std::lock_guard<CostmapFilter::mutex_t> guard(*getMutex());
 
-  rclcpp_lifecycle::LifecycleNode::SharedPtr node = node_.lock();
-  if (!node) {
-    throw std::runtime_error{"Failed to lock node"};
-  }
-
   if (!mask_costmap_) {
     // Show warning message every 2 seconds to not litter an output
     RCLCPP_WARN_THROTTLE(
-      node->get_logger(), *(node->get_clock()), 2000,
-      "Filter mask was not received");
+      logger_, *(clock_), 2000,
+      "KeepoutFilter: Filter mask was not received");
     return;
   }
 
@@ -171,8 +174,8 @@ void KeepoutFilter::process(
         transform_tolerance_);
     } catch (tf2::TransformException & ex) {
       RCLCPP_ERROR(
-        node->get_logger(),
-        "KeepoutFilter: failed to get costmap frame (%s) "
+        logger_,
+        "KeepoutFilter: Failed to get costmap frame (%s) "
         "transformation to mask frame (%s) with error: %s",
         global_frame_.c_str(), mask_frame_.c_str(), ex.what());
       return;
@@ -296,6 +299,7 @@ void KeepoutFilter::resetFilter()
 bool KeepoutFilter::isActive()
 {
   std::lock_guard<CostmapFilter::mutex_t> guard(*getMutex());
+
   if (mask_costmap_) {
     return true;
   }

nav2_costmap_2d/test/unit/keepout_filter_test.cpp

@@ -51,8 +51,8 @@ class InfoPublisher : public rclcpp::Node
       std::make_unique<nav2_msgs::msg::CostmapFilterInfo>();
     msg->type = 0;
     msg->filter_mask_topic = MASK_TOPIC;
-    msg->base = base;
-    msg->multiplier = multiplier;
+    msg->base = static_cast<float>(base);
+    msg->multiplier = static_cast<float>(multiplier);
 
     publisher_->publish(std::move(msg));
   }

nav2_system_tests/maps/keepout_mask.yaml

@@ -4,5 +4,3 @@ origin: [-10.000000, -10.000000, 0.000000]
 negate: 0
 occupied_thresh: 0.65
 free_thresh: 0.196
-map_type: occupancy
-