From 2e9dd69fe9cf672cbfcabfd182e1de97430381e9 Mon Sep 17 00:00:00 2001 From: bpwilcox Date: Thu, 6 Jun 2019 09:58:18 -0700 Subject: [PATCH] Re-enable costmap tests (footprint and inflation tests) (#805) * re-enabling obstacle, inflation, and footprint tests * remove static tests, redesign footprint_tests without Costmap2DROS * passing linters and uncrustify * replace for_each with for loop --- nav2_costmap_2d/package.xml | 1 + nav2_costmap_2d/src/costmap_2d_ros.cpp | 2 +- nav2_costmap_2d/src/layer.cpp | 6 +- .../test/integration/CMakeLists.txt | 97 ++---- .../test/integration/costmap_tests_launch.py | 2 +- .../test/integration/footprint_tests.cpp | 74 ++-- .../test/integration/inflation_tests.cpp | 83 +++-- .../test/integration/obstacle_tests.cpp | 13 +- .../test/integration/static_tests.cpp | 328 ------------------ 9 files changed, 149 insertions(+), 457 deletions(-) delete mode 100644 nav2_costmap_2d/test/integration/static_tests.cpp diff --git a/nav2_costmap_2d/package.xml b/nav2_costmap_2d/package.xml index ecaa7555b0..7a28cdb222 100644 --- a/nav2_costmap_2d/package.xml +++ b/nav2_costmap_2d/package.xml @@ -46,6 +46,7 @@ ament_cmake_pytest launch launch_testing + nav2_lifecycle_manager ament_cmake diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index 2fc49dc6b3..3c4e2d60bf 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -348,7 +348,7 @@ Costmap2DROS::mapUpdateLoop(double frequency) RCLCPP_DEBUG(get_logger(), "Entering loop"); - rclcpp::Rate r(frequency); // 200ms by default + rclcpp::Rate r(frequency); // 200ms by default while (rclcpp::ok() && !map_update_thread_shutdown_) { nav2_util::ExecutionTimer timer; diff --git a/nav2_costmap_2d/src/layer.cpp b/nav2_costmap_2d/src/layer.cpp index 4ef3af098a..3d170f537a 100644 --- a/nav2_costmap_2d/src/layer.cpp +++ b/nav2_costmap_2d/src/layer.cpp @@ -83,9 +83,9 @@ Layer::hasParameter(const std::string & param_name) void Layer::undeclareAllParameters() { - std::for_each(begin(local_params_), end(local_params_), [this](const std::string & param_name) { - node_->undeclare_parameter(getFullName(param_name)); - }); + for (auto & param_name : local_params_) { + node_->undeclare_parameter(getFullName(param_name)); + } local_params_.clear(); } diff --git a/nav2_costmap_2d/test/integration/CMakeLists.txt b/nav2_costmap_2d/test/integration/CMakeLists.txt index f7a57bf0cc..610cd1ec5a 100644 --- a/nav2_costmap_2d/test/integration/CMakeLists.txt +++ b/nav2_costmap_2d/test/integration/CMakeLists.txt @@ -1,35 +1,24 @@ -# ament_add_gtest_executable(footprint_tests_exec -# footprint_tests.cpp -# ) -# ament_target_dependencies(footprint_tests_exec -# ${dependencies} -# ) -# target_link_libraries(footprint_tests_exec -# nav2_costmap_2d_core -# layers -# ) - -# ament_add_gtest_executable(static_tests_exec -# static_tests.cpp -# ) -# ament_target_dependencies(static_tests_exec -# ${dependencies} -# ) -# target_link_libraries(static_tests_exec -# nav2_costmap_2d_core -# layers -# ) +ament_add_gtest_executable(footprint_tests_exec + footprint_tests.cpp +) +ament_target_dependencies(footprint_tests_exec + ${dependencies} +) +target_link_libraries(footprint_tests_exec + nav2_costmap_2d_core + layers +) -# ament_add_gtest_executable(inflation_tests_exec -# inflation_tests.cpp -# ) -# ament_target_dependencies(inflation_tests_exec -# ${dependencies} -# ) -# target_link_libraries(inflation_tests_exec -# nav2_costmap_2d_core -# layers -# ) +ament_add_gtest_executable(inflation_tests_exec + inflation_tests.cpp +) +ament_target_dependencies(inflation_tests_exec + ${dependencies} +) +target_link_libraries(inflation_tests_exec + nav2_costmap_2d_core + layers +) ament_add_gtest_executable(obstacle_tests_exec obstacle_tests.cpp @@ -42,35 +31,25 @@ target_link_libraries(obstacle_tests_exec layers ) -# ament_add_test(footprint_tests -# GENERATE_RESULT_FOR_RETURN_CODE_ZERO -# COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/costmap_tests_launch.py" -# WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" -# ENV -# TEST_MAP=${TEST_MAP_DIR}/TenByTen.yaml -# TEST_LAUNCH_DIR=${TEST_LAUNCH_DIR} -# TEST_EXECUTABLE=$ -# ) - -# ament_add_test(static_tests -# GENERATE_RESULT_FOR_RETURN_CODE_ZERO -# COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/costmap_tests_launch.py" -# WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" -# ENV -# TEST_MAP=${TEST_MAP_DIR}/TenByTen.yaml -# TEST_LAUNCH_DIR=${TEST_LAUNCH_DIR} -# TEST_EXECUTABLE=$ -# ) +ament_add_test(footprint_tests + GENERATE_RESULT_FOR_RETURN_CODE_ZERO + COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/costmap_tests_launch.py" + WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" + ENV + TEST_MAP=${TEST_MAP_DIR}/TenByTen.yaml + TEST_LAUNCH_DIR=${TEST_LAUNCH_DIR} + TEST_EXECUTABLE=$ +) -# ament_add_test(inflation_tests -# GENERATE_RESULT_FOR_RETURN_CODE_ZERO -# COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/costmap_tests_launch.py" -# WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" -# ENV -# TEST_MAP=${TEST_MAP_DIR}/TenByTen.yaml -# TEST_LAUNCH_DIR=${TEST_LAUNCH_DIR} -# TEST_EXECUTABLE=$ -# ) +ament_add_test(inflation_tests + GENERATE_RESULT_FOR_RETURN_CODE_ZERO + COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/costmap_tests_launch.py" + WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" + ENV + TEST_MAP=${TEST_MAP_DIR}/TenByTen.yaml + TEST_LAUNCH_DIR=${TEST_LAUNCH_DIR} + TEST_EXECUTABLE=$ +) ament_add_test(obstacle_tests GENERATE_RESULT_FOR_RETURN_CODE_ZERO diff --git a/nav2_costmap_2d/test/integration/costmap_tests_launch.py b/nav2_costmap_2d/test/integration/costmap_tests_launch.py index 8e51af5874..95c8f00642 100755 --- a/nav2_costmap_2d/test/integration/costmap_tests_launch.py +++ b/nav2_costmap_2d/test/integration/costmap_tests_launch.py @@ -20,9 +20,9 @@ from launch import LaunchDescription from launch import LaunchService from launch.actions import ExecuteProcess -import launch_ros.actions from launch.actions import IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource +import launch_ros.actions from launch_testing.legacy import LaunchTestService diff --git a/nav2_costmap_2d/test/integration/footprint_tests.cpp b/nav2_costmap_2d/test/integration/footprint_tests.cpp index 7ae29bd65f..2830390a3a 100644 --- a/nav2_costmap_2d/test/integration/footprint_tests.cpp +++ b/nav2_costmap_2d/test/integration/footprint_tests.cpp @@ -41,7 +41,7 @@ #include "rclcpp/rclcpp.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.h" #include "tf2_ros/transform_listener.h" -#include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "nav2_costmap_2d/footprint.hpp" class RclCppFixture { @@ -51,12 +51,14 @@ class RclCppFixture }; RclCppFixture g_rclcppfixture; -class FootprintTestNode : public nav2_costmap_2d::Costmap2DROS +class FootprintTestNode { public: - FootprintTestNode(std::string name, tf2_ros::Buffer & buffer) - : nav2_costmap_2d::Costmap2DROS(name, buffer) - {} + FootprintTestNode() + { + // Default footprint padding and footprint radius from Costmap2DROS + testFootprint(0.01f, 0.1); + } ~FootprintTestNode() {} @@ -66,18 +68,33 @@ class FootprintTestNode : public nav2_costmap_2d::Costmap2DROS if (footprint != "" && footprint != "[]") { std::vector new_footprint; if (nav2_costmap_2d::makeFootprintFromString(footprint, new_footprint)) { - nav2_costmap_2d::Costmap2DROS::setUnpaddedRobotFootprint(new_footprint); + setRobotFootprint(new_footprint); } else { - RCLCPP_ERROR(get_logger(), "Invalid footprint string"); + RCLCPP_ERROR(rclcpp::get_logger("footprint_tester"), "Invalid footprint string"); } } } + void testFootprint(double footprint_padding, double robot_radius) { footprint_padding_ = footprint_padding; - nav2_costmap_2d::Costmap2DROS::setUnpaddedRobotFootprint( - nav2_costmap_2d::makeFootprintFromRadius(robot_radius)); + setRobotFootprint(nav2_costmap_2d::makeFootprintFromRadius(robot_radius)); + } + + std::vector getRobotFootprint() + { + return footprint_; } + +protected: + void setRobotFootprint(const std::vector & points) + { + footprint_ = points; + nav2_costmap_2d::padFootprint(footprint_, footprint_padding_); + } + + double footprint_padding_; + std::vector footprint_; }; class TestNode : public ::testing::Test @@ -85,37 +102,20 @@ class TestNode : public ::testing::Test public: TestNode() { - auto node = nav2_util::LifecycleNode::make_shared("footprint_tests"); - - tf_ = new tf2_ros::Buffer(node->get_clock()); - tfl_ = new tf2_ros::TransformListener(*tf_); - - // This empty transform is added to satisfy the constructor of - // Costmap2DROS, which waits for the transform from map to base_link - // to become available. - geometry_msgs::msg::TransformStamped base_rel_map; - base_rel_map.transform = tf2::toMsg(tf2::Transform::getIdentity()); - base_rel_map.child_frame_id = "base_link"; - base_rel_map.header.frame_id = "map"; - base_rel_map.header.stamp = node->now(); - tf_->setTransform(base_rel_map, "footprint_tests"); - - costmap_ = new FootprintTestNode("costmap_footprint_tests", *tf_); + footprint_tester_ = std::make_shared(); } ~TestNode() {} protected: - FootprintTestNode * costmap_; - tf2_ros::TransformListener * tfl_; - tf2_ros::Buffer * tf_; + std::shared_ptr footprint_tester_; }; // Start with empty test before updating test footprints TEST_F(TestNode, footprint_empty) { // FootprintTestNode cm("costmap_footprint_empty", *tf_); - std::vector footprint = costmap_->getRobotFootprint(); + std::vector footprint = footprint_tester_->getRobotFootprint(); // With no specification of footprint or radius, // defaults to 0.1 meter radius plus 0.01 meter padding. EXPECT_EQ(16, footprint.size()); @@ -127,9 +127,9 @@ TEST_F(TestNode, footprint_empty) TEST_F(TestNode, unpadded_footprint_from_string_param) { - costmap_->testFootprint(0.0, "[[1, 1], [-1, 1], [-1, -1]]"); + footprint_tester_->testFootprint(0.0, "[[1, 1], [-1, 1], [-1, -1]]"); - std::vector footprint = costmap_->getRobotFootprint(); + std::vector footprint = footprint_tester_->getRobotFootprint(); EXPECT_EQ(3, footprint.size()); EXPECT_EQ(1.0f, footprint[0].x); @@ -147,9 +147,9 @@ TEST_F(TestNode, unpadded_footprint_from_string_param) TEST_F(TestNode, padded_footprint_from_string_param) { - costmap_->testFootprint(0.5, "[[1, 1], [-1, 1], [-1, -1]]"); + footprint_tester_->testFootprint(0.5, "[[1, 1], [-1, 1], [-1, -1]]"); - std::vector footprint = costmap_->getRobotFootprint(); + std::vector footprint = footprint_tester_->getRobotFootprint(); EXPECT_EQ(3, footprint.size()); EXPECT_EQ(1.5f, footprint[0].x); @@ -167,8 +167,8 @@ TEST_F(TestNode, padded_footprint_from_string_param) TEST_F(TestNode, radius_param) { - costmap_->testFootprint(0, 10.0); - std::vector footprint = costmap_->getRobotFootprint(); + footprint_tester_->testFootprint(0, 10.0); + std::vector footprint = footprint_tester_->getRobotFootprint(); // Circular robot has 16-point footprint auto-generated. EXPECT_EQ(16, footprint.size()); @@ -185,8 +185,8 @@ TEST_F(TestNode, radius_param) TEST_F(TestNode, footprint_from_same_level_param) { - costmap_->testFootprint(0.0, "[[1, 2], [3, 4], [5, 6]]"); - std::vector footprint = costmap_->getRobotFootprint(); + footprint_tester_->testFootprint(0.0, "[[1, 2], [3, 4], [5, 6]]"); + std::vector footprint = footprint_tester_->getRobotFootprint(); EXPECT_EQ(3, footprint.size()); EXPECT_EQ(1.0f, footprint[0].x); diff --git a/nav2_costmap_2d/test/integration/inflation_tests.cpp b/nav2_costmap_2d/test/integration/inflation_tests.cpp index 36ec8c2b66..5aaff43f94 100644 --- a/nav2_costmap_2d/test/integration/inflation_tests.cpp +++ b/nav2_costmap_2d/test/integration/inflation_tests.cpp @@ -60,18 +60,13 @@ RclCppFixture g_rclcppfixture; class TestNode : public ::testing::Test { public: - TestNode() - { - node_ = nav2_util::LifecycleNode::make_shared("inflation_test_node"); - // Set cost_scaling_factor parameter to 1.0 for inflation layer - node_->set_parameters({rclcpp::Parameter("inflation.cost_scaling_factor", 1.0)}); - } + TestNode() {} ~TestNode() {} std::vector setRadii( nav2_costmap_2d::LayeredCostmap & layers, - double length, double width, double inflation_radius); + double length, double width); void validatePointInflation( unsigned int mx, unsigned int my, @@ -79,13 +74,15 @@ class TestNode : public ::testing::Test nav2_costmap_2d::InflationLayer * ilayer, double inflation_radius); + void initNode(double inflation_radius); + protected: nav2_util::LifecycleNode::SharedPtr node_; }; std::vector TestNode::setRadii( nav2_costmap_2d::LayeredCostmap & layers, - double length, double width, double inflation_radius) + double length, double width) { std::vector polygon; Point p; @@ -103,7 +100,6 @@ std::vector TestNode::setRadii( polygon.push_back(p); layers.setFootprint(polygon); - node_->set_parameters({rclcpp::Parameter("inflation.inflation_radius", inflation_radius)}); return polygon; } @@ -163,15 +159,41 @@ void TestNode::validatePointInflation( delete[] seen; } +void TestNode::initNode(double inflation_radius) +{ + std::vector parameters; + // Set cost_scaling_factor parameter to 1.0 for inflation layer + parameters.push_back(rclcpp::Parameter("inflation.cost_scaling_factor", 1.0)); + parameters.push_back(rclcpp::Parameter("inflation.inflation_radius", inflation_radius)); + + auto options = rclcpp::NodeOptions(); + options.parameter_overrides(parameters); + + node_ = std::make_shared( + "inflation_test_node", "", false, options); + + // Declare non-plugin specific costmap parameters + node_->declare_parameter("map_topic", rclcpp::ParameterValue(std::string("/map"))); + node_->declare_parameter("track_unknown_space", rclcpp::ParameterValue(false)); + node_->declare_parameter("use_maximum", rclcpp::ParameterValue(false)); + node_->declare_parameter("lethal_cost_threshold", rclcpp::ParameterValue(100)); + node_->declare_parameter("unknown_cost_value", + rclcpp::ParameterValue(static_cast(0xff))); + node_->declare_parameter("trinary_costmap", rclcpp::ParameterValue(true)); + node_->declare_parameter("transform_tolerance", rclcpp::ParameterValue(0.3)); + node_->declare_parameter("observation_sources", rclcpp::ParameterValue(std::string(""))); +} + TEST_F(TestNode, testAdjacentToObstacleCanStillMove) { + initNode(4.1); tf2_ros::Buffer tf(node_->get_clock()); nav2_costmap_2d::LayeredCostmap layers("frame", false, false); layers.resizeMap(10, 10, 1, 0, 0); // Footprint with inscribed radius = 2.1 // circumscribed radius = 3.1 - std::vector polygon = setRadii(layers, 2.1, 2.3, 4.1); + std::vector polygon = setRadii(layers, 2.1, 2.3); nav2_costmap_2d::ObstacleLayer * olayer = addObstacleLayer(layers, tf, node_); nav2_costmap_2d::InflationLayer * ilayer = addInflationLayer(layers, tf, node_); @@ -190,14 +212,16 @@ TEST_F(TestNode, testAdjacentToObstacleCanStillMove) EXPECT_EQ(nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE, costmap->getCost(1, 1)); } -TEST_F(TestNode, testInflationShouldNotCreateUnknowns) { +TEST_F(TestNode, testInflationShouldNotCreateUnknowns) +{ + initNode(4.1); tf2_ros::Buffer tf(node_->get_clock()); nav2_costmap_2d::LayeredCostmap layers("frame", false, false); layers.resizeMap(10, 10, 1, 0, 0); // Footprint with inscribed radius = 2.1 // circumscribed radius = 3.1 - std::vector polygon = setRadii(layers, 2.1, 2.3, 4.1); + std::vector polygon = setRadii(layers, 2.1, 2.3); nav2_costmap_2d::ObstacleLayer * olayer = addObstacleLayer(layers, tf, node_); nav2_costmap_2d::InflationLayer * ilayer = addInflationLayer(layers, tf, node_); @@ -211,18 +235,19 @@ TEST_F(TestNode, testInflationShouldNotCreateUnknowns) { EXPECT_EQ(countValues(*costmap, nav2_costmap_2d::NO_INFORMATION), 0); } - /** * Test for the cost function correctness with a larger range and different values */ -TEST_F(TestNode, testCostFunctionCorrectness) { +TEST_F(TestNode, testCostFunctionCorrectness) +{ + initNode(10.5); tf2_ros::Buffer tf(node_->get_clock()); nav2_costmap_2d::LayeredCostmap layers("frame", false, false); - layers.resizeMap(100, 100, 1, 0, 0); + layers.resizeMap(100, 100, 1, 0, 0); // Footprint with inscribed radius = 5.0 // circumscribed radius = 8.0 - std::vector polygon = setRadii(layers, 5.0, 6.25, 10.5); + std::vector polygon = setRadii(layers, 5.0, 6.25); nav2_costmap_2d::ObstacleLayer * olayer = addObstacleLayer(layers, tf, node_); nav2_costmap_2d::InflationLayer * ilayer = addInflationLayer(layers, tf, node_); @@ -283,15 +308,17 @@ TEST_F(TestNode, testCostFunctionCorrectness) { * the previously used priority queue. This is a more thorough * test of the cost function being correctly applied. */ -TEST_F(TestNode, testInflationOrderCorrectness) { +TEST_F(TestNode, testInflationOrderCorrectness) +{ + const double inflation_radius = 4.1; + initNode(inflation_radius); tf2_ros::Buffer tf(node_->get_clock()); nav2_costmap_2d::LayeredCostmap layers("frame", false, false); layers.resizeMap(10, 10, 1, 0, 0); // Footprint with inscribed radius = 2.1 // circumscribed radius = 3.1 - const double inflation_radius = 4.1; - std::vector polygon = setRadii(layers, 2.1, 2.3, inflation_radius); + std::vector polygon = setRadii(layers, 2.1, 2.3); nav2_costmap_2d::ObstacleLayer * olayer = addObstacleLayer(layers, tf, node_); nav2_costmap_2d::InflationLayer * ilayer = addInflationLayer(layers, tf, node_); @@ -311,13 +338,15 @@ TEST_F(TestNode, testInflationOrderCorrectness) { /** * Test inflation for both static and dynamic obstacles */ -TEST_F(TestNode, testInflation) { +TEST_F(TestNode, testInflation) +{ + initNode(1); tf2_ros::Buffer tf(node_->get_clock()); nav2_costmap_2d::LayeredCostmap layers("frame", false, false); // Footprint with inscribed radius = 2.1 // circumscribed radius = 3.1 - std::vector polygon = setRadii(layers, 1, 1, 1); + std::vector polygon = setRadii(layers, 1, 1); addStaticLayer(layers, tf, node_); nav2_costmap_2d::ObstacleLayer * olayer = addObstacleLayer(layers, tf, node_); @@ -379,13 +408,15 @@ TEST_F(TestNode, testInflation) { /** * Test specific inflation scenario to ensure we do not set inflated obstacles to be raw obstacles. */ -TEST_F(TestNode, testInflation2) { +TEST_F(TestNode, testInflation2) +{ + initNode(1); tf2_ros::Buffer tf(node_->get_clock()); nav2_costmap_2d::LayeredCostmap layers("frame", false, false); // Footprint with inscribed radius = 2.1 // circumscribed radius = 3.1 - std::vector polygon = setRadii(layers, 1, 1, 1); + std::vector polygon = setRadii(layers, 1, 1); addStaticLayer(layers, tf, node_); nav2_costmap_2d::ObstacleLayer * olayer = addObstacleLayer(layers, tf, node_); @@ -407,13 +438,15 @@ TEST_F(TestNode, testInflation2) { /** * Test inflation behavior, starting with an empty map */ -TEST_F(TestNode, testInflation3) { +TEST_F(TestNode, testInflation3) +{ + initNode(3); tf2_ros::Buffer tf(node_->get_clock()); nav2_costmap_2d::LayeredCostmap layers("frame", false, false); layers.resizeMap(10, 10, 1, 0, 0); // 1 2 3 - std::vector polygon = setRadii(layers, 1, 1.75, 3); + std::vector polygon = setRadii(layers, 1, 1.75); nav2_costmap_2d::ObstacleLayer * olayer = addObstacleLayer(layers, tf, node_); nav2_costmap_2d::InflationLayer * ilayer = addInflationLayer(layers, tf, node_); diff --git a/nav2_costmap_2d/test/integration/obstacle_tests.cpp b/nav2_costmap_2d/test/integration/obstacle_tests.cpp index aa1685a954..25b41e5d20 100644 --- a/nav2_costmap_2d/test/integration/obstacle_tests.cpp +++ b/nav2_costmap_2d/test/integration/obstacle_tests.cpp @@ -104,8 +104,15 @@ class TestNode : public ::testing::Test TestNode() { node_ = std::make_shared("obstacle_test_node"); - - node_->declare_parameter("map_topic", rclcpp::ParameterValue(string("/map"))); + node_->declare_parameter("map_topic", rclcpp::ParameterValue(std::string("/map"))); + node_->declare_parameter("track_unknown_space", rclcpp::ParameterValue(false)); + node_->declare_parameter("use_maximum", rclcpp::ParameterValue(false)); + node_->declare_parameter("lethal_cost_threshold", rclcpp::ParameterValue(100)); + node_->declare_parameter("unknown_cost_value", + rclcpp::ParameterValue(static_cast(0xff))); + node_->declare_parameter("trinary_costmap", rclcpp::ParameterValue(true)); + node_->declare_parameter("transform_tolerance", rclcpp::ParameterValue(0.3)); + node_->declare_parameter("observation_sources", rclcpp::ParameterValue(std::string(""))); } ~TestNode() {} @@ -224,7 +231,7 @@ TEST_F(TestNode, testRaytracing2) { */ TEST_F(TestNode, testWaveInterference) { tf2_ros::Buffer tf(node_->get_clock()); - + node_->set_parameter(rclcpp::Parameter("track_unknown_space", true)); // Start with an empty map, no rolling window, tracking unknown nav2_costmap_2d::LayeredCostmap layers("frame", false, true); layers.resizeMap(10, 10, 1, 0, 0); diff --git a/nav2_costmap_2d/test/integration/static_tests.cpp b/nav2_costmap_2d/test/integration/static_tests.cpp deleted file mode 100644 index 157b959e3c..0000000000 --- a/nav2_costmap_2d/test/integration/static_tests.cpp +++ /dev/null @@ -1,328 +0,0 @@ -/* - * Copyright (c) 2013, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** - * @author David Lu!! - * Test harness for StaticMap Layer for Costmap2D - */ - -#include - -#include "gtest/gtest.h" -#include "nav2_costmap_2d/costmap_2d.hpp" -#include "nav2_costmap_2d/layered_costmap.hpp" -#include "nav2_costmap_2d/obstacle_layer.hpp" -#include "nav2_costmap_2d/static_layer.hpp" -#include "nav2_costmap_2d/observation_buffer.hpp" -#include "nav2_costmap_2d/testing_helper.hpp" - -/** - * Tests the reset method - * -TEST(costmap, testResetForStaticMap){ - // Define a static map with a large object in the center - std::vector staticMap; - for(unsigned int i=0; i<10; i++){ - for(unsigned int j=0; j<10; j++){ - staticMap.push_back(nav2_costmap_2d::LETHAL_OBSTACLE); - } - } - - // Allocate the cost map, with a inflation to 3 cells all around - Costmap2D map(10, 10, RESOLUTION, 0.0, 0.0, 3, 3, 3, OBSTACLE_RANGE, MAX_Z, RAYTRACE_RANGE, 25, staticMap, THRESHOLD); - - // Populate the cost map with a wall around the perimeter. Free space should clear out the room. - pcl::PointCloud cloud; - cloud.points.resize(40); - - // Left wall - unsigned int ind = 0; - for (unsigned int i = 0; i < 10; i++){ - // Left - cloud.points[ind].x = 0; - cloud.points[ind].y = i; - cloud.points[ind].z = MAX_Z; - ind++; - - // Top - cloud.points[ind].x = i; - cloud.points[ind].y = 0; - cloud.points[ind].z = MAX_Z; - ind++; - - // Right - cloud.points[ind].x = 9; - cloud.points[ind].y = i; - cloud.points[ind].z = MAX_Z; - ind++; - - // Bottom - cloud.points[ind].x = i; - cloud.points[ind].y = 9; - cloud.points[ind].z = MAX_Z; - ind++; - } - - double wx = 5.0, wy = 5.0; - geometry_msgs::Point p; - p.x = wx; - p.y = wy; - p.z = MAX_Z; - Observation obs(p, cloud, OBSTACLE_RANGE, RAYTRACE_RANGE); - std::vector obsBuf; - obsBuf.push_back(obs); - - // Update the cost map for this observation - map.updateWorld(wx, wy, obsBuf, obsBuf); - - // Verify that we now have only 36 cells with lethal cost, thus provong that we have correctly cleared - // free space - int hitCount = 0; - for(unsigned int i=0; i < 10; ++i){ - for(unsigned int j=0; j < 10; ++j){ - if(map.getCost(i, j) == nav2_costmap_2d::LETHAL_OBSTACLE){ - hitCount++; - } - } - } - ASSERT_EQ(hitCount, 36); - - // Veriy that we have 64 non-leathal - hitCount = 0; - for(unsigned int i=0; i < 10; ++i){ - for(unsigned int j=0; j < 10; ++j){ - if(map.getCost(i, j) != nav2_costmap_2d::LETHAL_OBSTACLE) - hitCount++; - } - } - ASSERT_EQ(hitCount, 64); - - // Now if we reset the cost map, we should have our map go back to being completely occupied - map.resetMapOutsideWindow(wx, wy, 0.0, 0.0); - - //We should now go back to everything being occupied - hitCount = 0; - for(unsigned int i=0; i < 10; ++i){ - for(unsigned int j=0; j < 10; ++j){ - if(map.getCost(i, j) == nav2_costmap_2d::LETHAL_OBSTACLE) - hitCount++; - } - } - ASSERT_EQ(hitCount, 100); - -} - -/** Test for copying a window of a costmap * -TEST(costmap, testWindowCopy){ - Costmap2D map(GRID_WIDTH, GRID_HEIGHT, RESOLUTION, 0.0, 0.0, ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS, - 10.0, MAX_Z, 10.0, 25, MAP_10_BY_10, THRESHOLD); - - /* - for(unsigned int i = 0; i < 10; ++i){ - for(unsigned int j = 0; j < 10; ++j){ - printf("%3d ", map.getCost(i, j)); - } - printf("\n"); - } - printf("\n"); - * - - Costmap2D windowCopy; - - //first test that if we try to make a window that is too big, things fail - windowCopy.copyCostmapWindow(map, 2.0, 2.0, 6.0, 12.0); - ASSERT_EQ(windowCopy.getSizeInCellsX(), (unsigned int)0); - ASSERT_EQ(windowCopy.getSizeInCellsY(), (unsigned int)0); - - //Next, test that trying to make a map window itself fails - map.copyCostmapWindow(map, 2.0, 2.0, 6.0, 6.0); - ASSERT_EQ(map.getSizeInCellsX(), (unsigned int)10); - ASSERT_EQ(map.getSizeInCellsY(), (unsigned int)10); - - //Next, test that legal input generates the result that we expect - windowCopy.copyCostmapWindow(map, 2.0, 2.0, 6.0, 6.0); - ASSERT_EQ(windowCopy.getSizeInCellsX(), (unsigned int)6); - ASSERT_EQ(windowCopy.getSizeInCellsY(), (unsigned int)6); - - //check that we actually get the windo that we expect - for(unsigned int i = 0; i < windowCopy.getSizeInCellsX(); ++i){ - for(unsigned int j = 0; j < windowCopy.getSizeInCellsY(); ++j){ - ASSERT_EQ(windowCopy.getCost(i, j), map.getCost(i + 2, j + 2)); - //printf("%3d ", windowCopy.getCost(i, j)); - } - //printf("\n"); - } - -} - -//test for updating costmaps with static data -TEST(costmap, testFullyContainedStaticMapUpdate){ - Costmap2D map(5, 5, RESOLUTION, 0.0, 0.0, ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS, - 10.0, MAX_Z, 10.0, 25, MAP_5_BY_5, THRESHOLD); - - Costmap2D static_map(GRID_WIDTH, GRID_HEIGHT, RESOLUTION, 0.0, 0.0, ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS, - 10.0, MAX_Z, 10.0, 25, MAP_10_BY_10, THRESHOLD); - - map.updateStaticMapWindow(0, 0, 10, 10, MAP_10_BY_10); - - for(unsigned int i = 0; i < map.getSizeInCellsX(); ++i){ - for(unsigned int j = 0; j < map.getSizeInCellsY(); ++j){ - ASSERT_EQ(map.getCost(i, j), static_map.getCost(i, j)); - } - } -} - -TEST(costmap, testOverlapStaticMapUpdate){ - Costmap2D map(5, 5, RESOLUTION, 0.0, 0.0, ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS, - 10.0, MAX_Z, 10.0, 25, MAP_5_BY_5, THRESHOLD); - - Costmap2D static_map(GRID_WIDTH, GRID_HEIGHT, RESOLUTION, 0.0, 0.0, ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS, - 10.0, MAX_Z, 10.0, 25, MAP_10_BY_10, THRESHOLD); - - map.updateStaticMapWindow(-10, -10, 10, 10, MAP_10_BY_10); - - ASSERT_FLOAT_EQ(map.getOriginX(), -10); - ASSERT_FLOAT_EQ(map.getOriginX(), -10); - ASSERT_EQ(map.getSizeInCellsX(), (unsigned int)15); - ASSERT_EQ(map.getSizeInCellsY(), (unsigned int)15); - for(unsigned int i = 0; i < 10; ++i){ - for(unsigned int j = 0; j < 10; ++j){ - ASSERT_EQ(map.getCost(i, j), static_map.getCost(i, j)); - } - } - - std::vector blank(100); - - //check to make sure that inflation on updates are being done correctly - map.updateStaticMapWindow(-10, -10, 10, 10, blank); - - for(unsigned int i = 0; i < map.getSizeInCellsX(); ++i){ - for(unsigned int j = 0; j < map.getSizeInCellsY(); ++j){ - ASSERT_EQ(map.getCost(i, j), 0); - } - } - - std::vector fully_contained(25); - fully_contained[0] = 254; - fully_contained[4] = 254; - fully_contained[5] = 254; - fully_contained[9] = 254; - - Costmap2D small_static_map(5, 5, RESOLUTION, 0.0, 0.0, ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS, - 10.0, MAX_Z, 10.0, 25, fully_contained, THRESHOLD); - - map.updateStaticMapWindow(0, 0, 5, 5, fully_contained); - - ASSERT_FLOAT_EQ(map.getOriginX(), -10); - ASSERT_FLOAT_EQ(map.getOriginX(), -10); - ASSERT_EQ(map.getSizeInCellsX(), (unsigned int)15); - ASSERT_EQ(map.getSizeInCellsY(), (unsigned int)15); - for(unsigned int j = 0; j < 5; ++j){ - for(unsigned int i = 0; i < 5; ++i){ - ASSERT_EQ(map.getCost(i + 10, j + 10), small_static_map.getCost(i, j)); - } - } - -} - - -TEST(costmap, testStaticMap){ - Costmap2D map(GRID_WIDTH, GRID_HEIGHT, RESOLUTION, 0.0, 0.0, ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS, - 10.0, MAX_Z, 10.0, 25, MAP_10_BY_10, THRESHOLD); - - ASSERT_EQ(map.getSizeInCellsX(), (unsigned int)10); - ASSERT_EQ(map.getSizeInCellsY(), (unsigned int)10); - - // Verify that obstacles correctly identified from the static map. - std::vector occupiedCells; - - for(unsigned int i = 0; i < 10; ++i){ - for(unsigned int j = 0; j < 10; ++j){ - if(map.getCost(i, j) == nav2_costmap_2d::LETHAL_OBSTACLE){ - occupiedCells.push_back(map.getIndex(i, j)); - } - } - } - - ASSERT_EQ(occupiedCells.size(), (unsigned int)20); - - // Iterate over all id's and verify that they are present according to their - for(std::vector::const_iterator it = occupiedCells.begin(); it != occupiedCells.end(); ++it){ - unsigned int ind = *it; - unsigned int x, y; - map.indexToCells(ind, x, y); - ASSERT_EQ(find(occupiedCells, map.getIndex(x, y)), true); - ASSERT_EQ(MAP_10_BY_10[ind] >= 100, true); - ASSERT_EQ(map.getCost(x, y) >= 100, true); - } - - // Block of 200 - ASSERT_EQ(find(occupiedCells, map.getIndex(7, 2)), true); - ASSERT_EQ(find(occupiedCells, map.getIndex(8, 2)), true); - ASSERT_EQ(find(occupiedCells, map.getIndex(9, 2)), true); - ASSERT_EQ(find(occupiedCells, map.getIndex(7, 3)), true); - ASSERT_EQ(find(occupiedCells, map.getIndex(8, 3)), true); - ASSERT_EQ(find(occupiedCells, map.getIndex(9, 3)), true); - ASSERT_EQ(find(occupiedCells, map.getIndex(7, 4)), true); - ASSERT_EQ(find(occupiedCells, map.getIndex(8, 4)), true); - ASSERT_EQ(find(occupiedCells, map.getIndex(9, 4)), true); - - // Block of 100 - ASSERT_EQ(find(occupiedCells, map.getIndex(4, 3)), true); - ASSERT_EQ(find(occupiedCells, map.getIndex(4, 4)), true); - - // Block of 200 - ASSERT_EQ(find(occupiedCells, map.getIndex(3, 7)), true); - ASSERT_EQ(find(occupiedCells, map.getIndex(4, 7)), true); - ASSERT_EQ(find(occupiedCells, map.getIndex(5, 7)), true); - - - // Verify Coordinate Transformations, ROW MAJOR ORDER - ASSERT_EQ(worldToIndex(map, 0.0, 0.0), (unsigned int)0); - ASSERT_EQ(worldToIndex(map, 0.0, 0.99), (unsigned int)0); - ASSERT_EQ(worldToIndex(map, 0.0, 1.0), (unsigned int)10); - ASSERT_EQ(worldToIndex(map, 1.0, 0.99), (unsigned int)1); - ASSERT_EQ(worldToIndex(map, 9.99, 9.99), (unsigned int)99); - ASSERT_EQ(worldToIndex(map, 8.2, 3.4), (unsigned int)38); - - // Ensure we hit the middle of the cell for world co-ordinates - double wx, wy; - indexToWorld(map, 99, wx, wy); - ASSERT_EQ(wx, 9.5); - ASSERT_EQ(wy, 9.5); -} - -//*/ - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -}