diff --git a/nav2_map_server/src/occ_grid_loader.cpp b/nav2_map_server/src/occ_grid_loader.cpp index 55758aaacdfdfff6da43326b43353dca15ee0e21..52b4e95e627cba0c77244e57d3779bbd3173f248 100644 --- a/nav2_map_server/src/occ_grid_loader.cpp +++ b/nav2_map_server/src/occ_grid_loader.cpp @@ -131,7 +131,7 @@ bool OccGridLoader::loadMapFromYaml( std::string yaml_file, std::shared_ptr<nav2_msgs::srv::LoadMap::Response> response) { - if (yaml_file == "") { + if (yaml_file.empty()) { RCLCPP_ERROR(node_->get_logger(), "YAML file name is empty, can't load!"); response->result = nav2_msgs::srv::LoadMap::Response::RESULT_MAP_ID_DOES_NOT_EXIST; return false; @@ -182,6 +182,11 @@ nav2_util::CallbackReturn OccGridLoader::on_configure(const rclcpp_lifecycle::St const std::shared_ptr<rmw_request_id_t>/*request_header*/, const std::shared_ptr<nav_msgs::srv::GetMap::Request>/*request*/, std::shared_ptr<nav_msgs::srv::GetMap::Response> response) -> void { + if (node_->get_current_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { + RCLCPP_WARN(node_->get_logger(), + "Received GetMap request but not in ACTIVE state, ignoring!"); + return; + } RCLCPP_INFO(node_->get_logger(), "OccGridLoader: Handling GetMap request"); response->map = *msg_; }; @@ -194,6 +199,12 @@ nav2_util::CallbackReturn OccGridLoader::on_configure(const rclcpp_lifecycle::St const std::shared_ptr<rmw_request_id_t>/*request_header*/, const std::shared_ptr<nav2_msgs::srv::LoadMap::Request> request, std::shared_ptr<nav2_msgs::srv::LoadMap::Response> response) -> void { + // if not in ACTIVE state, ignore request + if (node_->get_current_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { + RCLCPP_WARN(node_->get_logger(), + "Received LoadMap request but not in ACTIVE state, ignoring!"); + return; + } RCLCPP_INFO(node_->get_logger(), "OccGridLoader: Handling LoadMap request"); // Check if map_id is a file if (request->type != nav2_msgs::srv::LoadMap::Request::TYPE_FILE) { @@ -206,10 +217,7 @@ nav2_util::CallbackReturn OccGridLoader::on_configure(const rclcpp_lifecycle::St if (loadMapFromYaml(request->map_id, response)) { response->map = *msg_; response->result = nav2_msgs::srv::LoadMap::Response::RESULT_SUCCESS; - // if in ACTIVE state, publish map - if (node_->get_current_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { - occ_pub_->publish(*msg_); - } + occ_pub_->publish(*msg_); // publish new map } };