m5p3nc3r/agv_pro_ros2
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

add humble-navigation2

Browse Source
This commit is contained in:
X-lanni
2025-05-27 19:03:40 +08:00
parent 974abb5e1e
commit e74ec539c2
1280 changed files with 204114 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
navigation2/nav2_costmap_2d/test/integration/CMakeLists.txt
+110
View File
@@ -0,0 +1,110 @@
ament_add_gtest_executable(footprint_tests_exec
footprint_tests.cpp
)
target_link_libraries(footprint_tests_exec
${PROJECT_NAME}::nav2_costmap_2d_core
${PROJECT_NAME}::layers
)
ament_add_gtest_executable(test_collision_checker_exec
test_costmap_topic_collision_checker.cpp
)
target_link_libraries(test_collision_checker_exec
${PROJECT_NAME}::nav2_costmap_2d_core
${PROJECT_NAME}::nav2_costmap_2d_client
${PROJECT_NAME}::layers
)
ament_add_gtest_executable(inflation_tests_exec
inflation_tests.cpp
)
target_link_libraries(inflation_tests_exec
${PROJECT_NAME}::nav2_costmap_2d_core
${PROJECT_NAME}::layers
)
ament_add_gtest_executable(obstacle_tests_exec
obstacle_tests.cpp
)
target_link_libraries(obstacle_tests_exec
${PROJECT_NAME}::nav2_costmap_2d_core
${PROJECT_NAME}::layers
)
ament_add_gtest_executable(range_tests_exec
range_tests.cpp
)
target_link_libraries(range_tests_exec
${PROJECT_NAME}::nav2_costmap_2d_core
${PROJECT_NAME}::layers
)
ament_add_gtest(dyn_params_tests
dyn_params_tests.cpp
)
target_link_libraries(dyn_params_tests
${PROJECT_NAME}::nav2_costmap_2d_core
)
ament_add_test(test_collision_checker
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=$<TARGET_FILE:test_collision_checker_exec>
)
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=$<TARGET_FILE:footprint_tests_exec>
)
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=$<TARGET_FILE:inflation_tests_exec>
)
ament_add_test(obstacle_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=$<TARGET_FILE:obstacle_tests_exec>
)
ament_add_test(range_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=$<TARGET_FILE:range_tests_exec>
)
## TODO(bpwilcox): this test (I believe) is intended to be launched with the simple_driving_test.xml,
## which has a dependency on rosbag playback
# ament_add_gtest_executable(costmap_tester
# costmap_tester.cpp
# )
# ament_target_dependencies(costmap_tester
# ${dependencies}
# )
# target_link_libraries(costmap_tester
# ${PROJECT_NAME}::nav2_costmap_2d_core
# layers
# )
navigation2/nav2_costmap_2d/test/integration/costmap_tester.cpp
+175
View File
@@ -0,0 +1,175 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2009, 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 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: Eitan Marder-Eppstein
*********************************************************************/
#include <memory>
#include "gtest/gtest.h"
#include "rclcpp/rclcpp.hpp"
#include "tf2_ros/transform_listener.h"
#include "nav2_costmap_2d/costmap_2d_ros.hpp"
#include "nav2_costmap_2d/cost_values.hpp"
namespace nav2_costmap_2d
{
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
class CostmapTester : public testing::Test
{
public:
explicit CostmapTester(tf2_ros::Buffer & tf);
void checkConsistentCosts();
void compareCellToNeighbors(
nav2_costmap_2d::Costmap2D & costmap,
unsigned int x, unsigned int y);
void compareCells(
nav2_costmap_2d::Costmap2D & costmap,
unsigned int x, unsigned int y, unsigned int nx, unsigned int ny);
virtual void TestBody() {}
};
CostmapTester::CostmapTester(tf2_ros::Buffer & tf)
{
costmap_ros_ = std::make_shared<nav2_costmap_2d::Costmap2DROS>("test_costmap", tf);
}
void CostmapTester::checkConsistentCosts()
{
nav2_costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap();
// get a copy of the costmap contained by our ros wrapper
costmap->saveMap("costmap_test.pgm");
// loop through the costmap and check for any unexpected drop-offs in costs
for (unsigned int i = 0; i < costmap->getSizeInCellsX(); ++i) {
for (unsigned int j = 0; j < costmap->getSizeInCellsY(); ++j) {
compareCellToNeighbors(*costmap, i, j);
}
}
}
void CostmapTester::compareCellToNeighbors(
nav2_costmap_2d::Costmap2D & costmap,
unsigned int x, unsigned int y)
{
// we'll compare the cost of this cell with that of
// its eight neighbors to see if they're reasonable
for (int offset_x = -1; offset_x <= 1; ++offset_x) {
for (int offset_y = -1; offset_y <= 1; ++offset_y) {
int nx = x + offset_x;
int ny = y + offset_y;
// check to make sure that the neighbor cell is a legal one
if (nx >= 0 && nx < static_cast<int>(costmap.getSizeInCellsX()) && ny >= 0 &&
ny < static_cast<int>(costmap.getSizeInCellsY()))
{
compareCells(costmap, x, y, nx, ny);
}
}
}
}
// for all lethal and inscribed costs,
// we'll make sure that their neighbors have the cost values we'd expect
void CostmapTester::compareCells(
nav2_costmap_2d::Costmap2D & costmap,
unsigned int x, unsigned int y, unsigned int nx, unsigned int ny)
{
double cell_distance = hypot(static_cast<int>(x - nx), static_cast<int>(y - ny));
unsigned char cell_cost = costmap.getCost(x, y);
unsigned char neighbor_cost = costmap.getCost(nx, ny);
if (cell_cost == nav2_costmap_2d::LETHAL_OBSTACLE) {
// if the cell is a lethal obstacle,
// then we know that all its neighbors should have equal or slighlty less cost
unsigned char expected_lowest_cost = 0;
EXPECT_TRUE(
neighbor_cost >= expected_lowest_cost ||
(cell_distance > 0 && neighbor_cost == nav2_costmap_2d::FREE_SPACE));
} else if (cell_cost == nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE) {
// the furthest valid distance from an obstacle
// is the inscribed radius plus the cell distance away
double furthest_valid_distance = 0;
unsigned char expected_lowest_cost = 0;
if (neighbor_cost < expected_lowest_cost) {
RCLCPP_ERROR(
rclcpp::get_logger(
"costmap_tester"),
"Cell cost (%d, %d): %d, neighbor cost (%d, %d): %d, expected lowest cost: %d, cell distance: %.2f, furthest valid distance: %.2f", // NOLINT
x, y, cell_cost, nx, ny, neighbor_cost, expected_lowest_cost,
cell_distance, furthest_valid_distance);
RCLCPP_ERROR(
rclcpp::get_logger("costmap_tester"), "Cell: (%d, %d), Neighbor: (%d, %d)",
x, y, nx, ny);
costmap.saveMap("failing_costmap.pgm");
}
EXPECT_TRUE(
neighbor_cost >= expected_lowest_cost ||
(furthest_valid_distance > 0 && neighbor_cost == nav2_costmap_2d::FREE_SPACE));
}
}
} // namespace nav2_costmap_2d
nav2_costmap_2d::CostmapTester * map_tester = NULL;
tf2_ros::TransformListener * tfl_;
tf2_ros::Buffer * tf_;
TEST(CostmapTester, checkConsistentCosts) {
map_tester->checkConsistentCosts();
}
void testCallback()
{
int test_result = RUN_ALL_TESTS();
RCLCPP_INFO(rclcpp::get_logger("costmap_tester"), "gtest return value: %d", test_result);
}
int main(int argc, char ** argv)
{
rclcpp::init(argc, argv);
auto node = nav2_util::LifecycleNode::make_shared("costmap_tester");
testing::InitGoogleTest(&argc, argv);
tf_ = new tf2_ros::Buffer(node->get_clock());
tfl_ = new tf2_ros::TransformListener(*tf_);
map_tester = new nav2_costmap_2d::CostmapTester(*tf_);
rclcpp::TimerBase::SharedPtr timer = node->create_wall_timer(30000ms, testCallback);
rclcpp::spin(costmap_ros_);
rclcpp::shutdown();
return 0;
}
navigation2/nav2_costmap_2d/test/integration/costmap_tests_launch.py
Executable
+59
View File
@@ -0,0 +1,59 @@
#!/usr/bin/env python3
# Copyright (c) 2018 Intel Corporation
#
# 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.
import os
import sys
from launch import LaunchDescription
from launch import LaunchService
from launch.actions import ExecuteProcess
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
import launch_ros.actions
from launch_testing.legacy import LaunchTestService
def main(argv=sys.argv[1:]):
launchFile = os.path.join(os.getenv('TEST_LAUNCH_DIR'), 'costmap_map_server.launch.py')
testExecutable = os.getenv('TEST_EXECUTABLE')
lifecycle_manager = launch_ros.actions.Node(
package='nav2_lifecycle_manager',
executable='lifecycle_manager',
name='lifecycle_manager',
output='screen',
parameters=[{'node_names': ['map_server']}, {'autostart': True}])
ld = LaunchDescription([
IncludeLaunchDescription(PythonLaunchDescriptionSource([launchFile])),
lifecycle_manager
])
test1_action = ExecuteProcess(
cmd=[testExecutable],
name='costmap_tests',
output='screen'
)
lts = LaunchTestService()
lts.add_test_action(ld, test1_action)
ls = LaunchService(argv=argv)
ls.include_launch_description(ld)
return lts.run(ls)
if __name__ == '__main__':
sys.exit(main())
navigation2/nav2_costmap_2d/test/integration/dyn_params_tests.cpp
+105
View File
@@ -0,0 +1,105 @@
// Copyright (c) 2021 Wyca Robotics
//
// 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.
#include <memory>
#include <vector>
#include "gtest/gtest.h"
#include "rclcpp/rclcpp.hpp"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include "nav2_costmap_2d/costmap_2d_ros.hpp"
#include "tf2_ros/transform_broadcaster.h"
class RclCppFixture
{
public:
RclCppFixture() {rclcpp::init(0, nullptr);}
~RclCppFixture() {rclcpp::shutdown();}
};
RclCppFixture g_rclcppfixture;
class DynParamTestNode
{
public:
DynParamTestNode() {}
~DynParamTestNode() {}
};
TEST(DynParamTestNode, testDynParamsSet)
{
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("dyn_param_tester");
auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>("test_costmap");
costmap->on_configure(rclcpp_lifecycle::State());
// Set tf between default global_frame and robot_base_frame in order not to block in on_activate
std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_ =
std::make_unique<tf2_ros::TransformBroadcaster>(node);
geometry_msgs::msg::TransformStamped t;
t.header.stamp = node->get_clock()->now();
t.header.frame_id = "map";
t.child_frame_id = "base_link";
tf_broadcaster_->sendTransform(t);
t.header.frame_id = "map";
t.child_frame_id = "test_frame";
tf_broadcaster_->sendTransform(t);
costmap->on_activate(rclcpp_lifecycle::State());
auto parameter_client = std::make_shared<rclcpp::AsyncParametersClient>(
node->shared_from_this(),
"/test_costmap/test_costmap",
rmw_qos_profile_parameters);
auto results1 = parameter_client->set_parameters_atomically(
{
rclcpp::Parameter("robot_radius", 1.234),
rclcpp::Parameter("footprint_padding", 2.345),
rclcpp::Parameter("transform_tolerance", 3.456),
rclcpp::Parameter("publish_frequency", 4.567),
rclcpp::Parameter("resolution", 5.678),
rclcpp::Parameter("origin_x", 6.789),
rclcpp::Parameter("origin_y", 7.891),
rclcpp::Parameter("width", 2),
rclcpp::Parameter("height", 3),
rclcpp::Parameter(
"footprint",
"[[-0.325, -0.325], [-0.325, 0.325], [0.325, 0.325], [0.46, 0.0], [0.325, -0.325]]"),
rclcpp::Parameter("robot_base_frame", "test_frame"),
});
// Try setting robot_base_frame to an invalid frame, should be rejected
auto results2 = parameter_client->set_parameters_atomically(
{
rclcpp::Parameter("robot_base_frame", "wrong_test_frame"),
});
rclcpp::spin_some(costmap->get_node_base_interface());
EXPECT_EQ(costmap->get_parameter("robot_radius").as_double(), 1.234);
EXPECT_EQ(costmap->get_parameter("footprint_padding").as_double(), 2.345);
EXPECT_EQ(costmap->get_parameter("transform_tolerance").as_double(), 3.456);
EXPECT_EQ(costmap->get_parameter("publish_frequency").as_double(), 4.567);
EXPECT_EQ(costmap->get_parameter("resolution").as_double(), 5.678);
EXPECT_EQ(costmap->get_parameter("origin_x").as_double(), 6.789);
EXPECT_EQ(costmap->get_parameter("origin_y").as_double(), 7.891);
EXPECT_EQ(costmap->get_parameter("width").as_int(), 2);
EXPECT_EQ(costmap->get_parameter("height").as_int(), 3);
EXPECT_EQ(
costmap->get_parameter("footprint").as_string(),
"[[-0.325, -0.325], [-0.325, 0.325], [0.325, 0.325], [0.46, 0.0], [0.325, -0.325]]");
EXPECT_EQ(costmap->get_parameter("robot_base_frame").as_string(), "test_frame");
costmap->on_deactivate(rclcpp_lifecycle::State());
costmap->on_cleanup(rclcpp_lifecycle::State());
costmap->on_shutdown(rclcpp_lifecycle::State());
}
navigation2/nav2_costmap_2d/test/integration/footprint_tests.cpp
+204
View File
@@ -0,0 +1,204 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2009, 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 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: Dave Hershberger
*********************************************************************/
#include <memory>
#include <string>
#include <vector>
#include "gtest/gtest.h"
#include "rclcpp/rclcpp.hpp"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include "tf2_ros/transform_listener.h"
#include "nav2_costmap_2d/footprint.hpp"
class RclCppFixture
{
public:
RclCppFixture() {rclcpp::init(0, nullptr);}
~RclCppFixture() {rclcpp::shutdown();}
};
RclCppFixture g_rclcppfixture;
class FootprintTestNode
{
public:
FootprintTestNode()
{
// Default footprint padding and footprint radius from Costmap2DROS
testFootprint(0.01f, 0.1);
}
~FootprintTestNode() {}
void testFootprint(double footprint_padding, std::string footprint)
{
footprint_padding_ = footprint_padding;
if (footprint != "" && footprint != "[]") {
std::vector<geometry_msgs::msg::Point> new_footprint;
if (nav2_costmap_2d::makeFootprintFromString(footprint, new_footprint)) {
setRobotFootprint(new_footprint);
} else {
RCLCPP_ERROR(rclcpp::get_logger("footprint_tester"), "Invalid footprint string");
}
}
}
void testFootprint(double footprint_padding, double robot_radius)
{
footprint_padding_ = footprint_padding;
setRobotFootprint(nav2_costmap_2d::makeFootprintFromRadius(robot_radius));
}
std::vector<geometry_msgs::msg::Point> getRobotFootprint()
{
return footprint_;
}
protected:
void setRobotFootprint(const std::vector<geometry_msgs::msg::Point> & points)
{
footprint_ = points;
nav2_costmap_2d::padFootprint(footprint_, footprint_padding_);
}
double footprint_padding_;
std::vector<geometry_msgs::msg::Point> footprint_;
};
class TestNode : public ::testing::Test
{
public:
TestNode()
{
footprint_tester_ = std::make_shared<FootprintTestNode>();
}
~TestNode() {}
protected:
std::shared_ptr<FootprintTestNode> footprint_tester_;
};
// Start with empty test before updating test footprints
TEST_F(TestNode, footprint_empty)
{
// FootprintTestNode cm("costmap_footprint_empty", *tf_);
std::vector<geometry_msgs::msg::Point> footprint = footprint_tester_->getRobotFootprint();
// With no specification of footprint or radius,
// defaults to 0.1 meter radius plus 0.01 meter padding.
EXPECT_EQ(16u, footprint.size());
EXPECT_NEAR(0.11f, footprint[0].x, 0.0001);
EXPECT_NEAR(0.0f, footprint[0].y, 0.0001);
EXPECT_EQ(0.0f, footprint[0].z);
}
TEST_F(TestNode, unpadded_footprint_from_string_param)
{
footprint_tester_->testFootprint(0.0, "[[1, 1], [-1, 1], [-1, -1]]");
std::vector<geometry_msgs::msg::Point> footprint = footprint_tester_->getRobotFootprint();
EXPECT_EQ(3u, footprint.size());
EXPECT_EQ(1.0f, footprint[0].x);
EXPECT_EQ(1.0f, footprint[0].y);
EXPECT_EQ(0.0f, footprint[0].z);
EXPECT_EQ(-1.0f, footprint[1].x);
EXPECT_EQ(1.0f, footprint[1].y);
EXPECT_EQ(0.0f, footprint[1].z);
EXPECT_EQ(-1.0f, footprint[2].x);
EXPECT_EQ(-1.0f, footprint[2].y);
EXPECT_EQ(0.0f, footprint[2].z);
}
TEST_F(TestNode, padded_footprint_from_string_param)
{
footprint_tester_->testFootprint(0.5, "[[1, 1], [-1, 1], [-1, -1]]");
std::vector<geometry_msgs::msg::Point> footprint = footprint_tester_->getRobotFootprint();
EXPECT_EQ(3u, footprint.size());
EXPECT_EQ(1.5f, footprint[0].x);
EXPECT_EQ(1.5f, footprint[0].y);
EXPECT_EQ(0.0f, footprint[0].z);
EXPECT_EQ(-1.5f, footprint[1].x);
EXPECT_EQ(1.5f, footprint[1].y);
EXPECT_EQ(0.0f, footprint[1].z);
EXPECT_EQ(-1.5f, footprint[2].x);
EXPECT_EQ(-1.5f, footprint[2].y);
EXPECT_EQ(0.0f, footprint[2].z);
}
TEST_F(TestNode, radius_param)
{
footprint_tester_->testFootprint(0, 10.0);
std::vector<geometry_msgs::msg::Point> footprint = footprint_tester_->getRobotFootprint();
// Circular robot has 16-point footprint auto-generated.
EXPECT_EQ(16u, footprint.size());
// Check the first point
EXPECT_EQ(10.0f, footprint[0].x);
EXPECT_EQ(0.0f, footprint[0].y);
EXPECT_EQ(0.0f, footprint[0].z);
// Check the 4th point, which should be 90 degrees around the circle from the first.
EXPECT_NEAR(0.0f, footprint[4].x, 0.0001);
EXPECT_NEAR(10.0f, footprint[4].y, 0.0001);
EXPECT_EQ(0.0f, footprint[4].z);
}
TEST_F(TestNode, footprint_from_same_level_param)
{
footprint_tester_->testFootprint(0.0, "[[1, 2], [3, 4], [5, 6]]");
std::vector<geometry_msgs::msg::Point> footprint = footprint_tester_->getRobotFootprint();
EXPECT_EQ(3u, footprint.size());
EXPECT_EQ(1.0f, footprint[0].x);
EXPECT_EQ(2.0f, footprint[0].y);
EXPECT_EQ(0.0f, footprint[0].z);
EXPECT_EQ(3.0f, footprint[1].x);
EXPECT_EQ(4.0f, footprint[1].y);
EXPECT_EQ(0.0f, footprint[1].z);
EXPECT_EQ(5.0f, footprint[2].x);
EXPECT_EQ(6.0f, footprint[2].y);
EXPECT_EQ(0.0f, footprint[2].z);
}
navigation2/nav2_costmap_2d/test/integration/inflation_tests.cpp
+644
View File
@@ -0,0 +1,644 @@
/*
* 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 InflationLayer for Costmap2D
*/
#include <gtest/gtest.h>
#include <cmath>
#include <map>
#include <memory>
#include <string>
#include <vector>
#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/inflation_layer.hpp"
#include "nav2_costmap_2d/observation_buffer.hpp"
#include "../testing_helper.hpp"
#include "nav2_util/node_utils.hpp"
#include "nav2_costmap_2d/costmap_2d_ros.hpp"
using geometry_msgs::msg::Point;
using nav2_costmap_2d::CellData;
class RclCppFixture
{
public:
RclCppFixture() {rclcpp::init(0, nullptr);}
~RclCppFixture() {rclcpp::shutdown();}
};
RclCppFixture g_rclcppfixture;
class TestNode : public ::testing::Test
{
public:
TestNode() {}
~TestNode() {}
std::vector<Point> setRadii(
nav2_costmap_2d::LayeredCostmap & layers,
double length, double width);
void validatePointInflation(
unsigned int mx, unsigned int my,
nav2_costmap_2d::Costmap2D * costmap,
std::shared_ptr<nav2_costmap_2d::InflationLayer> & ilayer,
double inflation_radius);
void initNode(std::vector<rclcpp::Parameter> parameters);
void initNode(double inflation_radius);
void waitForMap(std::shared_ptr<nav2_costmap_2d::StaticLayer> & slayer);
protected:
nav2_util::LifecycleNode::SharedPtr node_;
};
std::vector<Point> TestNode::setRadii(
nav2_costmap_2d::LayeredCostmap & layers,
double length, double width)
{
std::vector<Point> polygon;
Point p;
p.x = width;
p.y = length;
polygon.push_back(p);
p.x = width;
p.y = -length;
polygon.push_back(p);
p.x = -width;
p.y = -length;
polygon.push_back(p);
p.x = -width;
p.y = length;
polygon.push_back(p);
layers.setFootprint(polygon);
return polygon;
}
void TestNode::waitForMap(std::shared_ptr<nav2_costmap_2d::StaticLayer> & slayer)
{
while (!slayer->isCurrent()) {
rclcpp::spin_some(node_->get_node_base_interface());
}
}
// Test that a single point gets inflated properly
void TestNode::validatePointInflation(
unsigned int mx, unsigned int my,
nav2_costmap_2d::Costmap2D * costmap,
std::shared_ptr<nav2_costmap_2d::InflationLayer> & ilayer,
double inflation_radius)
{
bool * seen = new bool[costmap->getSizeInCellsX() * costmap->getSizeInCellsY()];
memset(seen, false, costmap->getSizeInCellsX() * costmap->getSizeInCellsY() * sizeof(bool));
std::map<double, std::vector<CellData>> m;
CellData initial(costmap->getIndex(mx, my), mx, my, mx, my);
m[0].push_back(initial);
for (std::map<double, std::vector<CellData>>::iterator bin = m.begin();
bin != m.end(); ++bin)
{
for (unsigned int i = 0; i < bin->second.size(); ++i) {
const CellData cell = bin->second[i];
if (!seen[cell.index_]) {
seen[cell.index_] = true;
unsigned int dx = (cell.x_ > cell.src_x_) ? cell.x_ - cell.src_x_ : cell.src_x_ - cell.x_;
unsigned int dy = (cell.y_ > cell.src_y_) ? cell.y_ - cell.src_y_ : cell.src_y_ - cell.y_;
double dist = std::hypot(dx, dy);
unsigned char expected_cost = ilayer->computeCost(dist);
ASSERT_TRUE(costmap->getCost(cell.x_, cell.y_) >= expected_cost);
if (dist > inflation_radius) {
continue;
}
if (dist == bin->first) {
// Adding to our current bin could cause a reallocation
// Which appears to cause the iterator to get messed up
dist += 0.001;
}
if (cell.x_ > 0) {
CellData data(costmap->getIndex(cell.x_ - 1, cell.y_),
cell.x_ - 1, cell.y_, cell.src_x_, cell.src_y_);
m[dist].push_back(data);
}
if (cell.y_ > 0) {
CellData data(costmap->getIndex(cell.x_, cell.y_ - 1),
cell.x_, cell.y_ - 1, cell.src_x_, cell.src_y_);
m[dist].push_back(data);
}
if (cell.x_ < costmap->getSizeInCellsX() - 1) {
CellData data(costmap->getIndex(cell.x_ + 1, cell.y_),
cell.x_ + 1, cell.y_, cell.src_x_, cell.src_y_);
m[dist].push_back(data);
}
if (cell.y_ < costmap->getSizeInCellsY() - 1) {
CellData data(costmap->getIndex(cell.x_, cell.y_ + 1),
cell.x_, cell.y_ + 1, cell.src_x_, cell.src_y_);
m[dist].push_back(data);
}
}
}
}
delete[] seen;
}
void TestNode::initNode(std::vector<rclcpp::Parameter> parameters)
{
auto options = rclcpp::NodeOptions();
options.parameter_overrides(parameters);
node_ = std::make_shared<nav2_util::LifecycleNode>(
"inflation_test_node", "", 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<unsigned char>(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("")));
}
void TestNode::initNode(double inflation_radius)
{
std::vector<rclcpp::Parameter> 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));
initNode(parameters);
}
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<Point> polygon = setRadii(layers, 2.1, 2.3);
std::shared_ptr<nav2_costmap_2d::ObstacleLayer> olayer = nullptr;
addObstacleLayer(layers, tf, node_, olayer);
std::shared_ptr<nav2_costmap_2d::InflationLayer> ilayer = nullptr;
addInflationLayer(layers, tf, node_, ilayer);
layers.setFootprint(polygon);
addObservation(olayer, 0, 0, MAX_Z);
layers.updateMap(0, 0, 0);
nav2_costmap_2d::Costmap2D * costmap = layers.getCostmap();
// printMap(*costmap);
EXPECT_EQ(nav2_costmap_2d::LETHAL_OBSTACLE, costmap->getCost(0, 0));
EXPECT_EQ(nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE, costmap->getCost(1, 0));
EXPECT_EQ(nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE, costmap->getCost(2, 0));
EXPECT_TRUE(nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE > costmap->getCost(3, 0));
EXPECT_TRUE(nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE > costmap->getCost(2, 1));
EXPECT_EQ(nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE, costmap->getCost(1, 1));
}
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<Point> polygon = setRadii(layers, 2.1, 2.3);
std::shared_ptr<nav2_costmap_2d::ObstacleLayer> olayer = nullptr;
addObstacleLayer(layers, tf, node_, olayer);
std::shared_ptr<nav2_costmap_2d::InflationLayer> ilayer = nullptr;
addInflationLayer(layers, tf, node_, ilayer);
layers.setFootprint(polygon);
addObservation(olayer, 0, 0, MAX_Z);
layers.updateMap(0, 0, 0);
nav2_costmap_2d::Costmap2D * costmap = layers.getCostmap();
EXPECT_EQ(countValues(*costmap, nav2_costmap_2d::NO_INFORMATION), 0u);
}
TEST_F(TestNode, testInflationInUnkown)
{
std::vector<rclcpp::Parameter> 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", 4.1));
parameters.push_back(rclcpp::Parameter("inflation.inflate_unknown", true));
initNode(parameters);
node_->set_parameter(rclcpp::Parameter("track_unknown_space", true));
tf2_ros::Buffer tf(node_->get_clock());
nav2_costmap_2d::LayeredCostmap layers("frame", false, true);
layers.resizeMap(9, 9, 1, 0, 0);
// Footprint with inscribed radius = 2.1
// circumscribed radius = 3.1
std::vector<Point> polygon = setRadii(layers, 2.1, 2.3);
std::shared_ptr<nav2_costmap_2d::ObstacleLayer> olayer = nullptr;
addObstacleLayer(layers, tf, node_, olayer);
std::shared_ptr<nav2_costmap_2d::InflationLayer> ilayer = nullptr;
addInflationLayer(layers, tf, node_, ilayer);
layers.setFootprint(polygon);
addObservation(olayer, 4, 4, MAX_Z, 0.0, 0.0, MAX_Z, true, false);
layers.updateMap(0, 0, 0);
nav2_costmap_2d::Costmap2D * costmap = layers.getCostmap();
// Only the 4 corners of the map should remain unknown
EXPECT_EQ(countValues(*costmap, nav2_costmap_2d::NO_INFORMATION), 4u);
}
TEST_F(TestNode, testInflationAroundUnkown)
{
auto inflation_radius = 4.1;
std::vector<rclcpp::Parameter> 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));
parameters.push_back(rclcpp::Parameter("inflation.inflate_around_unknown", true));
initNode(parameters);
node_->set_parameter(rclcpp::Parameter("track_unknown_space", true));
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<Point> polygon = setRadii(layers, 2.1, 2.3);
std::shared_ptr<nav2_costmap_2d::InflationLayer> ilayer = nullptr;
addInflationLayer(layers, tf, node_, ilayer);
layers.setFootprint(polygon);
layers.updateMap(0, 0, 0);
layers.getCostmap()->setCost(4, 4, nav2_costmap_2d::NO_INFORMATION);
ilayer->updateCosts(*layers.getCostmap(), 0, 0, 10, 10);
validatePointInflation(4, 4, layers.getCostmap(), ilayer, inflation_radius);
}
/**
* Test for the cost function correctness with a larger range and different values
*/
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);
// Footprint with inscribed radius = 5.0
// circumscribed radius = 8.0
std::vector<Point> polygon = setRadii(layers, 5.0, 6.25);
std::shared_ptr<nav2_costmap_2d::ObstacleLayer> olayer = nullptr;
addObstacleLayer(layers, tf, node_, olayer);
std::shared_ptr<nav2_costmap_2d::InflationLayer> ilayer = nullptr;
addInflationLayer(layers, tf, node_, ilayer);
layers.setFootprint(polygon);
addObservation(olayer, 50, 50, MAX_Z);
layers.updateMap(0, 0, 0);
nav2_costmap_2d::Costmap2D * map = layers.getCostmap();
// Verify that the circumscribed cost lower bound is as expected: based on the cost function.
// unsigned char c = ilayer->computeCost(8.0);
// ASSERT_EQ(ilayer->getCircumscribedCost(), c);
for (unsigned int i = 0; i <= (unsigned int)ceil(5.0); i++) {
// To the right
ASSERT_EQ(map->getCost(50 + i, 50) >= nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
ASSERT_EQ(map->getCost(50 + i, 50) >= nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
// To the left
ASSERT_EQ(map->getCost(50 - i, 50) >= nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
ASSERT_EQ(map->getCost(50 - i, 50) >= nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
// Down
ASSERT_EQ(map->getCost(50, 50 + i) >= nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
ASSERT_EQ(map->getCost(50, 50 + i) >= nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
// Up
ASSERT_EQ(map->getCost(50, 50 - i) >= nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
ASSERT_EQ(map->getCost(50, 50 - i) >= nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
}
// Verify the normalized cost attenuates as expected
for (unsigned int i = (unsigned int)(ceil(5.0) + 1); i <= (unsigned int)ceil(10.5); i++) {
unsigned char expectedValue = ilayer->computeCost(i / 1.0);
ASSERT_EQ(map->getCost(50 + i, 50), expectedValue);
}
// Update with no hits. Should clear (revert to the static map
/*map->resetMapOutsideWindow(0, 0, 0.0, 0.0);
cloud.points.resize(0);
p.x = 0.0;
p.y = 0.0;
p.z = MAX_Z;
Observation obs2(p, cloud, 100.0, 100.0);
std::vector<Observation> obsBuf2;
obsBuf2.push_back(obs2);
map->updateWorld(0, 0, obsBuf2, obsBuf2);
for(unsigned int i = 0; i < 100; i++)
for(unsigned int j = 0; j < 100; j++)
ASSERT_EQ(map->getCost(i, j), nav2_costmap_2d::FREE_SPACE);*/
}
/**
* Test that there is no regression and that costs do not get
* underestimated with the distance-as-key map used to replace
* the previously used priority queue. This is a more thorough
* test of the cost function being correctly applied.
*/
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
std::vector<Point> polygon = setRadii(layers, 2.1, 2.3);
std::shared_ptr<nav2_costmap_2d::ObstacleLayer> olayer = nullptr;
addObstacleLayer(layers, tf, node_, olayer);
std::shared_ptr<nav2_costmap_2d::InflationLayer> ilayer = nullptr;
addInflationLayer(layers, tf, node_, ilayer);
layers.setFootprint(polygon);
// Add two diagonal cells, they would induce problems under the
// previous implementations
addObservation(olayer, 4, 4, MAX_Z);
addObservation(olayer, 5, 5, MAX_Z);
layers.updateMap(0, 0, 0);
validatePointInflation(4, 4, layers.getCostmap(), ilayer, inflation_radius);
validatePointInflation(5, 5, layers.getCostmap(), ilayer, inflation_radius);
}
/**
* Test inflation for both static and dynamic obstacles
*/
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<Point> polygon = setRadii(layers, 1, 1);
std::shared_ptr<nav2_costmap_2d::StaticLayer> slayer = nullptr;
addStaticLayer(layers, tf, node_, slayer);
std::shared_ptr<nav2_costmap_2d::ObstacleLayer> olayer = nullptr;
addObstacleLayer(layers, tf, node_, olayer);
std::shared_ptr<nav2_costmap_2d::InflationLayer> ilayer = nullptr;
addInflationLayer(layers, tf, node_, ilayer);
layers.setFootprint(polygon);
nav2_costmap_2d::Costmap2D * costmap = layers.getCostmap();
waitForMap(slayer);
layers.updateMap(0, 0, 0);
// printMap(*costmap);
ASSERT_EQ(countValues(*costmap, nav2_costmap_2d::LETHAL_OBSTACLE), 20u);
ASSERT_EQ(countValues(*costmap, nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE), 28u);
/*/ Iterate over all id's and verify they are obstacles
for(std::vector<unsigned int>::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.getCost(x, y) == nav2_costmap_2d::LETHAL_OBSTACLE ||
map.getCost(x, y) == nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
}*/
addObservation(olayer, 0, 0, 0.4);
layers.updateMap(0, 0, 0);
// It and its 2 neighbors makes 3 obstacles
ASSERT_EQ(
countValues(*costmap, nav2_costmap_2d::LETHAL_OBSTACLE) +
countValues(*costmap, nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE), 51u);
// @todo Rewrite
// Add an obstacle at <2,0> which will inflate and refresh to of the other inflated cells
addObservation(olayer, 2, 0);
layers.updateMap(0, 0, 0);
// Now we expect insertions for it, and 2 more neighbors, but not all 5.
// Free space will propagate from
// the origin to the target, clearing the point at <0, 0>,
// but not over-writing the inflation of the obstacle
// at <0, 1>
ASSERT_EQ(
countValues(*costmap, nav2_costmap_2d::LETHAL_OBSTACLE) +
countValues(*costmap, nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE), 54u);
// Add an obstacle at <1, 9>. This will inflate obstacles around it
addObservation(olayer, 1, 9);
layers.updateMap(0, 0, 0);
ASSERT_EQ(costmap->getCost(1, 9), nav2_costmap_2d::LETHAL_OBSTACLE);
ASSERT_EQ(costmap->getCost(0, 9), nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE);
ASSERT_EQ(costmap->getCost(2, 9), nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE);
// Add an obstacle and verify that it over-writes its inflated status
addObservation(olayer, 0, 9);
layers.updateMap(0, 0, 0);
ASSERT_EQ(costmap->getCost(0, 9), nav2_costmap_2d::LETHAL_OBSTACLE);
}
/**
* Test specific inflation scenario to ensure we do not set inflated obstacles to be raw obstacles.
*/
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<Point> polygon = setRadii(layers, 1, 1);
std::shared_ptr<nav2_costmap_2d::StaticLayer> slayer = nullptr;
addStaticLayer(layers, tf, node_, slayer);
std::shared_ptr<nav2_costmap_2d::ObstacleLayer> olayer = nullptr;
addObstacleLayer(layers, tf, node_, olayer);
std::shared_ptr<nav2_costmap_2d::InflationLayer> ilayer = nullptr;
addInflationLayer(layers, tf, node_, ilayer);
layers.setFootprint(polygon);
waitForMap(slayer);
// Creat a small L-Shape all at once
addObservation(olayer, 1, 1, MAX_Z);
addObservation(olayer, 2, 1, MAX_Z);
addObservation(olayer, 2, 2, MAX_Z);
layers.updateMap(0, 0, 0);
nav2_costmap_2d::Costmap2D * costmap = layers.getCostmap();
// printMap(*costmap);
ASSERT_EQ(costmap->getCost(2, 3), nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE);
ASSERT_EQ(costmap->getCost(3, 3), nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE);
}
/**
* Test inflation behavior, starting with an empty map
*/
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<Point> polygon = setRadii(layers, 1, 1.75);
std::shared_ptr<nav2_costmap_2d::ObstacleLayer> olayer = nullptr;
addObstacleLayer(layers, tf, node_, olayer);
std::shared_ptr<nav2_costmap_2d::InflationLayer> ilayer = nullptr;
addInflationLayer(layers, tf, node_, ilayer);
layers.setFootprint(polygon);
// There should be no occupied cells
nav2_costmap_2d::Costmap2D * costmap = layers.getCostmap();
ASSERT_EQ(countValues(*costmap, nav2_costmap_2d::LETHAL_OBSTACLE), 0u);
ASSERT_EQ(countValues(*costmap, nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE), 0u);
printMap(*costmap);
// Add an obstacle at 5,5
addObservation(olayer, 5, 5, MAX_Z);
layers.updateMap(0, 0, 0);
printMap(*costmap);
// Test fails because updated cell value is 0
ASSERT_EQ(countValues(*costmap, nav2_costmap_2d::FREE_SPACE, false), 29u);
ASSERT_EQ(countValues(*costmap, nav2_costmap_2d::LETHAL_OBSTACLE), 1u);
ASSERT_EQ(countValues(*costmap, nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE), 4u);
// Update again - should see no change
layers.updateMap(0, 0, 0);
ASSERT_EQ(countValues(*costmap, nav2_costmap_2d::FREE_SPACE, false), 29u);
ASSERT_EQ(countValues(*costmap, nav2_costmap_2d::LETHAL_OBSTACLE), 1u);
ASSERT_EQ(countValues(*costmap, nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE), 4u);
}
/**
* Test dynamic parameter setting of inflation layer
*/
TEST_F(TestNode, testDynParamsSet)
{
auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>("test_costmap");
costmap->set_parameter(rclcpp::Parameter("global_frame", std::string("base_link")));
costmap->on_configure(rclcpp_lifecycle::State());
costmap->on_activate(rclcpp_lifecycle::State());
auto parameter_client = std::make_shared<rclcpp::AsyncParametersClient>(
costmap->get_node_base_interface(), costmap->get_node_topics_interface(),
costmap->get_node_graph_interface(),
costmap->get_node_services_interface());
auto results = parameter_client->set_parameters_atomically(
{
rclcpp::Parameter("inflation_layer.inflation_radius", 0.0),
rclcpp::Parameter("inflation_layer.cost_scaling_factor", 0.0),
rclcpp::Parameter("inflation_layer.inflate_unknown", true),
rclcpp::Parameter("inflation_layer.inflate_around_unknown", true),
rclcpp::Parameter("inflation_layer.enabled", false)
});
rclcpp::spin_until_future_complete(
costmap->get_node_base_interface(),
results);
EXPECT_EQ(costmap->get_parameter("inflation_layer.inflation_radius").as_double(), 0.0);
EXPECT_EQ(costmap->get_parameter("inflation_layer.cost_scaling_factor").as_double(), 0.0);
EXPECT_EQ(costmap->get_parameter("inflation_layer.inflate_unknown").as_bool(), true);
EXPECT_EQ(costmap->get_parameter("inflation_layer.inflate_around_unknown").as_bool(), true);
EXPECT_EQ(costmap->get_parameter("inflation_layer.enabled").as_bool(), false);
costmap->on_deactivate(rclcpp_lifecycle::State());
costmap->on_cleanup(rclcpp_lifecycle::State());
costmap->on_shutdown(rclcpp_lifecycle::State());
}
navigation2/nav2_costmap_2d/test/integration/obstacle_tests.cpp
+556
View File
@@ -0,0 +1,556 @@
/*
* 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 ObstacleLayer for Costmap2D
*/
#include <memory>
#include <string>
#include <algorithm>
#include <utility>
#include "gtest/gtest.h"
#include "nav2_costmap_2d/costmap_2d.hpp"
#include "nav2_costmap_2d/layered_costmap.hpp"
#include "nav2_costmap_2d/observation_buffer.hpp"
#include "../testing_helper.hpp"
#include "nav2_costmap_2d/costmap_2d_ros.hpp"
using std::begin;
using std::end;
using std::for_each;
using std::all_of;
using std::none_of;
using std::pair;
using std::string;
class RclCppFixture
{
public:
RclCppFixture() {rclcpp::init(0, nullptr);}
~RclCppFixture() {rclcpp::shutdown();}
};
RclCppFixture g_rclcppfixture;
class TestLifecycleNode : public nav2_util::LifecycleNode
{
public:
explicit TestLifecycleNode(const string & name)
: nav2_util::LifecycleNode(name)
{
}
nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State &)
{
return nav2_util::CallbackReturn::SUCCESS;
}
nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State &)
{
return nav2_util::CallbackReturn::SUCCESS;
}
nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State &)
{
return nav2_util::CallbackReturn::SUCCESS;
}
nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State &)
{
return nav2_util::CallbackReturn::SUCCESS;
}
nav2_util::CallbackReturn onShutdown(const rclcpp_lifecycle::State &)
{
return nav2_util::CallbackReturn::SUCCESS;
}
nav2_util::CallbackReturn onError(const rclcpp_lifecycle::State &)
{
return nav2_util::CallbackReturn::SUCCESS;
}
};
class TestNode : public ::testing::Test
{
public:
TestNode()
{
node_ = std::make_shared<TestLifecycleNode>("obstacle_test_node");
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<unsigned char>(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() {}
protected:
std::shared_ptr<TestLifecycleNode> node_;
};
/*
* For reference, the static map looks like this:
*
* 0 0 0 0 0 0 0 254 254 254
*
* 0 0 0 0 0 0 0 254 254 254
*
* 0 0 0 254 254 254 0 0 0 0
*
* 0 0 0 0 0 0 0 0 0 0
*
* 0 0 0 0 0 0 0 0 0 0
*
* 0 0 0 0 254 0 0 254 254 254
*
* 0 0 0 0 254 0 0 254 254 254
*
* 0 0 0 0 0 0 0 254 254 254
*
* 0 0 0 0 0 0 0 0 0 0
*
* 0 0 0 0 0 0 0 0 0 0
*
* upper left is 0,0, lower right is 9,9
*/
#if (0)
/**
* Test for ray tracing free space
*/
TEST_F(TestNode, testRaytracing) {
tf2_ros::Buffer tf(node_->get_clock());
nav2_costmap_2d::LayeredCostmap layers("frame", false, false);
addStaticLayer(layers, tf, node_);
auto olayer = addObstacleLayer(layers, tf, node_);
// Add a point at 0, 0, 0
addObservation(olayer, 0.0, 0.0, MAX_Z / 2, 0, 0, MAX_Z / 2);
// This actually puts the LETHAL (254) point in the costmap at (0,0)
layers.updateMap(0, 0, 0); // 0, 0, 0 is robot pose
// printMap(*(layers.getCostmap()));
int lethal_count = countValues(*(layers.getCostmap()), nav2_costmap_2d::LETHAL_OBSTACLE);
// We expect just one obstacle to be added (20 in static map)
ASSERT_EQ(lethal_count, 21);
}
/**
* Test for ray tracing free space
*/
TEST_F(TestNode, testRaytracing2) {
tf2_ros::Buffer tf(node_->get_clock());
nav2_costmap_2d::LayeredCostmap layers("frame", false, false);
addStaticLayer(layers, tf, node_);
auto olayer = addObstacleLayer(layers, tf, node_);
// If we print map now, it is 10x10 all value 0
// printMap(*(layers.getCostmap()));
// Update will fill in the costmap with the static map
layers.updateMap(0, 0, 0);
// If we print the map now, we get the static map
// printMap(*(layers.getCostmap()));
// Static map has 20 LETHAL cells (see diagram above)
int obs_before = countValues(*(layers.getCostmap()), nav2_costmap_2d::LETHAL_OBSTACLE);
ASSERT_EQ(obs_before, 20);
// The sensor origin will be <0,0>. So if we add an obstacle at 9,9,
// we would expect cells <0, 0> thru <8, 8> to be traced through
// however the static map is not cleared by obstacle layer
addObservation(olayer, 9.5, 9.5, MAX_Z / 2, 0.5, 0.5, MAX_Z / 2);
layers.updateMap(0, 0, 0);
// If we print map now, we have static map + <9,9> is LETHAL
// printMap(*(layers.getCostmap()));
int obs_after = countValues(*(layers.getCostmap()), nav2_costmap_2d::LETHAL_OBSTACLE);
// Change from previous test:
// No obstacles from the static map will be cleared, so the
// net change is +1.
ASSERT_EQ(obs_after, obs_before + 1);
// Fill in the diagonal, <7,7> and <9,9> already filled in, <0,0> is robot
for (int i = 0; i < olayer->getSizeInCellsY(); ++i) {
olayer->setCost(i, i, nav2_costmap_2d::LETHAL_OBSTACLE);
}
// This will updateBounds, which will raytrace the static observation added
// above, thus clearing out the diagonal again!
layers.updateMap(0, 0, 0);
// Map now has diagonal except <0,0> filled with LETHAL (254)
// printMap(*(layers.getCostmap()));
int with_static = countValues(*(layers.getCostmap()), nav2_costmap_2d::LETHAL_OBSTACLE);
// Should thus be the same
ASSERT_EQ(with_static, obs_after);
// If 21 are filled, 79 should be free
ASSERT_EQ(79, countValues(*(layers.getCostmap()), nav2_costmap_2d::FREE_SPACE));
}
/**
* Test for wave interference
*/
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);
auto olayer = addObstacleLayer(layers, tf, node_);
// If we print map now, it is 10x10, all cells are 255 (NO_INFORMATION)
// printMap(*(layers.getCostmap()));
// Lay out 3 obstacles in a line - along the diagonal, separated by a cell.
addObservation(olayer, 3.0, 3.0, MAX_Z);
addObservation(olayer, 5.0, 5.0, MAX_Z);
addObservation(olayer, 7.0, 7.0, MAX_Z);
layers.updateMap(0, 0, 0);
nav2_costmap_2d::Costmap2D * costmap = layers.getCostmap();
// 3 obstacle cells are filled, <1,1>,<2,2>,<4,4> and <6,6> are now free
// <0,0> is footprint and is free
// printMap(*costmap);
ASSERT_EQ(3, countValues(*costmap, nav2_costmap_2d::LETHAL_OBSTACLE));
ASSERT_EQ(92, countValues(*costmap, nav2_costmap_2d::NO_INFORMATION));
ASSERT_EQ(5, countValues(*costmap, nav2_costmap_2d::FREE_SPACE));
}
/**
* Make sure we ignore points outside of our z threshold
*/
TEST_F(TestNode, testZThreshold) {
tf2_ros::Buffer tf(node_->get_clock());
// Start with an empty map
nav2_costmap_2d::LayeredCostmap layers("frame", false, true);
layers.resizeMap(10, 10, 1, 0, 0);
auto olayer = addObstacleLayer(layers, tf, node_);
// A point cloud with 2 points falling in a cell with a non-lethal cost
addObservation(olayer, 0.0, 5.0, 0.4);
addObservation(olayer, 1.0, 5.0, 2.2);
layers.updateMap(0, 0, 0);
nav2_costmap_2d::Costmap2D * costmap = layers.getCostmap();
ASSERT_EQ(countValues(*costmap, nav2_costmap_2d::LETHAL_OBSTACLE), 1);
}
/**
* Verify that dynamic obstacles are added
*/
TEST_F(TestNode, testDynamicObstacles) {
tf2_ros::Buffer tf(node_->get_clock());
nav2_costmap_2d::LayeredCostmap layers("frame", false, false);
addStaticLayer(layers, tf, node_);
auto olayer = addObstacleLayer(layers, tf, node_);
// Add a point cloud and verify its insertion. There should be only one new one
addObservation(olayer, 0.0, 0.0);
addObservation(olayer, 0.0, 0.0);
addObservation(olayer, 0.0, 0.0);
layers.updateMap(0, 0, 0);
nav2_costmap_2d::Costmap2D * costmap = layers.getCostmap();
// Should now have 1 insertion and no deletions
ASSERT_EQ(countValues(*costmap, nav2_costmap_2d::LETHAL_OBSTACLE), 21);
// Repeating the call - we should see no insertions or deletions
ASSERT_EQ(countValues(*costmap, nav2_costmap_2d::LETHAL_OBSTACLE), 21);
}
/**
* Verify that if we add a point that is already a static obstacle we do not end up with a new ostacle
*/
TEST_F(TestNode, testMultipleAdditions) {
tf2_ros::Buffer tf(node_->get_clock());
nav2_costmap_2d::LayeredCostmap layers("frame", false, false);
addStaticLayer(layers, tf, node_);
auto olayer = addObstacleLayer(layers, tf, node_);
// A point cloud with one point that falls within an existing obstacle
addObservation(olayer, 9.5, 0.0);
layers.updateMap(0, 0, 0);
nav2_costmap_2d::Costmap2D * costmap = layers.getCostmap();
// printMap(*costmap);
ASSERT_EQ(countValues(*costmap, nav2_costmap_2d::LETHAL_OBSTACLE), 20);
}
#endif
/**
* Verify correct init/reset cycling of layer
*/
TEST_F(TestNode, testRepeatedResets) {
tf2_ros::Buffer tf(node_->get_clock());
nav2_costmap_2d::LayeredCostmap layers("frame", false, false);
std::shared_ptr<nav2_costmap_2d::StaticLayer> slayer = nullptr;
addStaticLayer(layers, tf, node_, slayer);
// TODO(orduno) Add obstacle layer
// Define a node-level parameter
pair<string, string> node_dummy = {"node_dummy_param", "node_dummy_val"};
node_->declare_parameter(node_dummy.first, rclcpp::ParameterValue(node_dummy.second));
// Define a layer-level parameter
pair<string, string> layer_dummy = {"dummy_param", "dummy_val"};
// Set parameters
auto plugins = layers.getPlugins();
for_each(
begin(*plugins), end(*plugins), [&layer_dummy](const auto & plugin) {
string layer_param = layer_dummy.first + "_" + plugin->getName();
// Notice we are using Layer::declareParameter
plugin->declareParameter(layer_param, rclcpp::ParameterValue(layer_dummy.second));
});
// Check that all parameters have been set
// node-level param
ASSERT_TRUE(node_->has_parameter(node_dummy.first));
// layer-level param
ASSERT_TRUE(
all_of(
begin(*plugins), end(*plugins), [&layer_dummy](const auto & plugin) {
string layer_param = layer_dummy.first + "_" + plugin->getName();
return plugin->hasParameter(layer_param);
}));
// Reset all layers. Parameters should be declared if not declared, otherwise skipped.
ASSERT_NO_THROW(
for_each(
begin(*plugins), end(*plugins), [](const auto & plugin) {
plugin->reset();
}));
}
/**
* Test for ray tracing free space
*/
TEST_F(TestNode, testRaytracing) {
tf2_ros::Buffer tf(node_->get_clock());
nav2_costmap_2d::LayeredCostmap layers("frame", false, false);
layers.resizeMap(10, 10, 1, 0, 0);
std::shared_ptr<nav2_costmap_2d::StaticLayer> slayer = nullptr;
addStaticLayer(layers, tf, node_, slayer);
std::shared_ptr<nav2_costmap_2d::ObstacleLayer> olayer = nullptr;
addObstacleLayer(layers, tf, node_, olayer);
addObservation(olayer, 0.0, 0.0, MAX_Z / 2, 0, 0, MAX_Z / 2);
// This actually puts the LETHAL (254) point in the costmap at (0,0)
layers.updateMap(0, 0, 0); // 0, 0, 0 is robot pose
// printMap(*(layers.getCostmap()));
int lethal_count = countValues(*(layers.getCostmap()), nav2_costmap_2d::LETHAL_OBSTACLE);
ASSERT_EQ(lethal_count, 1);
addObservation(olayer, 1.0, 1.0, MAX_Z / 2, 0, 0, MAX_Z / 2, true, true, 100.0, 5.0, 100.0, 5.0);
// This actually puts the LETHAL (254) point in the costmap at (0,0)
layers.updateMap(0, 0, 0); // 0, 0, 0 is robot pose
// printMap(*(layers.getCostmap()));
// New observation should not be recorded as min_range is higher than obstacle range
lethal_count = countValues(*(layers.getCostmap()), nav2_costmap_2d::LETHAL_OBSTACLE);
ASSERT_EQ(lethal_count, 1);
}
/**
* Test dynamic parameter setting of obstacle layer
*/
TEST_F(TestNode, testDynParamsSetObstacle)
{
auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>("test_costmap");
// Add obstacle layer
std::vector<std::string> plugins_str;
plugins_str.push_back("obstacle_layer");
costmap->set_parameter(rclcpp::Parameter("plugins", plugins_str));
costmap->declare_parameter(
"obstacle_layer.plugin",
rclcpp::ParameterValue(std::string("nav2_costmap_2d::ObstacleLayer")));
costmap->set_parameter(rclcpp::Parameter("global_frame", std::string("base_link")));
costmap->on_configure(rclcpp_lifecycle::State());
costmap->on_activate(rclcpp_lifecycle::State());
auto parameter_client = std::make_shared<rclcpp::AsyncParametersClient>(
costmap->get_node_base_interface(), costmap->get_node_topics_interface(),
costmap->get_node_graph_interface(),
costmap->get_node_services_interface());
auto results = parameter_client->set_parameters_atomically(
{
rclcpp::Parameter("obstacle_layer.combination_method", 5),
rclcpp::Parameter("obstacle_layer.max_obstacle_height", 4.0),
rclcpp::Parameter("obstacle_layer.enabled", false),
rclcpp::Parameter("obstacle_layer.footprint_clearing_enabled", false)
});
rclcpp::spin_until_future_complete(
costmap->get_node_base_interface(),
results);
EXPECT_EQ(costmap->get_parameter("obstacle_layer.combination_method").as_int(), 5);
EXPECT_EQ(costmap->get_parameter("obstacle_layer.max_obstacle_height").as_double(), 4.0);
EXPECT_EQ(costmap->get_parameter("obstacle_layer.enabled").as_bool(), false);
EXPECT_EQ(costmap->get_parameter("obstacle_layer.footprint_clearing_enabled").as_bool(), false);
costmap->on_deactivate(rclcpp_lifecycle::State());
costmap->on_cleanup(rclcpp_lifecycle::State());
costmap->on_shutdown(rclcpp_lifecycle::State());
}
/**
* Test dynamic parameter setting of voxel layer
*/
TEST_F(TestNode, testDynParamsSetVoxel)
{
auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>("test_costmap");
// Add voxel layer
std::vector<std::string> plugins_str;
plugins_str.push_back("voxel_layer");
costmap->set_parameter(rclcpp::Parameter("plugins", plugins_str));
costmap->declare_parameter(
"voxel_layer.plugin",
rclcpp::ParameterValue(std::string("nav2_costmap_2d::VoxelLayer")));
costmap->set_parameter(rclcpp::Parameter("global_frame", std::string("base_link")));
costmap->on_configure(rclcpp_lifecycle::State());
costmap->on_activate(rclcpp_lifecycle::State());
auto parameter_client = std::make_shared<rclcpp::AsyncParametersClient>(
costmap->get_node_base_interface(), costmap->get_node_topics_interface(),
costmap->get_node_graph_interface(),
costmap->get_node_services_interface());
auto results = parameter_client->set_parameters_atomically(
{
rclcpp::Parameter("voxel_layer.combination_method", 0),
rclcpp::Parameter("voxel_layer.mark_threshold", 1),
rclcpp::Parameter("voxel_layer.unknown_threshold", 10),
rclcpp::Parameter("voxel_layer.z_resolution", 0.4),
rclcpp::Parameter("voxel_layer.origin_z", 1.0),
rclcpp::Parameter("voxel_layer.z_voxels", 14),
rclcpp::Parameter("voxel_layer.max_obstacle_height", 4.0),
rclcpp::Parameter("voxel_layer.footprint_clearing_enabled", false),
rclcpp::Parameter("voxel_layer.enabled", false),
rclcpp::Parameter("voxel_layer.publish_voxel_map", true)
});
rclcpp::spin_until_future_complete(
costmap->get_node_base_interface(),
results);
EXPECT_EQ(costmap->get_parameter("voxel_layer.combination_method").as_int(), 0);
EXPECT_EQ(costmap->get_parameter("voxel_layer.mark_threshold").as_int(), 1);
EXPECT_EQ(costmap->get_parameter("voxel_layer.unknown_threshold").as_int(), 10);
EXPECT_EQ(costmap->get_parameter("voxel_layer.z_resolution").as_double(), 0.4);
EXPECT_EQ(costmap->get_parameter("voxel_layer.origin_z").as_double(), 1.0);
EXPECT_EQ(costmap->get_parameter("voxel_layer.z_voxels").as_int(), 14);
EXPECT_EQ(costmap->get_parameter("voxel_layer.max_obstacle_height").as_double(), 4.0);
EXPECT_EQ(costmap->get_parameter("voxel_layer.footprint_clearing_enabled").as_bool(), false);
EXPECT_EQ(costmap->get_parameter("voxel_layer.enabled").as_bool(), false);
EXPECT_EQ(costmap->get_parameter("voxel_layer.publish_voxel_map").as_bool(), true);
costmap->on_deactivate(rclcpp_lifecycle::State());
costmap->on_cleanup(rclcpp_lifecycle::State());
costmap->on_shutdown(rclcpp_lifecycle::State());
}
/**
* Test dynamic parameter setting of static layer
*/
TEST_F(TestNode, testDynParamsSetStatic)
{
auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>("test_costmap");
costmap->set_parameter(rclcpp::Parameter("global_frame", std::string("base_link")));
costmap->on_configure(rclcpp_lifecycle::State());
costmap->on_activate(rclcpp_lifecycle::State());
auto parameter_client = std::make_shared<rclcpp::AsyncParametersClient>(
costmap->get_node_base_interface(), costmap->get_node_topics_interface(),
costmap->get_node_graph_interface(),
costmap->get_node_services_interface());
auto results = parameter_client->set_parameters_atomically(
{
rclcpp::Parameter("static_layer.transform_tolerance", 1.0),
rclcpp::Parameter("static_layer.enabled", false),
rclcpp::Parameter("static_layer.map_subscribe_transient_local", false),
rclcpp::Parameter("static_layer.map_topic", "dynamic_topic"),
rclcpp::Parameter("static_layer.subscribe_to_updates", true)
});
rclcpp::spin_until_future_complete(
costmap->get_node_base_interface(),
results);
EXPECT_EQ(costmap->get_parameter("static_layer.transform_tolerance").as_double(), 1.0);
EXPECT_EQ(costmap->get_parameter("static_layer.enabled").as_bool(), false);
EXPECT_EQ(costmap->get_parameter("static_layer.map_subscribe_transient_local").as_bool(), false);
EXPECT_EQ(costmap->get_parameter("static_layer.map_topic").as_string(), "dynamic_topic");
EXPECT_EQ(costmap->get_parameter("static_layer.subscribe_to_updates").as_bool(), true);
costmap->on_deactivate(rclcpp_lifecycle::State());
costmap->on_cleanup(rclcpp_lifecycle::State());
costmap->on_shutdown(rclcpp_lifecycle::State());
}
navigation2/nav2_costmap_2d/test/integration/range_tests.cpp
+294
View File
@@ -0,0 +1,294 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2020, Bytes Robotics
* 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 copyright holder 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 HOLDER 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.
*/
#include <memory>
#include <string>
#include <algorithm>
#include <utility>
#include <vector>
#include "gtest/gtest.h"
#include "nav2_costmap_2d/costmap_2d.hpp"
#include "nav2_costmap_2d/layered_costmap.hpp"
#include "nav2_costmap_2d/observation_buffer.hpp"
#include "../testing_helper.hpp"
#include "sensor_msgs/msg/range.hpp"
using std::begin;
using std::end;
using std::for_each;
using std::all_of;
using std::none_of;
using std::pair;
using std::string;
class RclCppFixture
{
public:
RclCppFixture()
{
rclcpp::init(0, nullptr);
}
~RclCppFixture()
{
rclcpp::shutdown();
}
};
RclCppFixture g_rclcppfixture;
class TestLifecycleNode : public nav2_util::LifecycleNode
{
public:
explicit TestLifecycleNode(const string & name)
: nav2_util::LifecycleNode(name)
{
}
nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State &)
{
return nav2_util::CallbackReturn::SUCCESS;
}
nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State &)
{
return nav2_util::CallbackReturn::SUCCESS;
}
nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State &)
{
return nav2_util::CallbackReturn::SUCCESS;
}
nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State &)
{
return nav2_util::CallbackReturn::SUCCESS;
}
nav2_util::CallbackReturn onShutdown(const rclcpp_lifecycle::State &)
{
return nav2_util::CallbackReturn::SUCCESS;
}
nav2_util::CallbackReturn onError(const rclcpp_lifecycle::State &)
{
return nav2_util::CallbackReturn::SUCCESS;
}
};
class TestNode : public ::testing::Test
{
public:
TestNode()
: node_(std::make_shared<TestLifecycleNode>("range_test_node")),
tf_(node_->get_clock())
{
tf_.setUsingDedicatedThread(true);
// Standard non-plugin specific 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<unsigned char>(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("range")));
node_->declare_parameter("global_frame", rclcpp::ParameterValue(std::string("map")));
// Range sensor specific parameters
node_->declare_parameter(
"range.topics",
rclcpp::ParameterValue(
std::vector<std::string>{"/range/topic"}));
node_->declare_parameter("range.phi", rclcpp::ParameterValue(1.2));
node_->declare_parameter("range.clear_on_max_reading", rclcpp::ParameterValue(true));
}
~TestNode() {}
protected:
std::shared_ptr<TestLifecycleNode> node_;
tf2_ros::Buffer tf_;
};
// Test clearing at max range
TEST_F(TestNode, testClearingAtMaxRange) {
geometry_msgs::msg::TransformStamped transform;
transform.header.stamp = node_->now();
transform.header.frame_id = "frame";
transform.child_frame_id = "base_link";
transform.transform.translation.y = 5;
transform.transform.translation.x = 2;
tf_.setTransform(transform, "default_authority", true);
nav2_costmap_2d::LayeredCostmap layers("frame", false, false);
layers.resizeMap(10, 10, 1, 0, 0);
std::shared_ptr<nav2_costmap_2d::RangeSensorLayer> rlayer{nullptr};
addRangeLayer(layers, tf_, node_, rlayer);
sensor_msgs::msg::Range msg;
msg.min_range = 1.0;
msg.max_range = 7.0;
msg.range = 2.0;
msg.header.stamp = node_->now();
msg.header.frame_id = "base_link";
msg.radiation_type = msg.ULTRASOUND;
msg.field_of_view = 0.174533; // 10 deg
rlayer->bufferIncomingRangeMsg(std::make_shared<sensor_msgs::msg::Range>(msg));
layers.updateMap(0, 0, 0); // 0, 0, 0 is robot pose
// printMap(*(layers.getCostmap()));
ASSERT_EQ(layers.getCostmap()->getCost(4, 5), 254);
msg.range = 7.0;
msg.header.stamp = node_->now();
rlayer->bufferIncomingRangeMsg(std::make_shared<sensor_msgs::msg::Range>(msg));
layers.updateMap(0, 0, 0); // 0, 0, 0 is robot pose
// printMap(*(layers.getCostmap()));
ASSERT_EQ(layers.getCostmap()->getCost(4, 5), 0);
}
// Testing fixed scan with robot forward motion
TEST_F(TestNode, testProbabalisticModelForward) {
geometry_msgs::msg::TransformStamped transform;
transform.header.stamp = node_->now();
transform.header.frame_id = "frame";
transform.child_frame_id = "base_link";
transform.transform.translation.y = 5;
transform.transform.translation.x = 2;
tf_.setTransform(transform, "default_authority", true);
nav2_costmap_2d::LayeredCostmap layers("frame", false, false);
layers.resizeMap(10, 10, 1, 0, 0);
std::shared_ptr<nav2_costmap_2d::RangeSensorLayer> rlayer{nullptr};
addRangeLayer(layers, tf_, node_, rlayer);
sensor_msgs::msg::Range msg;
msg.min_range = 1.0;
msg.max_range = 10.0;
msg.range = 3.0;
msg.header.stamp = node_->now();
msg.header.frame_id = "base_link";
msg.radiation_type = msg.ULTRASOUND;
msg.field_of_view = 0.174533; // 10 deg
rlayer->bufferIncomingRangeMsg(std::make_shared<sensor_msgs::msg::Range>(msg));
layers.updateMap(0, 0, 0); // 0, 0, 0 is robot pose
// printMap(*(layers.getCostmap()));
rlayer->bufferIncomingRangeMsg(std::make_shared<sensor_msgs::msg::Range>(msg));
transform.transform.translation.y = 5;
transform.transform.translation.x = 4;
tf_.setTransform(transform, "default_authority", true);
layers.updateMap(0, 0, 0); // 0, 0, 0 is robot pose
// printMap(*(layers.getCostmap()));
rlayer->bufferIncomingRangeMsg(std::make_shared<sensor_msgs::msg::Range>(msg));
transform.transform.translation.y = 5;
transform.transform.translation.x = 6;
tf_.setTransform(transform, "default_authority", true);
layers.updateMap(0, 0, 0); // 0, 0, 0 is robot pose
// printMap(*(layers.getCostmap()));
ASSERT_EQ(layers.getCostmap()->getCost(5, 5), 254);
ASSERT_EQ(layers.getCostmap()->getCost(6, 5), 0);
ASSERT_EQ(layers.getCostmap()->getCost(7, 5), 254);
ASSERT_EQ(layers.getCostmap()->getCost(8, 5), 0);
ASSERT_EQ(layers.getCostmap()->getCost(9, 5), 254);
}
// Testing fixed motion with downward movement
TEST_F(TestNode, testProbabalisticModelDownward) {
geometry_msgs::msg::TransformStamped transform;
transform.header.stamp = node_->now();
transform.header.frame_id = "frame";
transform.child_frame_id = "base_link";
transform.transform.translation.y = 3;
transform.transform.translation.x = 2;
tf_.setTransform(transform, "default_authority", true);
nav2_costmap_2d::LayeredCostmap layers("frame", false, false);
layers.resizeMap(10, 10, 1, 0, 0);
std::shared_ptr<nav2_costmap_2d::RangeSensorLayer> rlayer{nullptr};
addRangeLayer(layers, tf_, node_, rlayer);
sensor_msgs::msg::Range msg;
msg.min_range = 1.0;
msg.max_range = 10.0;
msg.range = 1.0;
msg.header.stamp = node_->now();
msg.header.frame_id = "base_link";
msg.radiation_type = msg.ULTRASOUND;
msg.field_of_view = 0.174533; // 10 deg
rlayer->bufferIncomingRangeMsg(std::make_shared<sensor_msgs::msg::Range>(msg));
layers.updateMap(0, 0, 0); // 0, 0, 0 is robot pose
// printMap(*(layers.getCostmap()));
rlayer->bufferIncomingRangeMsg(std::make_shared<sensor_msgs::msg::Range>(msg));
transform.transform.translation.y = 5;
transform.transform.translation.x = 2;
tf_.setTransform(transform, "default_authority", true);
layers.updateMap(0, 0, 0); // 0, 0, 0 is robot pose
// printMap(*(layers.getCostmap()));
rlayer->bufferIncomingRangeMsg(std::make_shared<sensor_msgs::msg::Range>(msg));
transform.transform.translation.y = 7;
transform.transform.translation.x = 2;
tf_.setTransform(transform, "default_authority", true);
layers.updateMap(0, 0, 0); // 0, 0, 0 is robot pose
// printMap(*(layers.getCostmap()));
ASSERT_EQ(layers.getCostmap()->getCost(3, 3), 254);
ASSERT_EQ(layers.getCostmap()->getCost(3, 4), 0);
ASSERT_EQ(layers.getCostmap()->getCost(3, 5), 254);
ASSERT_EQ(layers.getCostmap()->getCost(3, 6), 0);
ASSERT_EQ(layers.getCostmap()->getCost(3, 7), 254);
}
navigation2/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp
+359
View File
@@ -0,0 +1,359 @@
// Copyright (c) 2019 Intel Corporation
//
// 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.
#include <string>
#include <vector>
#include <memory>
#include <chrono>
#include "gtest/gtest.h"
#include "rclcpp/rclcpp.hpp"
#include "nav2_costmap_2d/costmap_topic_collision_checker.hpp"
#include "nav2_costmap_2d/costmap_2d.hpp"
#include "nav2_costmap_2d/layered_costmap.hpp"
#include "nav2_costmap_2d/static_layer.hpp"
#include "nav2_costmap_2d/inflation_layer.hpp"
#include "nav2_costmap_2d/costmap_2d_publisher.hpp"
#include "../testing_helper.hpp"
#include "nav2_util/robot_utils.hpp"
#include "nav2_util/node_utils.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "tf2_ros/buffer.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/create_timer_ros.h"
#include "tf2_ros/transform_broadcaster.h"
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wpedantic"
#include "tf2/utils.h"
#pragma GCC diagnostic pop
#include "nav2_util/geometry_utils.hpp"
using namespace std::chrono_literals;
using namespace std::placeholders;
using nav2_util::geometry_utils::orientationAroundZAxis;
class RclCppFixture
{
public:
RclCppFixture() {rclcpp::init(0, nullptr);}
~RclCppFixture() {rclcpp::shutdown();}
};
RclCppFixture g_rclcppfixture;
class DummyCostmapSubscriber : public nav2_costmap_2d::CostmapSubscriber
{
public:
DummyCostmapSubscriber(
nav2_util::LifecycleNode::SharedPtr node,
std::string & topic_name)
: CostmapSubscriber(node, topic_name)
{}
void setCostmap(nav2_msgs::msg::Costmap::SharedPtr msg)
{
costmap_msg_ = msg;
costmap_received_ = true;
}
};
class DummyFootprintSubscriber : public nav2_costmap_2d::FootprintSubscriber
{
public:
DummyFootprintSubscriber(
nav2_util::LifecycleNode::SharedPtr node,
std::string & topic_name,
tf2_ros::Buffer & tf_)
: FootprintSubscriber(node, topic_name, tf_)
{}
void setFootprint(geometry_msgs::msg::PolygonStamped::SharedPtr msg)
{
footprint_ = msg;
footprint_received_ = true;
}
};
class TestCollisionChecker : public nav2_util::LifecycleNode
{
public:
explicit TestCollisionChecker(std::string name)
: LifecycleNode(name),
global_frame_("map")
{
// Declare non-plugin specific costmap parameters
declare_parameter("map_topic", rclcpp::ParameterValue(std::string("map")));
declare_parameter("track_unknown_space", rclcpp::ParameterValue(true));
declare_parameter("use_maximum", rclcpp::ParameterValue(false));
declare_parameter("lethal_cost_threshold", rclcpp::ParameterValue(100));
declare_parameter(
"unknown_cost_value",
rclcpp::ParameterValue(static_cast<unsigned char>(0xff)));
declare_parameter("trinary_costmap", rclcpp::ParameterValue(true));
}
nav2_util::CallbackReturn
on_configure(const rclcpp_lifecycle::State & /*state*/)
{
RCLCPP_INFO(get_logger(), "Configuring");
callback_group_ = create_callback_group(
rclcpp::CallbackGroupType::MutuallyExclusive, false);
tf_buffer_ = std::make_shared<tf2_ros::Buffer>(get_clock());
auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(
get_node_base_interface(),
get_node_timers_interface(),
callback_group_);
tf_buffer_->setCreateTimerInterface(timer_interface);
tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(shared_from_this());
std::string costmap_topic = "costmap_raw";
std::string footprint_topic = "published_footprint";
costmap_sub_ = std::make_shared<DummyCostmapSubscriber>(
shared_from_this(),
costmap_topic);
footprint_sub_ = std::make_shared<DummyFootprintSubscriber>(
shared_from_this(),
footprint_topic,
*tf_buffer_);
collision_checker_ = std::make_unique<nav2_costmap_2d::CostmapTopicCollisionChecker>(
*costmap_sub_, *footprint_sub_, get_name());
layers_ = new nav2_costmap_2d::LayeredCostmap("map", false, false);
// Add Static Layer
std::shared_ptr<nav2_costmap_2d::StaticLayer> slayer = nullptr;
addStaticLayer(*layers_, *tf_buffer_, shared_from_this(), slayer, callback_group_);
while (!slayer->isCurrent()) {
rclcpp::spin_some(this->get_node_base_interface());
}
// Add Inflation Layer
std::shared_ptr<nav2_costmap_2d::InflationLayer> ilayer = nullptr;
addInflationLayer(*layers_, *tf_buffer_, shared_from_this(), ilayer, callback_group_);
executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
executor_->add_callback_group(callback_group_, get_node_base_interface());
executor_thread_ = std::make_unique<nav2_util::NodeThread>(executor_);
return nav2_util::CallbackReturn::SUCCESS;
}
nav2_util::CallbackReturn
on_activate(const rclcpp_lifecycle::State & /*state*/)
{
RCLCPP_INFO(get_logger(), "Activating");
return nav2_util::CallbackReturn::SUCCESS;
}
nav2_util::CallbackReturn
on_deactivate(const rclcpp_lifecycle::State & /*state*/)
{
RCLCPP_INFO(get_logger(), "Deactivating");
return nav2_util::CallbackReturn::SUCCESS;
}
nav2_util::CallbackReturn
on_cleanup(const rclcpp_lifecycle::State & /*state*/)
{
RCLCPP_INFO(get_logger(), "Cleaning Up");
delete layers_;
layers_ = nullptr;
executor_thread_.reset();
tf_buffer_.reset();
footprint_sub_.reset();
costmap_sub_.reset();
return nav2_util::CallbackReturn::SUCCESS;
}
~TestCollisionChecker() {}
bool testPose(double x, double y, double theta)
{
rclcpp::Time stamp = now();
publishPose(x, y, theta, stamp);
geometry_msgs::msg::Pose2D pose;
pose.x = x;
pose.y = y;
pose.theta = theta;
setPose(x, y, theta, stamp);
publishFootprint();
publishCostmap();
rclcpp::sleep_for(std::chrono::milliseconds(1000));
return collision_checker_->isCollisionFree(pose);
}
void setFootprint(double footprint_padding, double robot_radius)
{
std::vector<geometry_msgs::msg::Point> new_footprint;
new_footprint = nav2_costmap_2d::makeFootprintFromRadius(robot_radius);
nav2_costmap_2d::padFootprint(new_footprint, footprint_padding);
footprint_ = new_footprint;
layers_->setFootprint(footprint_);
}
protected:
void setPose(double x, double y, double theta, const rclcpp::Time & stamp)
{
x_ = x;
y_ = y;
yaw_ = theta;
stamp_ = stamp;
current_pose_.pose.position.x = x_;
current_pose_.pose.position.y = y_;
current_pose_.pose.position.z = 0;
current_pose_.pose.orientation = orientationAroundZAxis(yaw_);
current_pose_.header.stamp = stamp;
}
void publishFootprint()
{
geometry_msgs::msg::PolygonStamped oriented_footprint;
oriented_footprint.header.frame_id = global_frame_;
oriented_footprint.header.stamp = stamp_;
nav2_costmap_2d::transformFootprint(x_, y_, yaw_, footprint_, oriented_footprint);
footprint_sub_->setFootprint(
std::make_shared<geometry_msgs::msg::PolygonStamped>(oriented_footprint));
}
void publishCostmap()
{
layers_->updateMap(x_, y_, yaw_);
costmap_sub_->setCostmap(
std::make_shared<nav2_msgs::msg::Costmap>(toCostmapMsg(layers_->getCostmap())));
}
void publishPose(double x, double y, double /*theta*/, const rclcpp::Time & stamp)
{
geometry_msgs::msg::TransformStamped tf_stamped;
tf_stamped.header.frame_id = "map";
tf_stamped.header.stamp = stamp;
tf_stamped.child_frame_id = "base_link";
tf_stamped.transform.translation.x = x;
tf_stamped.transform.translation.y = y;
tf_stamped.transform.rotation.w = 1.0;
tf_broadcaster_->sendTransform(tf_stamped);
}
nav2_msgs::msg::Costmap
toCostmapMsg(nav2_costmap_2d::Costmap2D * costmap)
{
double resolution = costmap->getResolution();
double wx, wy;
costmap->mapToWorld(0, 0, wx, wy);
unsigned char * data = costmap->getCharMap();
nav2_msgs::msg::Costmap costmap_msg;
costmap_msg.header.frame_id = global_frame_;
costmap_msg.header.stamp = stamp_;
costmap_msg.metadata.layer = "master";
costmap_msg.metadata.resolution = resolution;
costmap_msg.metadata.size_x = costmap->getSizeInCellsX();
costmap_msg.metadata.size_y = costmap->getSizeInCellsY();
costmap_msg.metadata.origin.position.x = wx - resolution / 2;
costmap_msg.metadata.origin.position.y = wy - resolution / 2;
costmap_msg.metadata.origin.position.z = 0.0;
costmap_msg.metadata.origin.orientation.w = 1.0;
costmap_msg.data.resize(costmap_msg.metadata.size_x * costmap_msg.metadata.size_y);
for (unsigned int i = 0; i < costmap_msg.data.size(); i++) {
costmap_msg.data[i] = data[i];
}
return costmap_msg;
}
std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
rclcpp::CallbackGroup::SharedPtr callback_group_;
rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_;
std::unique_ptr<nav2_util::NodeThread> executor_thread_;
std::shared_ptr<DummyCostmapSubscriber> costmap_sub_;
std::shared_ptr<DummyFootprintSubscriber> footprint_sub_;
std::unique_ptr<nav2_costmap_2d::CostmapTopicCollisionChecker> collision_checker_;
nav2_costmap_2d::LayeredCostmap * layers_{nullptr};
std::string global_frame_;
double x_, y_, yaw_;
rclcpp::Time stamp_;
geometry_msgs::msg::PoseStamped current_pose_;
std::vector<geometry_msgs::msg::Point> footprint_;
};
class TestNode : public ::testing::Test
{
public:
TestNode()
{
collision_checker_ = std::make_shared<TestCollisionChecker>("test_collision_checker");
collision_checker_->on_configure(collision_checker_->get_current_state());
collision_checker_->on_activate(collision_checker_->get_current_state());
}
~TestNode()
{
collision_checker_->on_deactivate(collision_checker_->get_current_state());
collision_checker_->on_cleanup(collision_checker_->get_current_state());
}
protected:
std::shared_ptr<TestCollisionChecker> collision_checker_;
};
TEST_F(TestNode, unknownSpace)
{
collision_checker_->setFootprint(0, 1);
// Completely off map
ASSERT_EQ(collision_checker_->testPose(5, 13, 0), false);
// Partially off map
ASSERT_EQ(collision_checker_->testPose(5, 9.5, 0), false);
// In unknown region inside map
ASSERT_EQ(collision_checker_->testPose(2, 4, 0), false);
}
TEST_F(TestNode, FreeSpace)
{
collision_checker_->setFootprint(0, 1);
// In complete free space
ASSERT_EQ(collision_checker_->testPose(2, 8.5, 0), true);
// Partially in inscribed space
ASSERT_EQ(collision_checker_->testPose(2.5, 7, 0), true);
}
TEST_F(TestNode, CollisionSpace)
{
collision_checker_->setFootprint(0, 1);
// Completely in obstacle
ASSERT_EQ(collision_checker_->testPose(8.5, 6.5, 0), false);
// Partially in obstacle
ASSERT_EQ(collision_checker_->testPose(4.5, 4.5, 0), false);
}