From 496ce37c1a6506bd542017031ba2226575220527 Mon Sep 17 00:00:00 2001 From: enricosutera Date: Sun, 19 Nov 2023 20:07:46 +0100 Subject: [PATCH] binary filter check services availability in init --- .../plugins/costmap_filters/binary_filter.cpp | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/nav2_costmap_2d/plugins/costmap_filters/binary_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/binary_filter.cpp index a262cad59fe..9c36fc9f447 100644 --- a/nav2_costmap_2d/plugins/costmap_filters/binary_filter.cpp +++ b/nav2_costmap_2d/plugins/costmap_filters/binary_filter.cpp @@ -116,6 +116,19 @@ void BinaryFilter::initializeFilter( auto change_parameters_client = node->create_client( "/" + param.node_name + "/set_parameters"); change_parameters_clients_.push_back(change_parameters_client); + if (!change_parameters_client->wait_for_service( + std::chrono::milliseconds(change_parameter_timeout_))) + { + RCLCPP_ERROR( + logger_, "BinaryFilter: service %s not available. Skipping ...", + change_parameters_client->get_service_name()); + // TODO (@enricosutera) replace this once we figure out what to do + // throw std::runtime_error("BinaryFilter: Service not available!"); + } else { + RCLCPP_INFO( + logger_, "BinaryFilter: service %s available.", + change_parameters_client->get_service_name()); + } } // Get global frame required for binary state publisher