Costmap Filter enabling service

nav2_costmap_2d/CMakeLists.txt

@@ -17,6 +17,7 @@ find_package(rclcpp_lifecycle REQUIRED)
 find_package(rmw REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(std_msgs REQUIRED)
+find_package(std_srvs REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
 find_package(tf2 REQUIRED)
 find_package(tf2_ros REQUIRED)
@@ -66,6 +67,7 @@ set(dependencies
  rclcpp_lifecycle
  sensor_msgs
  std_msgs
+ std_srvs
  tf2
  tf2_geometry_msgs
  tf2_ros

nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp

@@ -42,6 +42,7 @@
 #include <mutex>
 
 #include "geometry_msgs/msg/pose2_d.hpp"
+#include "std_srvs/srv/set_bool.hpp"
 #include "nav2_costmap_2d/layer.hpp"
 
 namespace nav2_costmap_2d
@@ -153,6 +154,17 @@ class CostmapFilter : public Layer
  virtual void resetFilter() = 0;
 
 protected:
+ /**
+ * @brief Costmap filter enabling/disabling callback
+ * @param request_header Service request header
+ * @param request Service request
+ * @param response Service response
+ */
+ void enableCallback(
+ const std::shared_ptr<rmw_request_id_t> request_header,
+ const std::shared_ptr<std_srvs::srv::SetBool::Request> request,
+ std::shared_ptr<std_srvs::srv::SetBool::Response> response);
+
  /**
  * @brief: Name of costmap filter info topic
  */
@@ -168,6 +180,11 @@ class CostmapFilter : public Layer
  */
  tf2::Duration transform_tolerance_;
 
+ /**
+ * @brief: A service to enable/disable costmap filter
+ */
+ rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr enable_service_;
+
 private:
  /**
  * @brief: Latest robot position

nav2_costmap_2d/package.xml

@@ -32,6 +32,7 @@
  <depend>rclcpp_lifecycle</depend>
  <depend>sensor_msgs</depend>
  <depend>std_msgs</depend>
+ <depend>std_srvs</depend>
  <depend>tf2</depend>
  <depend>tf2_geometry_msgs</depend>
  <depend>tf2_ros</depend>

nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp

@@ -63,15 +63,25 @@ void CostmapFilter::onInitialize()
  try {
  // Declare common for all costmap filters parameters
  declareParameter("enabled", rclcpp::ParameterValue(true));
+ declareParameter("enable_service", rclcpp::ParameterValue("enable"));
  declareParameter("filter_info_topic", rclcpp::PARAMETER_STRING);
  declareParameter("transform_tolerance", rclcpp::ParameterValue(0.1));
 
  // Get parameters
  node->get_parameter(name_ + "." + "enabled", enabled_);
  filter_info_topic_ = node->get_parameter(name_ + "." + "filter_info_topic").as_string();
+ std::string enable_service_name;
+ node->get_parameter(name_ + "." + "enable_service", enable_service_name);
  double transform_tolerance;
  node->get_parameter(name_ + "." + "transform_tolerance", transform_tolerance);
  transform_tolerance_ = tf2::durationFromSec(transform_tolerance);
+
+ // Costmap Filter enabling service
+ enable_service_ = node->create_service<std_srvs::srv::SetBool>(
+ name_ + "/" + enable_service_name,
+ std::bind(
+ &CostmapFilter::enableCallback, this,
+ std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
  } catch (const std::exception & ex) {
  RCLCPP_ERROR(logger_, "Parameter problem: %s", ex.what());
  throw ex;
@@ -120,4 +130,18 @@ void CostmapFilter::updateCosts(
  current_ = true;
 }
 
+void CostmapFilter::enableCallback(
+ const std::shared_ptr<rmw_request_id_t>/*request_header*/,
+ const std::shared_ptr<std_srvs::srv::SetBool::Request> request,
+ std::shared_ptr<std_srvs::srv::SetBool::Response> response)
+{
+ enabled_ = request->data;
+ response->success = true;
+ if (enabled_) {
+ response->message = "Enabled";
+ } else {
+ response->message = "Disabled";
+ }
+}
+
 } // namespace nav2_costmap_2d

nav2_costmap_2d/test/unit/CMakeLists.txt

@@ -34,3 +34,8 @@ ament_add_gtest(copy_window_test copy_window_test.cpp)
 target_link_libraries(copy_window_test
  nav2_costmap_2d_core
 )
+
+ament_add_gtest(costmap_filter_service_test costmap_filter_service_test.cpp)
+target_link_libraries(costmap_filter_service_test
+ nav2_costmap_2d_core
+)

nav2_costmap_2d/test/unit/costmap_filter_service_test.cpp

@@ -0,0 +1,155 @@
+// Copyright (c) 2022 Samsung R&D Institute Russia
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License. Reserved.
+
+#include <gtest/gtest.h>
+
+#include <string>
+#include <memory>
+
+#include "rclcpp/rclcpp.hpp"
+#include "nav2_util/lifecycle_node.hpp"
+
+#include "nav2_costmap_2d/costmap_filters/costmap_filter.hpp"
+#include "std_srvs/srv/set_bool.hpp"
+
+static const char FILTER_NAME[]{"costmap_filter"};
+static const char ENABLE_SERVICE_NAME[]{"enable_service"};
+
+class CostmapFilterWrapper : public nav2_costmap_2d::CostmapFilter
+{
+public:
+ // Dummy implementations of virtual methods
+ void initializeFilter(
+ const std::string &) {}
+
+ void process(
+ nav2_costmap_2d::Costmap2D &,
+ int, int, int, int,
+ const geometry_msgs::msg::Pose2D &) {}
+
+ void resetFilter() {}
+
+ // Actual testing methods
+ void setName(const std::string & name)
+ {
+ name_ = name;
+ }
+
+ void setNode(const nav2_util::LifecycleNode::WeakPtr & node)
+ {
+ node_ = node;
+ }
+
+ bool getEnabled()
+ {
+ return enabled_;
+ }
+};
+
+class TestNode : public ::testing::Test
+{
+public:
+ TestNode()
+ {
+ // Create new LifecycleNode
+ node_ = std::make_shared<nav2_util::LifecycleNode>("test_node");
+
+ // Create new CostmapFilter
+ costmap_filter_ = std::make_shared<CostmapFilterWrapper>();
+ costmap_filter_->setNode(node_);
+ costmap_filter_->setName(FILTER_NAME);
+
+ // Set CostmapFilter ROS-parameters
+ node_->declare_parameter(
+ std::string(FILTER_NAME) + ".enable_service", rclcpp::ParameterValue(ENABLE_SERVICE_NAME));
+ node_->set_parameter(
+ rclcpp::Parameter(std::string(FILTER_NAME) + ".enable_service", ENABLE_SERVICE_NAME));
+ node_->declare_parameter(
+ std::string(FILTER_NAME) + ".filter_info_topic", rclcpp::ParameterValue("filter_info"));
+ node_->set_parameter(
+ rclcpp::Parameter(std::string(FILTER_NAME) + ".filter_info_topic", "filter_info"));
+ }
+
+ ~TestNode()
+ {
+ costmap_filter_.reset();
+ node_.reset();
+ }
+
+ template<class T>
+ typename T::Response::SharedPtr send_request(
+ nav2_util::LifecycleNode::SharedPtr node,
+ typename rclcpp::Client<T>::SharedPtr client,
+ typename T::Request::SharedPtr request)
+ {
+ auto result = client->async_send_request(request);
+
+ // Wait for the result
+ if (rclcpp::spin_until_future_complete(node, result) == rclcpp::FutureReturnCode::SUCCESS) {
+ return result.get();
+ } else {
+ return nullptr;
+ }
+ }
+
+protected:
+ nav2_util::LifecycleNode::SharedPtr node_;
+ std::shared_ptr<CostmapFilterWrapper> costmap_filter_;
+};
+
+TEST_F(TestNode, testEnableService)
+{
+ costmap_filter_->onInitialize();
+
+ RCLCPP_INFO(node_->get_logger(), "Testing enabling service");
+ auto req = std::make_shared<std_srvs::srv::SetBool::Request>();
+ auto client = node_->create_client<std_srvs::srv::SetBool>(
+ std::string(FILTER_NAME) + "/" + std::string(ENABLE_SERVICE_NAME));
+
+ RCLCPP_INFO(node_->get_logger(), "Waiting for enabling service");
+ ASSERT_TRUE(client->wait_for_service());
+
+ // Set costmap filter enabled
+ req->data = true;
+ auto resp = send_request<std_srvs::srv::SetBool>(node_, client, req);
+
+ ASSERT_NE(resp, nullptr);
+ ASSERT_TRUE(resp->success);
+ ASSERT_EQ(resp->message, "Enabled");
+ ASSERT_TRUE(costmap_filter_->getEnabled());
+
+ // Set costmap filter disabled
+ req->data = false;
+ resp = send_request<std_srvs::srv::SetBool>(node_, client, req);
+
+ ASSERT_NE(resp, nullptr);
+ ASSERT_TRUE(resp->success);
+ ASSERT_EQ(resp->message, "Disabled");
+ ASSERT_FALSE(costmap_filter_->getEnabled());
+}
+
+int main(int argc, char ** argv)
+{
+ // Initialize the system
+ testing::InitGoogleTest(&argc, argv);
+ rclcpp::init(argc, argv);
+
+ // Actual testing
+ bool test_result = RUN_ALL_TESTS();
+
+ // Shutdown
+ rclcpp::shutdown();
+
+ return test_result;
+}