backport the fix for setting binary_state as the default (#5459)

Signed-off-by: olaghattas <olaghattas@hotmail.com>

Author: olaghattas

nav2_costmap_2d/plugins/costmap_filters/binary_filter.cpp

@@ -96,8 +96,8 @@ void BinaryFilter::initializeFilter(
   base_ = BASE_DEFAULT;
   multiplier_ = MULTIPLIER_DEFAULT;
 
-  // Initialize state as "false" by-default
-  changeState(default_state_);
+  // Initialize state binary_state_ which at start its equal to default_state_
+  changeState(binary_state_);
 }
 
 void BinaryFilter::filterInfoCallback(
@@ -224,8 +224,11 @@ void BinaryFilter::resetFilter()
 {
   std::lock_guard<CostmapFilter::mutex_t> guard(*getMutex());
 
-  RCLCPP_INFO(logger_, "BinaryFilter: Resetting the filter to default state");
-  changeState(default_state_);
+  // Publishing new BinaryState in reset
+  std::unique_ptr<std_msgs::msg::Bool> msg =
+    std::make_unique<std_msgs::msg::Bool>();
+  msg->data = binary_state_;
+  binary_state_pub_->publish(std::move(msg));
 
   filter_info_sub_.reset();
   mask_sub_.reset();

nav2_costmap_2d/test/unit/binary_filter_test.cpp

@@ -235,6 +235,7 @@ class TestNode : public ::testing::Test
   std::shared_ptr<nav2_costmap_2d::Costmap2D> master_grid_;
 
   bool default_state_;
+  bool binary_state_;
 
 private:
   void waitSome(const std::chrono::nanoseconds & duration);
@@ -674,12 +675,13 @@ void TestNode::testResetFilter()
   binary_filter_->process(*master_grid_, min_i, min_j, max_i, max_j, pose);
   binary_state = waitBinaryState();
   verifyBinaryState(getSign(pose.x, pose.y, base, multiplier, flip_threshold), binary_state);
+  binary_state_ = binary_state->data;
 
-  // Reset binary filter and check its state was resetted to default
+  // Reset binary filter and check its state was resetted to binary_state_
   binary_filter_->resetFilter();
   binary_state = waitBinaryState();
   ASSERT_TRUE(binary_state != nullptr);
-  ASSERT_EQ(binary_state->data, default_state_);
+  ASSERT_EQ(binary_state->data, binary_state_);
 }
 
 void TestNode::resetMaps()