add humble-navigation2
This commit is contained in:
X-lanni
@@ -0,0 +1,51 @@
|
||||
include_directories(${PROJECT_SOURCE_DIR}/test)
|
||||
|
||||
# map_server component test
|
||||
ament_add_gtest_executable(test_map_server_node
|
||||
test_map_server_node.cpp
|
||||
${PROJECT_SOURCE_DIR}/test/test_constants.cpp
|
||||
)
|
||||
ament_target_dependencies(test_map_server_node rclcpp nav_msgs)
|
||||
target_link_libraries(test_map_server_node
|
||||
${library_name}
|
||||
)
|
||||
|
||||
ament_add_test(test_map_server_node
|
||||
GENERATE_RESULT_FOR_RETURN_CODE_ZERO
|
||||
COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_map_server_launch.py"
|
||||
WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
|
||||
ENV
|
||||
TEST_DIR=${TEST_DIR}
|
||||
TEST_LAUNCH_DIR=${TEST_LAUNCH_DIR}
|
||||
TEST_EXECUTABLE=$<TARGET_FILE:test_map_server_node>
|
||||
)
|
||||
|
||||
# map_saver component test
|
||||
ament_add_gtest_executable(test_map_saver_node
|
||||
test_map_saver_node.cpp
|
||||
${PROJECT_SOURCE_DIR}/test/test_constants.cpp
|
||||
)
|
||||
|
||||
ament_target_dependencies(test_map_saver_node rclcpp nav_msgs)
|
||||
target_link_libraries(test_map_saver_node
|
||||
${library_name}
|
||||
)
|
||||
|
||||
add_executable(test_map_saver_publisher
|
||||
test_map_saver_publisher.cpp
|
||||
${PROJECT_SOURCE_DIR}/test/test_constants.cpp
|
||||
)
|
||||
|
||||
target_link_libraries(test_map_saver_publisher
|
||||
${map_io_library_name}
|
||||
)
|
||||
|
||||
ament_add_test(test_map_saver_node
|
||||
GENERATE_RESULT_FOR_RETURN_CODE_ZERO
|
||||
COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_map_saver_launch.py"
|
||||
WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
|
||||
ENV
|
||||
TEST_DIR=${TEST_DIR}
|
||||
TEST_LAUNCH_DIR=${TEST_LAUNCH_DIR}
|
||||
TEST_EXECUTABLE=$<TARGET_FILE:test_map_saver_node>
|
||||
)
|
||||
@@ -0,0 +1,47 @@
|
||||
#!/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
|
||||
from launch_testing.legacy import LaunchTestService
|
||||
|
||||
|
||||
def main(argv=sys.argv[1:]):
|
||||
launchFile = os.path.join(os.getenv('TEST_LAUNCH_DIR'), 'map_saver_node.launch.py')
|
||||
testExecutable = os.getenv('TEST_EXECUTABLE')
|
||||
ld = LaunchDescription([
|
||||
IncludeLaunchDescription(PythonLaunchDescriptionSource([launchFile])),
|
||||
])
|
||||
test1_action = ExecuteProcess(
|
||||
cmd=[testExecutable],
|
||||
name='test_map_saver_node',
|
||||
)
|
||||
lts = LaunchTestService()
|
||||
lts.add_test_action(ld, test1_action)
|
||||
ls = LaunchService(argv=argv)
|
||||
ls.include_launch_description(ld)
|
||||
os.chdir(os.getenv('TEST_LAUNCH_DIR'))
|
||||
return lts.run(ls)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
sys.exit(main())
|
||||
@@ -0,0 +1,222 @@
|
||||
// Copyright (c) 2020 Samsung Research Russia
|
||||
// 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.
|
||||
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include <string>
|
||||
#include <memory>
|
||||
#include <filesystem>
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
|
||||
#include "test_constants/test_constants.h"
|
||||
#include "nav2_map_server/map_saver.hpp"
|
||||
#include "nav2_util/lifecycle_service_client.hpp"
|
||||
#include "nav2_msgs/srv/save_map.hpp"
|
||||
|
||||
#define TEST_DIR TEST_DIRECTORY
|
||||
|
||||
using std::filesystem::path;
|
||||
using lifecycle_msgs::msg::Transition;
|
||||
using namespace nav2_map_server; // NOLINT
|
||||
|
||||
class RclCppFixture
|
||||
{
|
||||
public:
|
||||
RclCppFixture() {rclcpp::init(0, nullptr);}
|
||||
~RclCppFixture() {rclcpp::shutdown();}
|
||||
};
|
||||
|
||||
RclCppFixture g_rclcppfixture;
|
||||
|
||||
class MapSaverTestFixture : public ::testing::Test
|
||||
{
|
||||
public:
|
||||
static void SetUpTestCase()
|
||||
{
|
||||
node_ = rclcpp::Node::make_shared("map_client_test");
|
||||
lifecycle_client_ =
|
||||
std::make_shared<nav2_util::LifecycleServiceClient>("map_saver", node_);
|
||||
RCLCPP_INFO(node_->get_logger(), "Creating Test Node");
|
||||
|
||||
std::this_thread::sleep_for(std::chrono::seconds(5)); // allow node to start up
|
||||
const std::chrono::seconds timeout(5);
|
||||
lifecycle_client_->change_state(Transition::TRANSITION_CONFIGURE, timeout);
|
||||
lifecycle_client_->change_state(Transition::TRANSITION_ACTIVATE, timeout);
|
||||
}
|
||||
|
||||
static void TearDownTestCase()
|
||||
{
|
||||
lifecycle_client_->change_state(Transition::TRANSITION_DEACTIVATE);
|
||||
lifecycle_client_->change_state(Transition::TRANSITION_CLEANUP);
|
||||
lifecycle_client_.reset();
|
||||
node_.reset();
|
||||
}
|
||||
|
||||
template<class T>
|
||||
typename T::Response::SharedPtr send_request(
|
||||
|
||||
rclcpp::Node::SharedPtr node,
|
||||
typename rclcpp::Client<T>::SharedPtr client,
|
||||
typename T::Request::SharedPtr request)
|
||||
{
|
||||
auto result = client->async_send_request(request);
|
||||
|
||||
// Wait for the result
|
||||
if (rclcpp::spin_until_future_complete(node, result) == rclcpp::FutureReturnCode::SUCCESS) {
|
||||
return result.get();
|
||||
} else {
|
||||
return nullptr;
|
||||
}
|
||||
}
|
||||
|
||||
protected:
|
||||
// Check that map_msg corresponds to reference pattern
|
||||
// Input: map_msg
|
||||
void verifyMapMsg(const nav_msgs::msg::OccupancyGrid & map_msg)
|
||||
{
|
||||
ASSERT_FLOAT_EQ(map_msg.info.resolution, g_valid_image_res);
|
||||
ASSERT_EQ(map_msg.info.width, g_valid_image_width);
|
||||
ASSERT_EQ(map_msg.info.height, g_valid_image_height);
|
||||
for (unsigned int i = 0; i < map_msg.info.width * map_msg.info.height; i++) {
|
||||
ASSERT_EQ(g_valid_image_content[i], map_msg.data[i]);
|
||||
}
|
||||
}
|
||||
|
||||
static rclcpp::Node::SharedPtr node_;
|
||||
static std::shared_ptr<nav2_util::LifecycleServiceClient> lifecycle_client_;
|
||||
};
|
||||
|
||||
|
||||
rclcpp::Node::SharedPtr MapSaverTestFixture::node_ = nullptr;
|
||||
std::shared_ptr<nav2_util::LifecycleServiceClient> MapSaverTestFixture::lifecycle_client_ =
|
||||
nullptr;
|
||||
|
||||
// Send map saving service request.
|
||||
// Load saved map and verify obtained OccupancyGrid.
|
||||
TEST_F(MapSaverTestFixture, SaveMap)
|
||||
{
|
||||
RCLCPP_INFO(node_->get_logger(), "Testing SaveMap service");
|
||||
auto req = std::make_shared<nav2_msgs::srv::SaveMap::Request>();
|
||||
auto client = node_->create_client<nav2_msgs::srv::SaveMap>(
|
||||
"/map_saver/save_map");
|
||||
|
||||
RCLCPP_INFO(node_->get_logger(), "Waiting for save_map service");
|
||||
ASSERT_TRUE(client->wait_for_service());
|
||||
|
||||
// 1. Send valid save_map serivce request
|
||||
req->map_topic = "map";
|
||||
req->map_url = path(g_tmp_dir) / path(g_valid_map_name);
|
||||
req->image_format = "png";
|
||||
req->map_mode = "trinary";
|
||||
req->free_thresh = g_default_free_thresh;
|
||||
req->occupied_thresh = g_default_occupied_thresh;
|
||||
auto resp = send_request<nav2_msgs::srv::SaveMap>(node_, client, req);
|
||||
ASSERT_EQ(resp->result, true);
|
||||
|
||||
// 2. Load saved map and verify it
|
||||
nav_msgs::msg::OccupancyGrid map_msg;
|
||||
LOAD_MAP_STATUS status = loadMapFromYaml(path(g_tmp_dir) / path(g_valid_yaml_file), map_msg);
|
||||
ASSERT_EQ(status, LOAD_MAP_SUCCESS);
|
||||
verifyMapMsg(map_msg);
|
||||
}
|
||||
|
||||
// Send map saving service request with default parameters.
|
||||
// Load saved map and verify obtained OccupancyGrid.
|
||||
TEST_F(MapSaverTestFixture, SaveMapDefaultParameters)
|
||||
{
|
||||
RCLCPP_INFO(node_->get_logger(), "Testing SaveMap service");
|
||||
auto req = std::make_shared<nav2_msgs::srv::SaveMap::Request>();
|
||||
auto client = node_->create_client<nav2_msgs::srv::SaveMap>(
|
||||
"/map_saver/save_map");
|
||||
|
||||
RCLCPP_INFO(node_->get_logger(), "Waiting for save_map service");
|
||||
ASSERT_TRUE(client->wait_for_service());
|
||||
|
||||
// 1. Send save_map serivce request with default parameters
|
||||
req->map_topic = "";
|
||||
req->map_url = path(g_tmp_dir) / path(g_valid_map_name);
|
||||
req->image_format = "";
|
||||
req->map_mode = "";
|
||||
req->free_thresh = 0.0;
|
||||
req->occupied_thresh = 0.0;
|
||||
auto resp = send_request<nav2_msgs::srv::SaveMap>(node_, client, req);
|
||||
ASSERT_EQ(resp->result, true);
|
||||
|
||||
// 2. Load saved map and verify it
|
||||
nav_msgs::msg::OccupancyGrid map_msg;
|
||||
LOAD_MAP_STATUS status = loadMapFromYaml(path(g_tmp_dir) / path(g_valid_yaml_file), map_msg);
|
||||
ASSERT_EQ(status, LOAD_MAP_SUCCESS);
|
||||
verifyMapMsg(map_msg);
|
||||
}
|
||||
|
||||
// Send map saving service requests with different sets of parameters.
|
||||
// In case of map is expected to be saved correctly, load map from a saved
|
||||
// file and verify obtained OccupancyGrid.
|
||||
TEST_F(MapSaverTestFixture, SaveMapInvalidParameters)
|
||||
{
|
||||
RCLCPP_INFO(node_->get_logger(), "Testing SaveMap service");
|
||||
auto req = std::make_shared<nav2_msgs::srv::SaveMap::Request>();
|
||||
auto client = node_->create_client<nav2_msgs::srv::SaveMap>(
|
||||
"/map_saver/save_map");
|
||||
|
||||
RCLCPP_INFO(node_->get_logger(), "Waiting for save_map service");
|
||||
ASSERT_TRUE(client->wait_for_service());
|
||||
|
||||
// 1. Trying to send save_map serivce request with different sets of parameters
|
||||
// In case of map is expected to be saved correctly, verify it
|
||||
req->map_topic = "invalid_map";
|
||||
req->map_url = path(g_tmp_dir) / path(g_valid_map_name);
|
||||
req->image_format = "png";
|
||||
req->map_mode = "trinary";
|
||||
req->free_thresh = g_default_free_thresh;
|
||||
req->occupied_thresh = g_default_occupied_thresh;
|
||||
auto resp = send_request<nav2_msgs::srv::SaveMap>(node_, client, req);
|
||||
ASSERT_EQ(resp->result, false);
|
||||
|
||||
req->map_topic = "map";
|
||||
req->image_format = "invalid_format";
|
||||
resp = send_request<nav2_msgs::srv::SaveMap>(node_, client, req);
|
||||
ASSERT_EQ(resp->result, true);
|
||||
nav_msgs::msg::OccupancyGrid map_msg;
|
||||
LOAD_MAP_STATUS status = loadMapFromYaml(path(g_tmp_dir) / path(g_valid_yaml_file), map_msg);
|
||||
ASSERT_EQ(status, LOAD_MAP_SUCCESS);
|
||||
verifyMapMsg(map_msg);
|
||||
|
||||
req->image_format = "png";
|
||||
req->map_mode = "invalid_mode";
|
||||
resp = send_request<nav2_msgs::srv::SaveMap>(node_, client, req);
|
||||
ASSERT_EQ(resp->result, true);
|
||||
status = loadMapFromYaml(path(g_tmp_dir) / path(g_valid_yaml_file), map_msg);
|
||||
ASSERT_EQ(status, LOAD_MAP_SUCCESS);
|
||||
verifyMapMsg(map_msg);
|
||||
|
||||
req->map_mode = "trinary";
|
||||
req->free_thresh = 2.0;
|
||||
req->occupied_thresh = 2.0;
|
||||
resp = send_request<nav2_msgs::srv::SaveMap>(node_, client, req);
|
||||
ASSERT_EQ(resp->result, false);
|
||||
|
||||
req->free_thresh = -2.0;
|
||||
req->occupied_thresh = -2.0;
|
||||
resp = send_request<nav2_msgs::srv::SaveMap>(node_, client, req);
|
||||
ASSERT_EQ(resp->result, false);
|
||||
|
||||
req->free_thresh = 0.7;
|
||||
req->occupied_thresh = 0.2;
|
||||
resp = send_request<nav2_msgs::srv::SaveMap>(node_, client, req);
|
||||
ASSERT_EQ(resp->result, false);
|
||||
}
|
||||
@@ -0,0 +1,59 @@
|
||||
// Copyright (c) 2020 Samsung Research Russia
|
||||
//
|
||||
// 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 <filesystem>
|
||||
#include <string>
|
||||
#include <memory>
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "nav2_map_server/map_io.hpp"
|
||||
#include "test_constants/test_constants.h"
|
||||
|
||||
#define TEST_DIR TEST_DIRECTORY
|
||||
|
||||
using namespace nav2_map_server; // NOLINT
|
||||
using std::filesystem::path;
|
||||
|
||||
class TestPublisher : public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
TestPublisher()
|
||||
: Node("map_publisher")
|
||||
{
|
||||
std::string pub_map_file = path(TEST_DIR) / path(g_valid_yaml_file);
|
||||
nav_msgs::msg::OccupancyGrid msg;
|
||||
LOAD_MAP_STATUS status = loadMapFromYaml(pub_map_file, msg);
|
||||
if (status != LOAD_MAP_SUCCESS) {
|
||||
RCLCPP_ERROR(get_logger(), "Can not load %s map file", pub_map_file.c_str());
|
||||
return;
|
||||
}
|
||||
|
||||
map_pub_ = create_publisher<nav_msgs::msg::OccupancyGrid>(
|
||||
"map",
|
||||
rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable());
|
||||
map_pub_->publish(msg);
|
||||
}
|
||||
|
||||
protected:
|
||||
rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr map_pub_;
|
||||
};
|
||||
|
||||
int main(int argc, char ** argv)
|
||||
{
|
||||
rclcpp::init(argc, argv);
|
||||
auto pub_node = std::make_shared<TestPublisher>();
|
||||
rclcpp::spin(pub_node);
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,47 @@
|
||||
#!/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
|
||||
from launch_testing.legacy import LaunchTestService
|
||||
|
||||
|
||||
def main(argv=sys.argv[1:]):
|
||||
launchFile = os.path.join(os.getenv('TEST_LAUNCH_DIR'), 'map_server_node.launch.py')
|
||||
testExecutable = os.getenv('TEST_EXECUTABLE')
|
||||
ld = LaunchDescription([
|
||||
IncludeLaunchDescription(PythonLaunchDescriptionSource([launchFile])),
|
||||
])
|
||||
test1_action = ExecuteProcess(
|
||||
cmd=[testExecutable],
|
||||
name='test_map_server_node',
|
||||
)
|
||||
lts = LaunchTestService()
|
||||
lts.add_test_action(ld, test1_action)
|
||||
ls = LaunchService(argv=argv)
|
||||
ls.include_launch_description(ld)
|
||||
os.chdir(os.getenv('TEST_LAUNCH_DIR'))
|
||||
return lts.run(ls)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
sys.exit(main())
|
||||
@@ -0,0 +1,237 @@
|
||||
// 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.
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include <string>
|
||||
#include <memory>
|
||||
#include <filesystem>
|
||||
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
|
||||
#include "test_constants/test_constants.h"
|
||||
#include "nav2_map_server/map_server.hpp"
|
||||
#include "nav2_util/lifecycle_service_client.hpp"
|
||||
#include "nav2_msgs/srv/load_map.hpp"
|
||||
using namespace std::chrono_literals;
|
||||
using namespace rclcpp; // NOLINT
|
||||
|
||||
#define TEST_DIR TEST_DIRECTORY
|
||||
|
||||
using std::filesystem::path;
|
||||
|
||||
using lifecycle_msgs::msg::Transition;
|
||||
|
||||
class RclCppFixture
|
||||
{
|
||||
public:
|
||||
RclCppFixture() {rclcpp::init(0, nullptr);}
|
||||
~RclCppFixture() {rclcpp::shutdown();}
|
||||
};
|
||||
|
||||
RclCppFixture g_rclcppfixture;
|
||||
|
||||
class MapServerTestFixture : public ::testing::Test
|
||||
{
|
||||
public:
|
||||
static void SetUpTestCase()
|
||||
{
|
||||
node_ = rclcpp::Node::make_shared("map_client_test");
|
||||
lifecycle_client_ =
|
||||
std::make_shared<nav2_util::LifecycleServiceClient>("map_server", node_);
|
||||
RCLCPP_INFO(node_->get_logger(), "Creating Test Node");
|
||||
|
||||
std::this_thread::sleep_for(std::chrono::seconds(5)); // allow node to start up
|
||||
const std::chrono::seconds timeout(5);
|
||||
lifecycle_client_->change_state(Transition::TRANSITION_CONFIGURE, timeout);
|
||||
lifecycle_client_->change_state(Transition::TRANSITION_ACTIVATE, timeout);
|
||||
}
|
||||
|
||||
static void TearDownTestCase()
|
||||
{
|
||||
lifecycle_client_->change_state(Transition::TRANSITION_DEACTIVATE);
|
||||
lifecycle_client_->change_state(Transition::TRANSITION_CLEANUP);
|
||||
lifecycle_client_->change_state(Transition::TRANSITION_UNCONFIGURED_SHUTDOWN);
|
||||
lifecycle_client_.reset();
|
||||
node_.reset();
|
||||
}
|
||||
|
||||
template<class T>
|
||||
typename T::Response::SharedPtr send_request(
|
||||
rclcpp::Node::SharedPtr node,
|
||||
typename rclcpp::Client<T>::SharedPtr client,
|
||||
typename T::Request::SharedPtr request)
|
||||
{
|
||||
auto result = client->async_send_request(request);
|
||||
|
||||
// Wait for the result
|
||||
if (rclcpp::spin_until_future_complete(node, result) == rclcpp::FutureReturnCode::SUCCESS) {
|
||||
return result.get();
|
||||
} else {
|
||||
return nullptr;
|
||||
}
|
||||
}
|
||||
|
||||
protected:
|
||||
// Check that map_msg corresponds to reference pattern
|
||||
// Input: map_msg
|
||||
void verifyMapMsg(const nav_msgs::msg::OccupancyGrid & map_msg)
|
||||
{
|
||||
ASSERT_FLOAT_EQ(map_msg.info.resolution, g_valid_image_res);
|
||||
ASSERT_EQ(map_msg.info.width, g_valid_image_width);
|
||||
ASSERT_EQ(map_msg.info.height, g_valid_image_height);
|
||||
for (unsigned int i = 0; i < map_msg.info.width * map_msg.info.height; i++) {
|
||||
ASSERT_EQ(g_valid_image_content[i], map_msg.data[i]);
|
||||
}
|
||||
}
|
||||
|
||||
static rclcpp::Node::SharedPtr node_;
|
||||
static std::shared_ptr<nav2_util::LifecycleServiceClient> lifecycle_client_;
|
||||
};
|
||||
|
||||
|
||||
rclcpp::Node::SharedPtr MapServerTestFixture::node_ = nullptr;
|
||||
std::shared_ptr<nav2_util::LifecycleServiceClient> MapServerTestFixture::lifecycle_client_ =
|
||||
nullptr;
|
||||
|
||||
|
||||
// Send map getting service request and verify obtained OccupancyGrid
|
||||
TEST_F(MapServerTestFixture, GetMap)
|
||||
{
|
||||
RCLCPP_INFO(node_->get_logger(), "Testing GetMap service");
|
||||
auto req = std::make_shared<nav_msgs::srv::GetMap::Request>();
|
||||
auto client = node_->create_client<nav_msgs::srv::GetMap>(
|
||||
"/map_server/map");
|
||||
|
||||
RCLCPP_INFO(node_->get_logger(), "Waiting for map service");
|
||||
ASSERT_TRUE(client->wait_for_service());
|
||||
|
||||
auto resp = send_request<nav_msgs::srv::GetMap>(node_, client, req);
|
||||
|
||||
verifyMapMsg(resp->map);
|
||||
}
|
||||
|
||||
// Send map loading service request and verify obtained OccupancyGrid
|
||||
TEST_F(MapServerTestFixture, LoadMap)
|
||||
{
|
||||
RCLCPP_INFO(node_->get_logger(), "Testing LoadMap service");
|
||||
auto req = std::make_shared<nav2_msgs::srv::LoadMap::Request>();
|
||||
auto client = node_->create_client<nav2_msgs::srv::LoadMap>(
|
||||
"/map_server/load_map");
|
||||
|
||||
RCLCPP_INFO(node_->get_logger(), "Waiting for load_map service");
|
||||
ASSERT_TRUE(client->wait_for_service());
|
||||
|
||||
req->map_url = path(TEST_DIR) / path(g_valid_yaml_file);
|
||||
auto resp = send_request<nav2_msgs::srv::LoadMap>(node_, client, req);
|
||||
|
||||
ASSERT_EQ(resp->result, nav2_msgs::srv::LoadMap::Response::RESULT_SUCCESS);
|
||||
verifyMapMsg(resp->map);
|
||||
}
|
||||
|
||||
// Send map loading service request without specifying which map to load
|
||||
TEST_F(MapServerTestFixture, LoadMapNull)
|
||||
{
|
||||
RCLCPP_INFO(node_->get_logger(), "Testing LoadMap service");
|
||||
auto req = std::make_shared<nav2_msgs::srv::LoadMap::Request>();
|
||||
auto client = node_->create_client<nav2_msgs::srv::LoadMap>(
|
||||
"/map_server/load_map");
|
||||
|
||||
RCLCPP_INFO(node_->get_logger(), "Waiting for load_map service");
|
||||
ASSERT_TRUE(client->wait_for_service());
|
||||
|
||||
req->map_url = "";
|
||||
RCLCPP_INFO(node_->get_logger(), "Sending load_map request with null file name");
|
||||
auto resp = send_request<nav2_msgs::srv::LoadMap>(node_, client, req);
|
||||
|
||||
ASSERT_EQ(resp->result, nav2_msgs::srv::LoadMap::Response::RESULT_MAP_DOES_NOT_EXIST);
|
||||
}
|
||||
|
||||
// Send map loading service request with non-existing yaml file
|
||||
TEST_F(MapServerTestFixture, LoadMapInvalidYaml)
|
||||
{
|
||||
RCLCPP_INFO(node_->get_logger(), "Testing LoadMap service");
|
||||
auto req = std::make_shared<nav2_msgs::srv::LoadMap::Request>();
|
||||
auto client = node_->create_client<nav2_msgs::srv::LoadMap>(
|
||||
"/map_server/load_map");
|
||||
|
||||
RCLCPP_INFO(node_->get_logger(), "Waiting for load_map service");
|
||||
ASSERT_TRUE(client->wait_for_service());
|
||||
|
||||
req->map_url = "invalid_file.yaml";
|
||||
RCLCPP_INFO(node_->get_logger(), "Sending load_map request with invalid yaml file name");
|
||||
auto resp = send_request<nav2_msgs::srv::LoadMap>(node_, client, req);
|
||||
|
||||
ASSERT_EQ(resp->result, nav2_msgs::srv::LoadMap::Response::RESULT_INVALID_MAP_METADATA);
|
||||
}
|
||||
|
||||
// Send map loading service request with yaml file containing non-existing map
|
||||
TEST_F(MapServerTestFixture, LoadMapInvalidImage)
|
||||
{
|
||||
RCLCPP_INFO(node_->get_logger(), "Testing LoadMap service");
|
||||
auto req = std::make_shared<nav2_msgs::srv::LoadMap::Request>();
|
||||
auto client = node_->create_client<nav2_msgs::srv::LoadMap>(
|
||||
"/map_server/load_map");
|
||||
|
||||
RCLCPP_INFO(node_->get_logger(), "Waiting for load_map service");
|
||||
ASSERT_TRUE(client->wait_for_service());
|
||||
|
||||
req->map_url = path(TEST_DIR) / "invalid_image.yaml";
|
||||
RCLCPP_INFO(node_->get_logger(), "Sending load_map request with invalid image file name");
|
||||
auto resp = send_request<nav2_msgs::srv::LoadMap>(node_, client, req);
|
||||
|
||||
ASSERT_EQ(resp->result, nav2_msgs::srv::LoadMap::Response::RESULT_INVALID_MAP_DATA);
|
||||
}
|
||||
|
||||
/**
|
||||
* Test behaviour of server if yaml_filename is set to an empty string.
|
||||
*/
|
||||
TEST_F(MapServerTestFixture, NoInitialMap)
|
||||
{
|
||||
// turn off node into unconfigured state
|
||||
lifecycle_client_->change_state(Transition::TRANSITION_DEACTIVATE);
|
||||
lifecycle_client_->change_state(Transition::TRANSITION_CLEANUP);
|
||||
|
||||
auto client = node_->create_client<nav_msgs::srv::GetMap>("/map_server/map");
|
||||
auto req = std::make_shared<nav_msgs::srv::GetMap::Request>();
|
||||
|
||||
auto parameters_client = std::make_shared<rclcpp::SyncParametersClient>(node_, "map_server");
|
||||
ASSERT_TRUE(parameters_client->wait_for_service(3s));
|
||||
|
||||
// set yaml_filename-parameter to empty string (essentially restart the node)
|
||||
RCLCPP_INFO(node_->get_logger(), "Removing yaml_filename-parameter before restarting");
|
||||
parameters_client->set_parameters({Parameter("yaml_filename", ParameterValue(""))});
|
||||
|
||||
// only configure node, to test behaviour of service while node is not active
|
||||
lifecycle_client_->change_state(Transition::TRANSITION_CONFIGURE, 3s);
|
||||
|
||||
RCLCPP_INFO(node_->get_logger(), "Testing LoadMap service while not being active");
|
||||
auto load_map_req = std::make_shared<nav2_msgs::srv::LoadMap::Request>();
|
||||
auto load_map_cl = node_->create_client<nav2_msgs::srv::LoadMap>("/map_server/load_map");
|
||||
|
||||
ASSERT_TRUE(load_map_cl->wait_for_service(3s));
|
||||
auto resp = send_request<nav2_msgs::srv::LoadMap>(node_, load_map_cl, load_map_req);
|
||||
|
||||
ASSERT_EQ(resp->result, nav2_msgs::srv::LoadMap::Response::RESULT_UNDEFINED_FAILURE);
|
||||
|
||||
// activate server and load map:
|
||||
lifecycle_client_->change_state(Transition::TRANSITION_ACTIVATE, 3s);
|
||||
RCLCPP_INFO(node_->get_logger(), "active again");
|
||||
|
||||
load_map_req->map_url = path(TEST_DIR) / path(g_valid_yaml_file);
|
||||
auto load_res = send_request<nav2_msgs::srv::LoadMap>(node_, load_map_cl, load_map_req);
|
||||
|
||||
ASSERT_EQ(load_res->result, nav2_msgs::srv::LoadMap::Response::RESULT_SUCCESS);
|
||||
verifyMapMsg(load_res->map);
|
||||
}
|
||||
Reference in New Issue
Block a user