From 15a0c639fb6d51a92a6f9742cee020acc59ef22b Mon Sep 17 00:00:00 2001 From: Alexey Merzlyakov Date: Tue, 24 Nov 2020 13:28:33 +0300 Subject: [PATCH] Keepout Filter cleanup --- .../costmap_filters/keepout_filter.cpp | 56 ++++++++++--------- .../test/unit/keepout_filter_test.cpp | 4 +- nav2_system_tests/maps/keepout_mask.yaml | 2 - 3 files changed, 32 insertions(+), 30 deletions(-) diff --git a/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp index 19cafb45e37..501523488f8 100644 --- a/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp +++ b/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( - 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,21 +83,25 @@ 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); } @@ -105,9 +109,9 @@ void KeepoutFilter::filterInfoCallback( // 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( 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 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 guard(*getMutex()); + if (mask_costmap_) { return true; } diff --git a/nav2_costmap_2d/test/unit/keepout_filter_test.cpp b/nav2_costmap_2d/test/unit/keepout_filter_test.cpp index 7d2da7584d4..cff800738bd 100644 --- a/nav2_costmap_2d/test/unit/keepout_filter_test.cpp +++ b/nav2_costmap_2d/test/unit/keepout_filter_test.cpp @@ -51,8 +51,8 @@ class InfoPublisher : public rclcpp::Node std::make_unique(); msg->type = 0; msg->filter_mask_topic = MASK_TOPIC; - msg->base = base; - msg->multiplier = multiplier; + msg->base = static_cast(base); + msg->multiplier = static_cast(multiplier); publisher_->publish(std::move(msg)); } diff --git a/nav2_system_tests/maps/keepout_mask.yaml b/nav2_system_tests/maps/keepout_mask.yaml index d6a466e04ff..aea4f80b8e5 100644 --- a/nav2_system_tests/maps/keepout_mask.yaml +++ b/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 -