From 73afaf3f36f2cbdf4a20c5e0c1e9dfbdd512b8cc Mon Sep 17 00:00:00 2001
From: Alexey Merzlyakov <60094858+AlexeyMerzlyakov@users.noreply.github.com>
Date: Fri, 6 Nov 2020 21:16:39 +0300
Subject: [PATCH] =?UTF-8?q?Support=20in=20keepout=20filter=20for=20mask=20?=
=?UTF-8?q?and=20costmap=20published=20in=20different=E2=80=A6=20(#2054)?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
* Support in keepout filter for mask and costmap published in different frames
This fixes incorrect keepouts position issue when filter mask and current
costmap layer are published in different frames. This might appear
(but not restricted only to) when keepout filter is enabled for local costmap
with rolling window.
* Add transform initialization
* Enhance transform failure message
---
doc/parameters/param_list.md | 1 +
.../costmap_filters/costmap_filter.hpp | 5 +
.../costmap_filters/keepout_filter.hpp | 3 +
.../costmap_filters/costmap_filter.cpp | 5 +
.../costmap_filters/keepout_filter.cpp | 161 ++++++++++++------
.../test/unit/keepout_filter_test.cpp | 118 ++++++++++---
6 files changed, 219 insertions(+), 74 deletions(-)
diff --git a/doc/parameters/param_list.md b/doc/parameters/param_list.md
index 6682b203..505cb9cb 100644
--- a/doc/parameters/param_list.md
+++ b/doc/parameters/param_list.md
@@ -176,6 +176,7 @@ When `plugins` parameter is not overridden, the following default plugins are lo
| ----------| --------| ------------|
| `<filter name>`.enabled | true | Whether it is enabled |
| `<filter name>`.filter_info_topic | N/A | Name of the CostmapFilterInfo topic having filter-related information |
+| `<filter name>`.transform_tolerance | 0.0 | TF tolerance |
# controller_server
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp
index 16918ed1..af7a9948 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp
@@ -124,6 +124,11 @@ protected:
*/
std::string mask_topic_;
+ /**
+ * @brief: mask_frame_->global_frame_ transform tolerance
+ */
+ tf2::Duration transform_tolerance_;
+
private:
/**
* @brief: Latest robot position
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/keepout_filter.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/keepout_filter.hpp
index 07cc73ea..01b40a7b 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/keepout_filter.hpp
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/keepout_filter.hpp
@@ -75,6 +75,9 @@ private:
rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr mask_sub_;
std::unique_ptr<Costmap2D> mask_costmap_;
+
+ std::string mask_frame_; // Frame where mask located in
+ std::string global_frame_; // Frame of currnet layer (master_grid)
};
} // namespace nav2_costmap_2d
diff --git a/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp
index dec1437f..7d0d2971 100644
--- a/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp
+++ b/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp
@@ -63,6 +63,7 @@ void CostmapFilter::onInitialize()
// Declare parameters
declareParameter("enabled", rclcpp::ParameterValue(true));
declareParameter("filter_info_topic");
+ declareParameter("transform_tolerance", rclcpp::ParameterValue(0.0));
// Get parameters
node->get_parameter(name_ + "." + "enabled", enabled_);
@@ -72,6 +73,10 @@ void CostmapFilter::onInitialize()
RCLCPP_ERROR(node->get_logger(), "filter_info_topic parameter is not set");
throw ex;
}
+ double transform_tolerance;
+ // This is global for whole costmap parameter
+ node->get_parameter(name_ + "." + "transform_tolerance", transform_tolerance);
+ transform_tolerance_ = tf2::durationFromSec(transform_tolerance);
}
void CostmapFilter::activate()
diff --git a/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp
index b3a23ebc..19cafb45 100644
--- a/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp
+++ b/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp
@@ -37,11 +37,16 @@
#include "nav2_costmap_2d/costmap_filters/keepout_filter.hpp"
+#include "tf2/convert.h"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
+#include "tf2_ros/transform_broadcaster.h"
+
namespace nav2_costmap_2d
{
KeepoutFilter::KeepoutFilter()
-: filter_info_sub_(nullptr), mask_sub_(nullptr), mask_costmap_(nullptr)
+: filter_info_sub_(nullptr), mask_sub_(nullptr), mask_costmap_(nullptr),
+ mask_frame_(""), global_frame_("")
{
}
@@ -64,6 +69,8 @@ void KeepoutFilter::initializeFilter(
filter_info_sub_ = node->create_subscription<nav2_msgs::msg::CostmapFilterInfo>(
filter_info_topic, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(),
std::bind(&KeepoutFilter::filterInfoCallback, this, std::placeholders::_1));
+
+ global_frame_ = layered_costmap_->getGlobalFrameID();
}
void KeepoutFilter::filterInfoCallback(
@@ -126,6 +133,7 @@ void KeepoutFilter::maskCallback(
// Making a new mask_costmap_
mask_costmap_ = std::make_unique<Costmap2D>(*msg);
+ mask_frame_ = msg->header.frame_id;
}
void KeepoutFilter::process(
@@ -148,57 +156,85 @@ void KeepoutFilter::process(
return;
}
- double wx, wy; // world coordinates
-
- // Optimization: iterate only in overlapped
- // (min_i, min_j)..(max_i, max_j) & mask_costmap_ area.
- //
- // mask_costmap_
- // *----------------------------*
- // | |
- // | |
- // | (2) |
- // *-----+-------* |
- // | |///////|<- overlapped area |
- // | |///////| to iterate in |
- // | *-------+--------------------*
- // | (1) |
- // | |
- // *-------------*
- // master_grid (min_i, min_j)..(max_i, max_j) window
- //
- // ToDo: after costmap rotation will be added, this should be re-worked.
-
- // Calculating bounds corresponding to bottom-left overlapping (1) corner
- int mg_min_x, mg_min_y; // masger_grid indexes of bottom-left (1) corner
- // mask_costmap_ -> master_grid intexes conversion
- const double half_cell_size = 0.5 * mask_costmap_->getResolution();
- wx = mask_costmap_->getOriginX() + half_cell_size;
- wy = mask_costmap_->getOriginY() + half_cell_size;
- master_grid.worldToMapNoBounds(wx, wy, mg_min_x, mg_min_y);
- // Calculation of (1) corner bounds
- if (mg_min_x >= max_i || mg_min_y >= max_j) {
- // There is no overlapping. Do nothing.
- return;
- }
- mg_min_x = std::max(min_i, mg_min_x);
- mg_min_y = std::max(min_j, mg_min_y);
-
- // Calculating bounds corresponding to top-right window (2) corner
- int mg_max_x, mg_max_y; // masger_grid indexes of top-right (2) corner
- // mask_costmap_ -> master_grid intexes conversion
- wx = mask_costmap_->getOriginX() +
- mask_costmap_->getSizeInCellsX() * mask_costmap_->getResolution() + half_cell_size;
- wy = mask_costmap_->getOriginY() +
- mask_costmap_->getSizeInCellsY() * mask_costmap_->getResolution() + half_cell_size;
- master_grid.worldToMapNoBounds(wx, wy, mg_max_x, mg_max_y);
- // Calculation of (2) corner bounds
- if (mg_max_x <= min_i || mg_max_y <= min_j) {
- // There is no overlapping. Do nothing.
- return;
+ tf2::Transform tf2_transform;
+ tf2_transform.setIdentity(); // initialize by identical transform
+ int mg_min_x, mg_min_y; // masger_grid indexes of bottom-left window corner
+ int mg_max_x, mg_max_y; // masger_grid indexes of top-right window corner
+
+ if (mask_frame_ != global_frame_) {
+ // Filter mask and current layer are in different frames:
+ // prepare frame transformation if mask_frame_ != global_frame_
+ geometry_msgs::msg::TransformStamped transform;
+ try {
+ transform = tf_->lookupTransform(
+ mask_frame_, global_frame_, tf2::TimePointZero,
+ transform_tolerance_);
+ } catch (tf2::TransformException & ex) {
+ RCLCPP_ERROR(
+ node->get_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;
+ }
+ tf2::fromMsg(transform.transform, tf2_transform);
+
+ mg_min_x = min_i;
+ mg_min_y = min_j;
+ mg_max_x = max_i;
+ mg_max_y = max_j;
+ } else {
+ // Filter mask and current layer are in the same frame:
+ // apply the following optimization - iterate only in overlapped
+ // (min_i, min_j)..(max_i, max_j) & mask_costmap_ area.
+ //
+ // mask_costmap_
+ // *----------------------------*
+ // | |
+ // | |
+ // | (2) |
+ // *-----+-------* |
+ // | |///////|<- overlapped area |
+ // | |///////| to iterate in |
+ // | *-------+--------------------*
+ // | (1) |
+ // | |
+ // *-------------*
+ // master_grid (min_i, min_j)..(max_i, max_j) window
+ //
+ // ToDo: after costmap rotation will be added, this should be re-worked.
+
+ double wx, wy; // world coordinates
+
+ // Calculating bounds corresponding to bottom-left overlapping (1) corner
+ // mask_costmap_ -> master_grid intexes conversion
+ const double half_cell_size = 0.5 * mask_costmap_->getResolution();
+ wx = mask_costmap_->getOriginX() + half_cell_size;
+ wy = mask_costmap_->getOriginY() + half_cell_size;
+ master_grid.worldToMapNoBounds(wx, wy, mg_min_x, mg_min_y);
+ // Calculation of (1) corner bounds
+ if (mg_min_x >= max_i || mg_min_y >= max_j) {
+ // There is no overlapping. Do nothing.
+ return;
+ }
+ mg_min_x = std::max(min_i, mg_min_x);
+ mg_min_y = std::max(min_j, mg_min_y);
+
+ // Calculating bounds corresponding to top-right window (2) corner
+ // mask_costmap_ -> master_grid intexes conversion
+ wx = mask_costmap_->getOriginX() +
+ mask_costmap_->getSizeInCellsX() * mask_costmap_->getResolution() + half_cell_size;
+ wy = mask_costmap_->getOriginY() +
+ mask_costmap_->getSizeInCellsY() * mask_costmap_->getResolution() + half_cell_size;
+ master_grid.worldToMapNoBounds(wx, wy, mg_max_x, mg_max_y);
+ // Calculation of (2) corner bounds
+ if (mg_max_x <= min_i || mg_max_y <= min_j) {
+ // There is no overlapping. Do nothing.
+ return;
+ }
+ mg_max_x = std::min(max_i, mg_max_x);
+ mg_max_y = std::min(max_j, mg_max_y);
}
- mg_max_x = std::min(max_i, mg_max_x);
- mg_max_y = std::min(max_j, mg_max_y);
// unsigned<-signed conversions.
unsigned const int mg_min_x_u = static_cast<unsigned int>(mg_min_x);
@@ -208,19 +244,34 @@ void KeepoutFilter::process(
unsigned int i, j; // master_grid iterators
unsigned int index; // corresponding index of master_grid
+ double gl_wx, gl_wy; // world coordinates in a global_frame_
+ double msk_wx, msk_wy; // world coordinates in a mask_frame_
unsigned int mx, my; // mask_costmap_ coordinates
unsigned char data, old_data; // master_grid element data
// Main master_grid updating loop
- // Iterate in overlapped window by master_grid indexes
+ // Iterate in costmap window by master_grid indexes
unsigned char * master_array = master_grid.getCharMap();
for (i = mg_min_x_u; i < mg_max_x_u; i++) {
for (j = mg_min_y_u; j < mg_max_y_u; j++) {
index = master_grid.getIndex(i, j);
old_data = master_array[index];
- // Calculating corresponding to (i, j) point at mask_costmap_
- master_grid.mapToWorld(i, j, wx, wy);
- if (mask_costmap_->worldToMap(wx, wy, mx, my)) {
+ // Calculating corresponding to (i, j) point at mask_costmap_:
+ // Get world coordinates in global_frame_
+ master_grid.mapToWorld(i, j, gl_wx, gl_wy);
+ if (mask_frame_ != global_frame_) {
+ // Transform (i, j) point from global_frame_ to mask_frame_
+ tf2::Vector3 point(gl_wx, gl_wy, 0);
+ point = tf2_transform * point;
+ msk_wx = point.x();
+ msk_wy = point.y();
+ } else {
+ // In this case master_grid and filter-mask are in the same frame
+ msk_wx = gl_wx;
+ msk_wy = gl_wy;
+ }
+ // Get mask coordinates corresponding to (i, j) point at mask_costmap_
+ if (mask_costmap_->worldToMap(msk_wx, msk_wy, mx, my)) {
data = mask_costmap_->getCost(mx, my);
// Update if mask_ data is valid and greater than existing master_grid's one
if (data == NO_INFORMATION) {
diff --git a/nav2_costmap_2d/test/unit/keepout_filter_test.cpp b/nav2_costmap_2d/test/unit/keepout_filter_test.cpp
index 43ac60b7..7d2da758 100644
--- a/nav2_costmap_2d/test/unit/keepout_filter_test.cpp
+++ b/nav2_costmap_2d/test/unit/keepout_filter_test.cpp
@@ -21,6 +21,10 @@
#include <functional>
#include "rclcpp/rclcpp.hpp"
+#include "nav2_util/lifecycle_node.hpp"
+#include "tf2_ros/buffer.h"
+#include "tf2_ros/transform_listener.h"
+#include "tf2_ros/transform_broadcaster.h"
#include "nav2_util/occ_grid_values.hpp"
#include "nav2_costmap_2d/cost_values.hpp"
#include "nav_msgs/msg/occupancy_grid.hpp"
@@ -97,14 +101,16 @@ public:
~TestNode() {}
protected:
- void createMaps(unsigned char master_value, int8_t mask_value);
+ void createMaps(unsigned char master_value, int8_t mask_value, const std::string & mask_frame);
void publishMaps();
void rePublishInfo(double base, double multiplier);
void rePublishMask();
void waitSome(const std::chrono::nanoseconds & duration);
- void createKeepoutFilter();
+ void createKeepoutFilter(const std::string & global_frame);
+ void createTFBroadcaster(const std::string & mask_frame, const std::string & global_frame);
void verifyMasterGrid(unsigned char free_value, unsigned char keepout_value);
void testStandardScenario(unsigned char free_value, unsigned char keepout_value);
+ void testFramesScenario(unsigned char free_value, unsigned char keepout_value);
void reset();
std::shared_ptr<nav2_costmap_2d::KeepoutFilter> keepout_filter_;
@@ -115,13 +121,19 @@ protected:
private:
nav2_util::LifecycleNode::SharedPtr node_;
+ std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
+ std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
+ std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
+ std::unique_ptr<geometry_msgs::msg::TransformStamped> transform_;
+
std::shared_ptr<nav_msgs::msg::OccupancyGrid> mask_;
std::shared_ptr<InfoPublisher> info_publisher_;
std::shared_ptr<MaskPublisher> mask_publisher_;
};
-void TestNode::createMaps(unsigned char master_value, int8_t mask_value)
+void TestNode::createMaps(
+ unsigned char master_value, int8_t mask_value, const std::string & mask_frame)
{
// Make map and mask put as follows:
//
@@ -149,6 +161,7 @@ void TestNode::createMaps(unsigned char master_value, int8_t mask_value)
height = 3;
mask_ = std::make_shared<nav_msgs::msg::OccupancyGrid>();
mask_->info.resolution = resolution;
+ mask_->header.frame_id = mask_frame;
mask_->info.width = width;
mask_->info.height = height;
mask_->info.origin.position.x = 3.0;
@@ -193,19 +206,26 @@ void TestNode::waitSome(const std::chrono::nanoseconds & duration)
}
}
-void TestNode::createKeepoutFilter()
+void TestNode::createKeepoutFilter(const std::string & global_frame)
{
node_ = std::make_shared<nav2_util::LifecycleNode>("test_node");
- tf2_ros::Buffer tf(node_->get_clock());
- nav2_costmap_2d::LayeredCostmap layers("frame", false, false);
+ tf_buffer_ = std::make_shared<tf2_ros::Buffer>(node_->get_clock());
+ tf_buffer_->setUsingDedicatedThread(true); // One-thread broadcasting-listening model
+ tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
+
+ nav2_costmap_2d::LayeredCostmap layers(global_frame, false, false);
+ node_->declare_parameter(
+ FILTER_NAME + ".transform_tolerance", rclcpp::ParameterValue(0.5));
+ node_->set_parameter(
+ rclcpp::Parameter(FILTER_NAME + ".transform_tolerance", 0.5));
node_->declare_parameter(
FILTER_NAME + ".filter_info_topic", rclcpp::ParameterValue(INFO_TOPIC));
node_->set_parameter(
rclcpp::Parameter(FILTER_NAME + ".filter_info_topic", INFO_TOPIC));
keepout_filter_ = std::make_shared<nav2_costmap_2d::KeepoutFilter>();
- keepout_filter_->initialize(&layers, FILTER_NAME, &tf, node_, nullptr, nullptr);
+ keepout_filter_->initialize(&layers, FILTER_NAME, tf_buffer_.get(), node_, nullptr, nullptr);
keepout_filter_->initializeFilter(INFO_TOPIC);
// Wait until mask will be received by KeepoutFilter
@@ -215,6 +235,29 @@ void TestNode::createKeepoutFilter()
}
}
+void TestNode::createTFBroadcaster(const std::string & mask_frame, const std::string & global_frame)
+{
+ tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(node_);
+
+ transform_ = std::make_unique<geometry_msgs::msg::TransformStamped>();
+ transform_->header.frame_id = mask_frame;
+ transform_->child_frame_id = global_frame;
+
+ transform_->header.stamp = node_->now();
+ transform_->transform.translation.x = 1.0;
+ transform_->transform.translation.y = 1.0;
+ transform_->transform.translation.z = 0.0;
+ transform_->transform.rotation.x = 0.0;
+ transform_->transform.rotation.y = 0.0;
+ transform_->transform.rotation.z = 0.0;
+ transform_->transform.rotation.w = 1.0;
+
+ tf_broadcaster_->sendTransform(*transform_);
+
+ // Allow tf_buffer_ to be filled by listener
+ waitSome(200ms);
+}
+
void TestNode::verifyMasterGrid(unsigned char free_value, unsigned char keepout_value)
{
unsigned int x, y;
@@ -265,6 +308,23 @@ void TestNode::testStandardScenario(unsigned char free_value, unsigned char keep
verifyMasterGrid(free_value, keepout_value);
}
+void TestNode::testFramesScenario(unsigned char free_value, unsigned char keepout_value)
+{
+ geometry_msgs::msg::Pose2D pose;
+ // Intersection window: added all 9 points because of map->odom frame shift
+ keepout_filter_->process(*master_grid_, 2, 2, 5, 5, pose);
+ keepout_points_.push_back(Point{2, 2});
+ keepout_points_.push_back(Point{2, 3});
+ keepout_points_.push_back(Point{2, 4});
+ keepout_points_.push_back(Point{3, 2});
+ keepout_points_.push_back(Point{3, 3});
+ keepout_points_.push_back(Point{3, 4});
+ keepout_points_.push_back(Point{4, 2});
+ keepout_points_.push_back(Point{4, 3});
+ keepout_points_.push_back(Point{4, 4});
+ verifyMasterGrid(free_value, keepout_value);
+}
+
void TestNode::reset()
{
mask_.reset();
@@ -273,15 +333,18 @@ void TestNode::reset()
mask_publisher_.reset();
keepout_filter_.reset();
node_.reset();
+ tf_listener_.reset();
+ tf_broadcaster_.reset();
+ tf_buffer_.reset();
keepout_points_.clear();
}
TEST_F(TestNode, testFreeMasterLethalKeepout)
{
// Initilize test system
- createMaps(nav2_costmap_2d::FREE_SPACE, nav2_util::OCC_GRID_OCCUPIED);
+ createMaps(nav2_costmap_2d::FREE_SPACE, nav2_util::OCC_GRID_OCCUPIED, "map");
publishMaps();
- createKeepoutFilter();
+ createKeepoutFilter("map");
// Test KeepoutFilter
testStandardScenario(nav2_costmap_2d::FREE_SPACE, nav2_costmap_2d::LETHAL_OBSTACLE);
@@ -296,9 +359,10 @@ TEST_F(TestNode, testUnknownMasterNonLethalKeepout)
// Initilize test system
createMaps(
nav2_costmap_2d::NO_INFORMATION,
- (nav2_util::OCC_GRID_OCCUPIED - nav2_util::OCC_GRID_FREE) / 2);
+ (nav2_util::OCC_GRID_OCCUPIED - nav2_util::OCC_GRID_FREE) / 2,
+ "map");
publishMaps();
- createKeepoutFilter();
+ createKeepoutFilter("map");
// Test KeepoutFilter
testStandardScenario(
@@ -313,9 +377,9 @@ TEST_F(TestNode, testUnknownMasterNonLethalKeepout)
TEST_F(TestNode, testFreeKeepout)
{
// Initilize test system
- createMaps(nav2_costmap_2d::FREE_SPACE, nav2_util::OCC_GRID_FREE);
+ createMaps(nav2_costmap_2d::FREE_SPACE, nav2_util::OCC_GRID_FREE, "map");
publishMaps();
- createKeepoutFilter();
+ createKeepoutFilter("map");
// Test KeepoutFilter
geometry_msgs::msg::Pose2D pose;
@@ -332,9 +396,9 @@ TEST_F(TestNode, testFreeKeepout)
TEST_F(TestNode, testUnknownKeepout)
{
// Initilize test system
- createMaps(nav2_costmap_2d::FREE_SPACE, nav2_util::OCC_GRID_UNKNOWN);
+ createMaps(nav2_costmap_2d::FREE_SPACE, nav2_util::OCC_GRID_UNKNOWN, "map");
publishMaps();
- createKeepoutFilter();
+ createKeepoutFilter("map");
// Test KeepoutFilter
geometry_msgs::msg::Pose2D pose;
@@ -351,9 +415,9 @@ TEST_F(TestNode, testUnknownKeepout)
TEST_F(TestNode, testInfoRePublish)
{
// Initilize test system
- createMaps(nav2_costmap_2d::FREE_SPACE, nav2_util::OCC_GRID_OCCUPIED);
+ createMaps(nav2_costmap_2d::FREE_SPACE, nav2_util::OCC_GRID_OCCUPIED, "map");
publishMaps();
- createKeepoutFilter();
+ createKeepoutFilter("map");
// Re-publish filter info (with incorrect base and multiplier)
// and test that everything is working after
@@ -370,9 +434,9 @@ TEST_F(TestNode, testInfoRePublish)
TEST_F(TestNode, testMaskRePublish)
{
// Initilize test system
- createMaps(nav2_costmap_2d::FREE_SPACE, nav2_util::OCC_GRID_OCCUPIED);
+ createMaps(nav2_costmap_2d::FREE_SPACE, nav2_util::OCC_GRID_OCCUPIED, "map");
publishMaps();
- createKeepoutFilter();
+ createKeepoutFilter("map");
// Re-publish filter mask and test that everything is working after
rePublishMask();
@@ -385,6 +449,22 @@ TEST_F(TestNode, testMaskRePublish)
reset();
}
+TEST_F(TestNode, testDifferentFrames)
+{
+ // Initilize test system
+ createMaps(nav2_costmap_2d::FREE_SPACE, nav2_util::OCC_GRID_OCCUPIED, "map");
+ publishMaps();
+ createKeepoutFilter("odom");
+ createTFBroadcaster("map", "odom");
+
+ // Test KeepoutFilter
+ testFramesScenario(nav2_costmap_2d::FREE_SPACE, nav2_costmap_2d::LETHAL_OBSTACLE);
+
+ // Clean-up
+ keepout_filter_->resetFilter();
+ reset();
+}
+
int main(int argc, char ** argv)
{
// Initialize the system
--
GitLab