diff --git a/nav2_map_server/README.md b/nav2_map_server/README.md index 64ef980be9..c0e7a2cf6a 100644 --- a/nav2_map_server/README.md +++ b/nav2_map_server/README.md @@ -94,6 +94,11 @@ Then, one would invoke this process with the params file that contains the param $ process_with_multiple_map_servers __params:=combined_params.yaml ``` + +The parameter for the initial map (yaml_filename) has to be set, but an empty string can be used if no initial map should be loaded. In this case, no map is loaded during +on_configure or published during on_activate. The _load_map_-service should the be used to load and publish a map. + + #### Map Saver Like in ROS1 `map_saver` could be used as CLI-executable. It was renamed to `map_saver_cli` diff --git a/nav2_map_server/include/nav2_map_server/map_server.hpp b/nav2_map_server/include/nav2_map_server/map_server.hpp index 20ab257c4e..d2a7e63f75 100644 --- a/nav2_map_server/include/nav2_map_server/map_server.hpp +++ b/nav2_map_server/include/nav2_map_server/map_server.hpp @@ -138,6 +138,9 @@ class MapServer : public nav2_util::LifecycleNode // The message to publish on the occupancy grid topic nav_msgs::msg::OccupancyGrid msg_; + + // true if msg_ was initialized + bool map_available_; }; } // namespace nav2_map_server diff --git a/nav2_map_server/src/map_server/map_server.cpp b/nav2_map_server/src/map_server/map_server.cpp index 5920e095be..6f2a6e0483 100644 --- a/nav2_map_server/src/map_server/map_server.cpp +++ b/nav2_map_server/src/map_server/map_server.cpp @@ -63,7 +63,7 @@ namespace nav2_map_server { MapServer::MapServer(const rclcpp::NodeOptions & options) -: nav2_util::LifecycleNode("map_server", "", options) +: nav2_util::LifecycleNode("map_server", "", options), map_available_(false) { RCLCPP_INFO(get_logger(), "Creating"); @@ -82,19 +82,26 @@ MapServer::on_configure(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Configuring"); - // Get the name of the YAML file to use + // Get the name of the YAML file to use (can be empty if no initial map should be used) std::string yaml_filename = get_parameter("yaml_filename").as_string(); - std::string topic_name = get_parameter("topic_name").as_string(); frame_id_ = get_parameter("frame_id").as_string(); - // Shared pointer to LoadMap::Response is also should be initialized - // in order to avoid null-pointer dereference - std::shared_ptr rsp = - std::make_shared(); - - if (!loadMapResponseFromYaml(yaml_filename, rsp)) { - throw std::runtime_error("Failed to load map yaml file: " + yaml_filename); + // only try to load map if parameter was set + if (!yaml_filename.empty()) { + // Shared pointer to LoadMap::Response is also should be initialized + // in order to avoid null-pointer dereference + std::shared_ptr rsp = + std::make_shared(); + + if (!loadMapResponseFromYaml(yaml_filename, rsp)) { + throw std::runtime_error("Failed to load map yaml file: " + yaml_filename); + } + } else { + RCLCPP_INFO( + get_logger(), + "yaml-filename parameter is empty, set map through '%s'-service", + load_map_service_name_.c_str()); } // Make name prefix for services @@ -125,8 +132,10 @@ MapServer::on_activate(const rclcpp_lifecycle::State & /*state*/) // Publish the map using the latched topic occ_pub_->on_activate(); - auto occ_grid = std::make_unique(msg_); - occ_pub_->publish(std::move(occ_grid)); + if (map_available_) { + auto occ_grid = std::make_unique(msg_); + occ_pub_->publish(std::move(occ_grid)); + } // create bond connection createBond(); @@ -155,6 +164,8 @@ MapServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/) occ_pub_.reset(); occ_service_.reset(); load_map_service_.reset(); + map_available_ = false; + msg_ = nav_msgs::msg::OccupancyGrid(); return nav2_util::CallbackReturn::SUCCESS; } @@ -192,6 +203,7 @@ void MapServer::loadMapCallback( RCLCPP_WARN( get_logger(), "Received LoadMap request but not in ACTIVE state, ignoring!"); + response->result = response->RESULT_UNDEFINED_FAILURE; return; } RCLCPP_INFO(get_logger(), "Handling LoadMap request"); @@ -217,9 +229,10 @@ bool MapServer::loadMapResponseFromYaml( response->result = nav2_msgs::srv::LoadMap::Response::RESULT_INVALID_MAP_DATA; return false; case LOAD_MAP_SUCCESS: - // Correcting msg_ header when it belongs to spiecific node + // Correcting msg_ header when it belongs to specific node updateMsgHeader(); + map_available_ = true; response->map = msg_; response->result = nav2_msgs::srv::LoadMap::Response::RESULT_SUCCESS; } diff --git a/nav2_map_server/test/component/test_map_server_node.cpp b/nav2_map_server/test/component/test_map_server_node.cpp index d034ca3c68..ae3a80982f 100644 --- a/nav2_map_server/test/component/test_map_server_node.cpp +++ b/nav2_map_server/test/component/test_map_server_node.cpp @@ -24,7 +24,9 @@ #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::experimental::filesystem::path; @@ -191,3 +193,45 @@ TEST_F(MapServerTestFixture, LoadMapInvalidImage) 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("/map_server/map"); + auto req = std::make_shared(); + + auto parameters_client = std::make_shared(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(); + auto load_map_cl = node_->create_client("/map_server/load_map"); + + ASSERT_TRUE(load_map_cl->wait_for_service(3s)); + auto resp = send_request(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(node_, load_map_cl, load_map_req); + + ASSERT_EQ(load_res->result, nav2_msgs::srv::LoadMap::Response::RESULT_SUCCESS); + verifyMapMsg(load_res->map); +}