diff --git a/nav2_map_server/CMakeLists.txt b/nav2_map_server/CMakeLists.txt index 5ce0bffd86..f8fc53ea86 100644 --- a/nav2_map_server/CMakeLists.txt +++ b/nav2_map_server/CMakeLists.txt @@ -1,45 +1,40 @@ cmake_minimum_required(VERSION 3.5) project(nav2_map_server) +list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_LIST_DIR}/cmake_modules) + find_package(ament_cmake REQUIRED) find_package(nav2_common REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_lifecycle REQUIRED) find_package(nav_msgs REQUIRED) -find_package(SDL REQUIRED) -find_package(SDL_image REQUIRED) find_package(yaml_cpp_vendor REQUIRED) find_package(std_msgs REQUIRED) find_package(tf2 REQUIRED) find_package(nav2_util REQUIRED) +find_package(GRAPHICSMAGICKCPP REQUIRED) nav2_package() -include_directories( - include - ${SDL_INCLUDE_DIR} - ${SDL_IMAGE_INCLUDE_DIRS} -) +include_directories(include) set(map_server_executable map_server) set(map_saver_executable map_saver) add_executable(${map_server_executable} - src/main.cpp -) + src/main.cpp) add_executable(${map_saver_executable} - src/map_saver.cpp -) + src/map_saver_cli.cpp) set(library_name ${map_server_executable}_core) add_library(${library_name} SHARED src/occ_grid_loader.cpp src/map_server.cpp - src/map_generator.cpp -) + src/map_saver.cpp + src/map_mode.cpp) set(map_server_dependencies rclcpp @@ -48,49 +43,41 @@ set(map_server_dependencies yaml_cpp_vendor std_msgs tf2 - nav2_util -) + nav2_util) set(map_saver_dependencies rclcpp nav_msgs - tf2 -) + tf2) ament_target_dependencies(${map_server_executable} - ${map_server_dependencies} -) + ${map_server_dependencies}) ament_target_dependencies(${map_saver_executable} - ${map_saver_dependencies} -) + ${map_saver_dependencies}) ament_target_dependencies(${library_name} - ${map_server_dependencies} -) + ${map_server_dependencies}) target_link_libraries(${map_server_executable} - ${library_name} -) + ${library_name}) target_link_libraries(${map_saver_executable} - ${library_name} -) + ${library_name}) + +target_include_directories(${library_name} SYSTEM PRIVATE + ${GRAPHICSMAGICKCPP_INCLUDE_DIRS}) target_link_libraries(${library_name} - ${SDL_LIBRARY} - ${SDL_IMAGE_LIBRARIES} -) + ${GRAPHICSMAGICKCPP_LIBRARIES}) install(TARGETS ${map_server_executable} ${library_name} ${map_saver_executable} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib - RUNTIME DESTINATION lib/${PROJECT_NAME} -) + RUNTIME DESTINATION lib/${PROJECT_NAME}) install(DIRECTORY include/ - DESTINATION include/ -) + DESTINATION include/) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) diff --git a/nav2_map_server/cmake_modules/FindGRAPHICSMAGICKCPP.cmake b/nav2_map_server/cmake_modules/FindGRAPHICSMAGICKCPP.cmake new file mode 100644 index 0000000000..37a2fbfb74 --- /dev/null +++ b/nav2_map_server/cmake_modules/FindGRAPHICSMAGICKCPP.cmake @@ -0,0 +1,34 @@ +# Copyright 2019 Rover 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. + +# CMake script for finding Magick++, the C++ interface for the +# GraphicsMagick library +# +# Output variables: +# GRAPHICSMAGICKCPP_FOUND - system has GraphicsMagick Magick++ +# GRAPHICSMAGICKCPP_INCLUDE_DIRS - include directories for Magick++ +# GRAPHICSMAGICKCPP_LIBRARIES - libraries you need to link to +include(FindPackageHandleStandardArgs) + +find_path(GRAPHICSMAGICKCPP_INCLUDE_DIRS + NAMES "Magick++.h" + PATH_SUFFIXES GraphicsMagick) + +find_library(GRAPHICSMAGICKCPP_LIBRARIES + NAMES "GraphicsMagick++") + +find_package_handle_standard_args( + GRAPHICSMAGICKCPP + GRAPHICSMAGICKCPP__LIBRARIES + GRAPHICSMAGICKCPP_INCLUDE_DIRS) \ No newline at end of file diff --git a/nav2_map_server/include/nav2_map_server/map_mode.hpp b/nav2_map_server/include/nav2_map_server/map_mode.hpp new file mode 100644 index 0000000000..a37a800f47 --- /dev/null +++ b/nav2_map_server/include/nav2_map_server/map_mode.hpp @@ -0,0 +1,55 @@ +// Copyright 2019 Rover 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. + +#ifndef NAV2_MAP_SERVER__MAP_MODE_HPP_ +#define NAV2_MAP_SERVER__MAP_MODE_HPP_ + +#include +#include +namespace nav2_map_server +{ +/// Describes the relation between image pixel values and map occupancy status (0-100; -1) +/// Lightness refers to the mean of a given pixel's RGB channels on a scale from 0 to 255. +enum class MapMode +{ + /// Together with associated threshold values (occupied and free): + /// lightness >= occupied threshold - Occupied (100) + /// ... (anything in between) - Unknown (-1) + /// lightness <= free threshold - Free (0) + Trinary, + /// Together with associated threshold values (occupied and free): + /// alpha < 1.0 - Unknown (-1) + /// lightness >= occ_th - Occupied (100) + /// ... (linearly interpolate to) + /// lightness <= free_th - Free (0) + Scale, + /// Lightness = 0 - Free (0) + /// ... (linearly interpolate to) + /// Lightness = 100 - Occupied (100) + /// Lightness >= 101 - Unknown + Raw, +}; + +/// Convert a MapMode enum to the name of the map mode +/// @throw std::invalid_argument if the given value is not a defined map mode +/// @return String identifier of the given map mode +const char * map_mode_to_string(MapMode map_mode); + +/// Convert the name of a map mode to a MapMode enum +/// @throw std::invalid_argument if the name does not name a map mode +/// @return map mode corresponding to the string +MapMode map_mode_from_string(std::string map_mode_name); +} // namespace nav2_map_server + +#endif // NAV2_MAP_SERVER__MAP_MODE_HPP_ diff --git a/nav2_map_server/include/nav2_map_server/map_generator.hpp b/nav2_map_server/include/nav2_map_server/map_saver.hpp similarity index 64% rename from nav2_map_server/include/nav2_map_server/map_generator.hpp rename to nav2_map_server/include/nav2_map_server/map_saver.hpp index a6230d377d..9be787a5d4 100644 --- a/nav2_map_server/include/nav2_map_server/map_generator.hpp +++ b/nav2_map_server/include/nav2_map_server/map_saver.hpp @@ -12,32 +12,39 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NAV2_MAP_SERVER__MAP_GENERATOR_HPP_ -#define NAV2_MAP_SERVER__MAP_GENERATOR_HPP_ +#ifndef NAV2_MAP_SERVER__MAP_SAVER_HPP_ +#define NAV2_MAP_SERVER__MAP_SAVER_HPP_ #include -#include "rclcpp/rclcpp.hpp" +#include "map_mode.hpp" #include "nav_msgs/srv/get_map.hpp" +#include "rclcpp/rclcpp.hpp" namespace nav2_map_server { - -class MapGenerator : public rclcpp::Node +class MapSaver : public rclcpp::Node { public: - MapGenerator(const std::string & mapname, int threshold_occupied, int threshold_free); + explicit MapSaver(const rclcpp::NodeOptions & options); void mapCallback(const nav_msgs::msg::OccupancyGrid::SharedPtr map); - bool saved_map_; + std::shared_future map_saved_future() {return save_next_map_promise.get_future().share();} -private: - std::string mapname_; +protected: rclcpp::Subscription::ConstSharedPtr map_sub_; + + void try_write_map_to_file(const nav_msgs::msg::OccupancyGrid & map); + + std::promise save_next_map_promise; + + std::string image_format; + std::string mapname_; int threshold_occupied_; int threshold_free_; + nav2_map_server::MapMode map_mode; }; } // namespace nav2_map_server -#endif // NAV2_MAP_SERVER__MAP_GENERATOR_HPP_ +#endif // NAV2_MAP_SERVER__MAP_SAVER_HPP_ diff --git a/nav2_map_server/include/nav2_map_server/occ_grid_loader.hpp b/nav2_map_server/include/nav2_map_server/occ_grid_loader.hpp index 92409a95ff..23206e5719 100644 --- a/nav2_map_server/include/nav2_map_server/occ_grid_loader.hpp +++ b/nav2_map_server/include/nav2_map_server/occ_grid_loader.hpp @@ -18,15 +18,16 @@ #include #include #include +#include "nav2_map_server/occ_grid_loader.hpp" -#include "rclcpp/rclcpp.hpp" +#include "map_mode.hpp" +#include "nav2_util/lifecycle_node.hpp" #include "nav_msgs/msg/occupancy_grid.hpp" #include "nav_msgs/srv/get_map.hpp" -#include "nav2_util/lifecycle_node.hpp" +#include "rclcpp/rclcpp.hpp" namespace nav2_map_server { - class OccGridLoader : public nav2_util::LifecycleHelperInterface { public: @@ -46,19 +47,23 @@ class OccGridLoader : public nav2_util::LifecycleHelperInterface // The name of the YAML file from which to get the conversion parameters std::string yaml_filename_; - typedef enum { TRINARY, SCALE, RAW } MapMode; typedef struct { + std::string image_file_name; double resolution{0}; std::vector origin{0, 0, 0}; - double free_thresh{0}; - double occupied_thresh{0}; - MapMode mode{TRINARY}; - int negate{0}; + double free_thresh; + double occupied_thresh; + MapMode mode; + bool negate; } LoadParameters; + // Load and parse the given YAML file + /// @throw YAML::Exception + LoadParameters load_map_yaml(const std::string & yaml_filename_); + // Load the image and generate an OccupancyGrid - void loadMapFromFile(const std::string & filename, LoadParameters * loadParameters); + void loadMapFromFile(const LoadParameters & loadParameters); // A service to provide the occupancy grid (GetMap) and the message to return rclcpp::Service::SharedPtr occ_service_; diff --git a/nav2_map_server/package.xml b/nav2_map_server/package.xml index 4947a9c2d9..59f1b2b534 100644 --- a/nav2_map_server/package.xml +++ b/nav2_map_server/package.xml @@ -7,22 +7,22 @@ Refactored map server for ROS2 Navigation Brian Wilcox - Apache License 2.0 + BSD License 2.0 ament_cmake nav2_common + rclcpp_lifecycle nav_msgs std_msgs rclcpp - sdl - sdl-image yaml_cpp_vendor launch_ros launch_testing tf2 nav2_util + graphicsmagick ament_lint_common ament_lint_auto diff --git a/nav2_map_server/src/map_generator.cpp b/nav2_map_server/src/map_generator.cpp deleted file mode 100644 index 562a29891b..0000000000 --- a/nav2_map_server/src/map_generator.cpp +++ /dev/null @@ -1,118 +0,0 @@ -/* - * map_saver - * Copyright (c) 2008, 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 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. - */ - -#include "nav2_map_server/map_generator.hpp" - -#include -#include -#include -#include "rclcpp/rclcpp.hpp" -#include "nav_msgs/srv/get_map.hpp" -#include "nav_msgs/msg/occupancy_grid.h" -#include "tf2/LinearMath/Quaternion.h" -#include "tf2/LinearMath/Matrix3x3.h" - -namespace nav2_map_server -{ - -/** - * @brief Map generation node. - */ -MapGenerator::MapGenerator(const std::string & mapname, int threshold_occupied, int threshold_free) -: Node("map_saver"), - saved_map_(false), - mapname_(mapname), - threshold_occupied_(threshold_occupied), - threshold_free_(threshold_free) -{ - RCLCPP_INFO(get_logger(), "Waiting for the map"); - map_sub_ = create_subscription( - "map", rclcpp::SystemDefaultsQoS(), - std::bind(&MapGenerator::mapCallback, this, std::placeholders::_1)); -} - -void MapGenerator::mapCallback(const nav_msgs::msg::OccupancyGrid::SharedPtr map) -{ - rclcpp::Logger logger = this->get_logger(); - RCLCPP_INFO(logger, "Received a %d X %d map @ %.3f m/pix", - map->info.width, - map->info.height, - map->info.resolution); - - - std::string mapdatafile = mapname_ + ".pgm"; - RCLCPP_INFO(logger, "Writing map occupancy data to %s", mapdatafile.c_str()); - FILE * out = fopen(mapdatafile.c_str(), "w"); - if (!out) { - RCLCPP_ERROR(logger, "Couldn't save map file to %s", mapdatafile.c_str()); - return; - } - - fprintf(out, "P5\n# CREATOR: map_saver.cpp %.3f m/pix\n%d %d\n255\n", - map->info.resolution, map->info.width, map->info.height); - for (unsigned int y = 0; y < map->info.height; y++) { - for (unsigned int x = 0; x < map->info.width; x++) { - unsigned int i = x + (map->info.height - y - 1) * map->info.width; - if (map->data[i] >= 0 && map->data[i] <= threshold_free_) { // [0,free) - fputc(254, out); - } else if (map->data[i] >= threshold_occupied_) { // (occ,255] - fputc(000, out); - } else { // occ [0.25,0.65] - fputc(205, out); - } - } - } - - fclose(out); - - std::string mapmetadatafile = mapname_ + ".yaml"; - RCLCPP_INFO(logger, "Writing map occupancy data to %s", mapmetadatafile.c_str()); - FILE * yaml = fopen(mapmetadatafile.c_str(), "w"); - - geometry_msgs::msg::Quaternion orientation = map->info.origin.orientation; - tf2::Matrix3x3 mat(tf2::Quaternion(orientation.x, - orientation.y, - orientation.z, - orientation.w)); - double yaw, pitch, roll; - mat.getEulerYPR(yaw, pitch, roll); - - fprintf(yaml, "image: %s\nresolution: %f\norigin: [%f, %f, %f]\n", - mapdatafile.c_str(), map->info.resolution, - map->info.origin.position.x, map->info.origin.position.y, yaw); - fprintf(yaml, "negate: 0\noccupied_thresh: 0.65\nfree_thresh: 0.196\n\n"); - - fclose(yaml); - - RCLCPP_INFO(logger, "Done\n"); - saved_map_ = true; -} - -} // namespace nav2_map_server diff --git a/nav2_map_server/src/map_mode.cpp b/nav2_map_server/src/map_mode.cpp new file mode 100644 index 0000000000..d0edfcfe52 --- /dev/null +++ b/nav2_map_server/src/map_mode.cpp @@ -0,0 +1,52 @@ +// Copyright 2019 Rover 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 "nav2_map_server/map_mode.hpp" + +#include +#include + +namespace nav2_map_server +{ +const char * map_mode_to_string(MapMode map_mode) +{ + switch (map_mode) { + case MapMode::Trinary: + return "trinary"; + case MapMode::Scale: + return "scale"; + case MapMode::Raw: + return "raw"; + default: + throw std::invalid_argument("map_mode"); + } +} + +MapMode map_mode_from_string(std::string map_mode_name) +{ + for (auto & c : map_mode_name) { + c = tolower(c); + } + + if (map_mode_name == "scale") { + return MapMode::Scale; + } else if (map_mode_name == "raw") { + return MapMode::Raw; + } else if (map_mode_name == "trinary") { + return MapMode::Trinary; + } else { + throw std::invalid_argument("map_mode_name"); + } +} +} // namespace nav2_map_server diff --git a/nav2_map_server/src/map_saver.cpp b/nav2_map_server/src/map_saver.cpp index fe3369e52c..8c50569b34 100644 --- a/nav2_map_server/src/map_saver.cpp +++ b/nav2_map_server/src/map_saver.cpp @@ -1,5 +1,5 @@ /* - * map_saver + * Copyright 2019 Rover Robotics * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * @@ -28,83 +28,223 @@ * POSSIBILITY OF SUCH DAMAGE. */ +#include "nav2_map_server/map_saver.hpp" + #include -#include -#include -#include +#include #include +#include +#include +#include + +#include "Magick++.h" +#include "nav2_map_server/map_mode.hpp" +#include "nav_msgs/msg/occupancy_grid.h" +#include "nav_msgs/srv/get_map.hpp" #include "rclcpp/rclcpp.hpp" -#include "nav2_map_server/map_generator.hpp" +#include "tf2/LinearMath/Matrix3x3.h" +#include "tf2/LinearMath/Quaternion.h" +#include "yaml-cpp/yaml.h" -#define USAGE "Usage: \n" \ - " map_saver -h\n" \ - " map_saver [--occ ] [--free ] " \ - "[-f ] [ROS remapping args]" +namespace nav2_map_server +{ +MapSaver::MapSaver(const rclcpp::NodeOptions & options) +: Node("map_saver", options), save_next_map_promise{} +{ + Magick::InitializeMagick(nullptr); + { + mapname_ = declare_parameter("output_file_no_ext", "map"); + if (mapname_.empty()) { + throw std::runtime_error("Map name not provided"); + } + threshold_occupied_ = declare_parameter("threshold_occupied", 65); + if (100 < threshold_occupied_) { + throw std::runtime_error("Threshold_occupied must be 100 or less"); + } + threshold_free_ = declare_parameter("threshold_free", 25); + if (threshold_free_ < 0) { + throw std::runtime_error("Free threshold must be 0 or greater"); + } + if (threshold_occupied_ <= threshold_free_) { + throw std::runtime_error("Threshold_free must be smaller than threshold_occupied"); + } -using namespace std::chrono_literals; + std::string mode_str = declare_parameter("map_mode", "trinary"); + try { + map_mode = map_mode_from_string(mode_str); + } catch (std::invalid_argument &) { + map_mode = MapMode::Trinary; + RCLCPP_WARN( + get_logger(), "Map mode parameter not recognized: '%s', using default value (trinary)", + mode_str.c_str()); + } -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - rclcpp::Logger logger = rclcpp::get_logger("map_saver"); - - std::string mapname = "map"; - int threshold_occupied = 65; - int threshold_free = 25; - - for (int i = 1; i < argc; i++) { - if (!strcmp(argv[i], "-h")) { - puts(USAGE); - return 0; - } else if (!strcmp(argv[i], "-f")) { - if (++i < argc) { - mapname = argv[i]; - } else { - puts(USAGE); - return 1; - } - } else if (!strcmp(argv[i], "--occ")) { - if (++i < argc) { - threshold_occupied = atoi(argv[i]); - if (threshold_occupied < 1 || threshold_occupied > 100) { - RCLCPP_ERROR(logger, "Threshold_occupied must be between 1 and 100"); - return 1; + image_format = declare_parameter("image_format", map_mode == MapMode::Scale ? "png" : "pgm"); + std::transform( + image_format.begin(), image_format.end(), image_format.begin(), + [](unsigned char c) {return std::tolower(c);}); + const std::vector BLESSED_FORMATS{"bmp", "pgm", "png"}; + if ( + std::find(BLESSED_FORMATS.begin(), BLESSED_FORMATS.end(), image_format) == + BLESSED_FORMATS.end()) + { + std::stringstream ss; + bool first = true; + for (auto & format_name : BLESSED_FORMATS) { + if (!first) { + ss << ", "; } - } else { - puts(USAGE); - return 1; + ss << "'" << format_name << "'"; + first = false; } - } else if (!strcmp(argv[i], "--free")) { - if (++i < argc) { - threshold_free = atoi(argv[i]); - if (threshold_free < 0 || threshold_free > 100) { - RCLCPP_ERROR(logger, "Threshold_free must be between 0 and 100"); - return 1; - } - } else { - puts(USAGE); - return 1; + RCLCPP_WARN( + get_logger(), "Requested image format '%s' is not one of the recommended formats: %s", + image_format.c_str(), ss.str().c_str()); + } + const std::string FALLBACK_FORMAT = "png"; + + try { + Magick::CoderInfo info(image_format); + if (!info.isWritable()) { + RCLCPP_WARN( + get_logger(), "Format '%s' is not writable. Using '%s' instead", + image_format.c_str(), FALLBACK_FORMAT.c_str()); + image_format = FALLBACK_FORMAT; } - } else { - puts(USAGE); - return 1; + } catch (Magick::ErrorOption & e) { + RCLCPP_WARN( + get_logger(), "Format '%s' is not usable. Using '%s' instead:\n%s", + image_format.c_str(), FALLBACK_FORMAT.c_str(), e.what()); + image_format = FALLBACK_FORMAT; + } + if ( + map_mode == MapMode::Scale && + (image_format == "pgm" || image_format == "jpg" || image_format == "jpeg")) + { + RCLCPP_WARN( + get_logger(), + "Map mode 'scale' requires transparency, but format '%s' does not support it. Consider " + "switching image format to 'png'.", + image_format.c_str()); } + + RCLCPP_INFO(get_logger(), "Waiting for the map"); + map_sub_ = create_subscription( + "map", rclcpp::SystemDefaultsQoS(), + std::bind(&MapSaver::mapCallback, this, std::placeholders::_1)); } +} - if (threshold_occupied <= threshold_free) { - RCLCPP_ERROR(logger, "Threshold_free must be smaller than threshold_occupied"); - return 1; +void MapSaver::try_write_map_to_file(const nav_msgs::msg::OccupancyGrid & map) +{ + auto logger = get_logger(); + RCLCPP_INFO( + logger, "Received a %d X %d map @ %.3f m/pix", map.info.width, map.info.height, + map.info.resolution); + + std::string mapdatafile = mapname_ + "." + image_format; + { + // should never see this color, so the initialization value is just for debugging + Magick::Image image({map.info.width, map.info.height}, "red"); + + // In scale mode, we need the alpha (matte) channel. Else, we don't. + // NOTE: GraphicsMagick seems to have trouble loading the alpha channel when saved with + // Magick::GreyscaleMatte, so we use TrueColorMatte instead. + image.type(map_mode == MapMode::Scale ? Magick::TrueColorMatteType : Magick::GrayscaleType); + + // Since we only need to support 100 different pixel levels, 8 bits is fine + image.depth(8); + + for (size_t y = 0; y < map.info.height; y++) { + for (size_t x = 0; x < map.info.width; x++) { + int8_t map_cell = map.data[map.info.width * (map.info.height - y - 1) + x]; + + Magick::Color pixel; + + switch (map_mode) { + case MapMode::Trinary: + if (map_cell < 0 || 100 < map_cell) { + pixel = Magick::ColorGray(205 / 255.0); + } else if (map_cell <= threshold_free_) { + pixel = Magick::ColorGray(254 / 255.0); + } else if (threshold_occupied_ <= map_cell) { + pixel = Magick::ColorGray(0 / 255.0); + } else { + pixel = Magick::ColorGray(205 / 255.0); + } + break; + case MapMode::Scale: + if (map_cell < 0 || 100 < map_cell) { + pixel = Magick::ColorGray{0.5}; + pixel.alphaQuantum(TransparentOpacity); + } else { + pixel = Magick::ColorGray{(100.0 - map_cell) / 100.0}; + } + break; + case MapMode::Raw: + Magick::Quantum q; + if (map_cell < 0 || 100 < map_cell) { + q = MaxRGB; + } else { + q = map_cell / 255.0 * MaxRGB; + } + pixel = Magick::Color(q, q, q); + break; + default: + throw std::runtime_error("Invalid map mode"); + } + image.pixelColor(x, y, pixel); + } + } + + RCLCPP_INFO(logger, "Writing map occupancy data to %s", mapdatafile.c_str()); + image.write(mapdatafile); } - auto map_gen = std::make_shared(mapname, threshold_occupied, - threshold_free); + std::string mapmetadatafile = mapname_ + ".yaml"; + { + std::ofstream yaml(mapmetadatafile); + + geometry_msgs::msg::Quaternion orientation = map.info.origin.orientation; + tf2::Matrix3x3 mat(tf2::Quaternion(orientation.x, orientation.y, orientation.z, orientation.w)); + double yaw, pitch, roll; + mat.getEulerYPR(yaw, pitch, roll); + + YAML::Emitter e; + e << YAML::Precision(3); + e << YAML::BeginMap; + e << YAML::Key << "image" << YAML::Value << mapdatafile; + e << YAML::Key << "mode" << YAML::Value << map_mode_to_string(map_mode); + e << YAML::Key << "resolution" << YAML::Value << map.info.resolution; + e << YAML::Key << "origin" << YAML::Flow << YAML::BeginSeq << map.info.origin.position.x << + map.info.origin.position.y << yaw << YAML::EndSeq; + e << YAML::Key << "negate" << YAML::Value << 0; + e << YAML::Key << "occupied_thresh" << YAML::Value << threshold_occupied_ / 100.0; + e << YAML::Key << "free_thresh" << YAML::Value << threshold_free_ / 100.0; + + if (!e.good()) { + RCLCPP_WARN( + logger, "YAML writer failed with an error %s. The map metadata may be invalid.", + e.GetLastError().c_str()); + } - auto sleep_dur = std::chrono::milliseconds(100); - while (!map_gen->saved_map_ && rclcpp::ok()) { - rclcpp::spin_some(map_gen); - rclcpp::sleep_for(sleep_dur); + RCLCPP_INFO(logger, "Writing map metadata to %s", mapmetadatafile.c_str()); + std::ofstream(mapmetadatafile) << e.c_str(); } + RCLCPP_INFO(logger, "Map saved"); +} + +void MapSaver::mapCallback(const nav_msgs::msg::OccupancyGrid::SharedPtr map) +{ + auto current_promise = std::move(save_next_map_promise); + save_next_map_promise = std::promise(); - rclcpp::shutdown(); - return 0; + try { + try_write_map_to_file(*map); + current_promise.set_value(); + } catch (std::exception & e) { + RCLCPP_ERROR(get_logger(), "Failed to write map for reason: %s", e.what()); + current_promise.set_exception(std::current_exception()); + } } +} // namespace nav2_map_server diff --git a/nav2_map_server/src/map_saver_cli.cpp b/nav2_map_server/src/map_saver_cli.cpp new file mode 100644 index 0000000000..22ef3d27ae --- /dev/null +++ b/nav2_map_server/src/map_saver_cli.cpp @@ -0,0 +1,108 @@ +// Copyright 2019 Rover Robotics +// Copyright (c) 2008, Willow Garage, Inc. +// +// 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 +#include +#include + +#include "nav2_map_server/map_saver.hpp" +#include "rclcpp/rclcpp.hpp" + +const char * USAGE_STRING{ + "Usage: \n" + " map_saver -h/--help\n" + " map_saver [--occ ] [--free ] [--fmt ] " + "[--mode trinary/scale/raw] [-f ] [ROS remapping args]"}; + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + + auto logger = rclcpp::get_logger("map_saver_cli"); + + std::vector arguments(argv + 1, argv + argc); + std::vector params_from_args; + for (auto it = arguments.begin(); it != arguments.end(); it++) { + if (*it == "-h" || *it == "--help") { + std::cout << USAGE_STRING << std::endl; + return 0; + } else if (*it == "-f") { + if (++it == arguments.end()) { + RCLCPP_WARN(logger, "Argument ignored: -f should be followed by a value."); + continue; + } + params_from_args.emplace_back("output_file_no_ext", *it); + } else if (*it == "--occ") { + if (++it == arguments.end()) { + RCLCPP_WARN(logger, "Argument ignored: --occ should be followed by a value."); + continue; + } + params_from_args.emplace_back("threshold_occupied", atoi(it->c_str())); + } else if (*it == "--free") { + if (++it == arguments.end()) { + RCLCPP_WARN(logger, "Argument ignored: --free should be followed by a value."); + continue; + } + params_from_args.emplace_back("threshold_free", atoi(it->c_str())); + } else if (*it == "--mode") { + if (++it == arguments.end()) { + RCLCPP_WARN(logger, "Argument ignored: --mode should be followed by a value."); + continue; + } + params_from_args.emplace_back("map_mode", *it); + } else if (*it == "--fmt") { + if (++it == arguments.end()) { + RCLCPP_WARN(logger, "Argument ignored: --fmt should be followed by a value."); + continue; + } + params_from_args.emplace_back("image_format", *it); + } else { + RCLCPP_WARN(logger, "Ignoring unrecognized argument '%s'", it->c_str()); + } + } + + auto node = std::make_shared( + rclcpp::NodeOptions().parameter_overrides(params_from_args)); + auto future = node->map_saved_future(); + + int retcode; + switch (rclcpp::spin_until_future_complete(node, future)) { + case rclcpp::executor::FutureReturnCode::INTERRUPTED: + std::cout << "Map saver failed: interrupted" << std::endl; + retcode = 1; + break; + case rclcpp::executor::FutureReturnCode::TIMEOUT: + std::cout << "Map saver failed: timeout" << std::endl; + retcode = 1; + break; + case rclcpp::executor::FutureReturnCode::SUCCESS: + try { + future.get(); + std::cout << "Map saver succeeded" << std::endl; + retcode = 0; + } catch (std::exception & e) { + std::cout << "Map saver failed: " << e.what() << std::endl; + retcode = 1; + } + break; + default: + std::cerr << "this should never happen" << std::endl; + retcode = -1; + break; + } + + rclcpp::shutdown(); + return retcode; +} diff --git a/nav2_map_server/src/occ_grid_loader.cpp b/nav2_map_server/src/occ_grid_loader.cpp index 0743673994..98cbc1ce74 100644 --- a/nav2_map_server/src/occ_grid_loader.cpp +++ b/nav2_map_server/src/occ_grid_loader.cpp @@ -1,5 +1,7 @@ -/* +/* Copyright 2019 Rover Robotics + * Copyright 2018 Brian Gerkey * Copyright (c) 2008, Willow Garage, Inc. + * * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -27,9 +29,6 @@ * POSSIBILITY OF SUCH DAMAGE. */ -// This file contains helper functions for loading images as maps. -// Author: Brian Gerkey - #include "nav2_map_server/occ_grid_loader.hpp" #include @@ -38,117 +37,77 @@ #include #include +#include "Magick++.h" #include "tf2/LinearMath/Quaternion.h" -#include "SDL/SDL_image.h" #include "yaml-cpp/yaml.h" using namespace std::chrono_literals; namespace nav2_map_server { - OccGridLoader::OccGridLoader( - rclcpp_lifecycle::LifecycleNode::SharedPtr node, - std::string & yaml_filename) + rclcpp_lifecycle::LifecycleNode::SharedPtr node, std::string & yaml_filename) : node_(node), yaml_filename_(yaml_filename) { RCLCPP_INFO(node_->get_logger(), "OccGridLoader: Creating"); } -OccGridLoader::~OccGridLoader() -{ - RCLCPP_INFO(node_->get_logger(), "OccGridLoader: Destroying"); -} +OccGridLoader::~OccGridLoader() {RCLCPP_INFO(node_->get_logger(), "OccGridLoader: Destroying");} -nav2_util::CallbackReturn -OccGridLoader::on_configure(const rclcpp_lifecycle::State & /*state*/) +/// Get the given subnode value. +/// The only reason this function exists is to wrap the exceptions in slightly nicer error messages, +/// including the name of the failed key +/// @throw YAML::Exception +template +T yaml_get_value(const YAML::Node & node, const std::string & key) { - RCLCPP_INFO(node_->get_logger(), "OccGridLoader: Configuring"); - - msg_ = std::make_unique(); - - // The YAML document from which to get the conversion parameters - YAML::Node doc = YAML::LoadFile(yaml_filename_); - LoadParameters loadParameters; - - // Get the name of the map file - std::string map_filename; try { - map_filename = doc["image"].as(); - if (map_filename.size() == 0) { - throw std::runtime_error("The image tag cannot be an empty string"); - } - if (map_filename[0] != '/') { - // dirname can modify what you pass it - char * fname_copy = strdup(yaml_filename_.c_str()); - map_filename = std::string(dirname(fname_copy)) + '/' + map_filename; - free(fname_copy); - } + return node[key].as(); } catch (YAML::Exception & e) { - std::string err("'" + yaml_filename_ + - "' does not contain an image tag or it is invalid: " + e.what()); - throw std::runtime_error(err); + std::stringstream ss; + ss << "Failed to parse YAML tag '" << key << "' for reason: " << e.msg; + throw YAML::Exception(e.mark, ss.str()); } +} - try { - loadParameters.resolution = doc["resolution"].as(); - } catch (YAML::Exception & e) { - std::string err("The map does not contain a resolution tag or it is invalid: %s", e.what()); - throw std::runtime_error(err); - } +OccGridLoader::LoadParameters OccGridLoader::load_map_yaml(const std::string & yaml_filename) +{ + YAML::Node doc = YAML::LoadFile(yaml_filename); + LoadParameters loadParameters; - try { - loadParameters.origin[0] = doc["origin"][0].as(); - loadParameters.origin[1] = doc["origin"][1].as(); - loadParameters.origin[2] = doc["origin"][2].as(); - } catch (YAML::Exception & e) { - std::string err("The map does not contain an origin tag or it is invalid: %s", e.what()); - throw std::runtime_error(err); + auto image_file_name = yaml_get_value(doc, "image"); + if (image_file_name.empty()) { + throw YAML::Exception(doc["image"].Mark(), "The image tag was empty."); } - - try { - loadParameters.free_thresh = doc["free_thresh"].as(); - } catch (YAML::Exception & e) { - std::string err("The map does not contain a free_thresh tag or it is invalid: %s", e.what()); - throw std::runtime_error(err); + if (image_file_name[0] != '/') { + // dirname takes a mutable char *, so we copy into a vector + std::vector fname_copy(yaml_filename.begin(), yaml_filename.end()); + image_file_name = std::string(dirname(fname_copy.data())) + '/' + image_file_name; } - - try { - loadParameters.occupied_thresh = doc["occupied_thresh"].as(); - } catch (YAML::Exception & e) { - std::string err("The map does not contain an " - "occupied_thresh tag or it is invalid: %s", e.what()); - throw std::runtime_error(err); + loadParameters.image_file_name = image_file_name; + + loadParameters.resolution = yaml_get_value(doc, "resolution"); + loadParameters.origin = yaml_get_value>(doc, "origin"); + if (loadParameters.origin.size() != 3) { + throw YAML::Exception( + doc["origin"].Mark(), "value of the 'origin' tag should have 3 elements, not " + + std::to_string(loadParameters.origin.size())); } - std::string mode_str; - try { - mode_str = doc["mode"].as(); - - // Convert the string version of the mode name to one of the enumeration values - if (mode_str == "trinary") { - loadParameters.mode = TRINARY; - } else if (mode_str == "scale") { - loadParameters.mode = SCALE; - } else if (mode_str == "raw") { - loadParameters.mode = RAW; - } else { - RCLCPP_WARN(node_->get_logger(), - "Mode parameter not recognized: '%s', using default value (trinary)", - mode_str.c_str()); - loadParameters.mode = TRINARY; - } - } catch (YAML::Exception & e) { - RCLCPP_WARN(node_->get_logger(), - "Mode parameter not set, using default value (trinary): %s", e.what()); - loadParameters.mode = TRINARY; + loadParameters.free_thresh = yaml_get_value(doc, "free_thresh"); + loadParameters.occupied_thresh = yaml_get_value(doc, "occupied_thresh"); + + auto map_mode_node = doc["mode"]; + if (!map_mode_node.IsDefined()) { + loadParameters.mode = MapMode::Trinary; + } else { + loadParameters.mode = map_mode_from_string(map_mode_node.as()); } try { - loadParameters.negate = doc["negate"].as(); - } catch (YAML::Exception & e) { - std::string err("The map does not contain a negate tag or it is invalid: %s", e.what()); - throw std::runtime_error(err); + loadParameters.negate = yaml_get_value(doc, "negate"); + } catch (YAML::Exception &) { + loadParameters.negate = yaml_get_value(doc, "negate"); } RCLCPP_DEBUG(node_->get_logger(), "resolution: %f", loadParameters.resolution); @@ -157,11 +116,41 @@ OccGridLoader::on_configure(const rclcpp_lifecycle::State & /*state*/) RCLCPP_DEBUG(node_->get_logger(), "origin[2]: %f", loadParameters.origin[2]); RCLCPP_DEBUG(node_->get_logger(), "free_thresh: %f", loadParameters.free_thresh); RCLCPP_DEBUG(node_->get_logger(), "occupied_thresh: %f", loadParameters.occupied_thresh); - RCLCPP_DEBUG(node_->get_logger(), "mode_str: %s", mode_str.c_str()); - RCLCPP_DEBUG(node_->get_logger(), "mode: %d", loadParameters.mode); + RCLCPP_DEBUG(node_->get_logger(), "mode: %s", map_mode_to_string(loadParameters.mode)); RCLCPP_DEBUG(node_->get_logger(), "negate: %d", loadParameters.negate); - loadMapFromFile(map_filename, &loadParameters); + return loadParameters; +} + +nav2_util::CallbackReturn OccGridLoader::on_configure(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(node_->get_logger(), "OccGridLoader: Configuring"); + + msg_ = std::make_unique(); + LoadParameters loadParameters; + try { + loadParameters = load_map_yaml(yaml_filename_); + } catch (YAML::Exception & e) { + RCLCPP_ERROR( + node_->get_logger(), "Failed processing YAML file %s at position (%d:%d) for reason: %s", + yaml_filename_.c_str(), e.mark.line, e.mark.column, e.what()); + throw std::runtime_error("Failed to load map yaml file."); + } catch (std::exception & e) { + RCLCPP_ERROR( + node_->get_logger(), "Failed to parse map YAML loaded from file %s for reason: %s", + yaml_filename_.c_str(), e.what()); + throw std::runtime_error("Failed to load map yaml file."); + } + + try { + loadMapFromFile(loadParameters); + } catch (std::exception & e) { + RCLCPP_ERROR( + node_->get_logger(), "Failed to load image file %s for reason: %s", + loadParameters.image_file_name.c_str(), e.what()); + + throw std::runtime_error("Failed to load map image file."); + } // Create a service callback handle auto handle_occ_callback = [this]( @@ -182,8 +171,7 @@ OccGridLoader::on_configure(const rclcpp_lifecycle::State & /*state*/) return nav2_util::CallbackReturn::SUCCESS; } -nav2_util::CallbackReturn -OccGridLoader::on_activate(const rclcpp_lifecycle::State & /*state*/) +nav2_util::CallbackReturn OccGridLoader::on_activate(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(node_->get_logger(), "OccGridLoader: Activating"); @@ -198,8 +186,7 @@ OccGridLoader::on_activate(const rclcpp_lifecycle::State & /*state*/) return nav2_util::CallbackReturn::SUCCESS; } -nav2_util::CallbackReturn -OccGridLoader::on_deactivate(const rclcpp_lifecycle::State & /*state*/) +nav2_util::CallbackReturn OccGridLoader::on_deactivate(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(node_->get_logger(), "OccGridLoader: Deactivating"); @@ -208,8 +195,7 @@ OccGridLoader::on_deactivate(const rclcpp_lifecycle::State & /*state*/) return nav2_util::CallbackReturn::SUCCESS; } -nav2_util::CallbackReturn -OccGridLoader::on_cleanup(const rclcpp_lifecycle::State & /*state*/) +nav2_util::CallbackReturn OccGridLoader::on_cleanup(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(node_->get_logger(), "OccGridLoader: Cleaning up"); @@ -220,116 +206,103 @@ OccGridLoader::on_cleanup(const rclcpp_lifecycle::State & /*state*/) return nav2_util::CallbackReturn::SUCCESS; } -void -OccGridLoader::loadMapFromFile(const std::string & map_name, LoadParameters * loadParameters) +void OccGridLoader::loadMapFromFile(const LoadParameters & loadParameters) { - RCLCPP_DEBUG(node_->get_logger(), "OccGridLoader: loadMapFromFile"); - - // Load the image using SDL. If we get NULL back, the image load failed. - SDL_Surface * img; - if (!(img = IMG_Load(map_name.c_str()))) { - std::string errmsg = std::string("failed to open image file \"") + - map_name + std::string("\": ") + IMG_GetError(); - throw std::runtime_error(errmsg); - } + Magick::InitializeMagick(nullptr); + nav_msgs::msg::OccupancyGrid msg; + + Magick::Image img(loadParameters.image_file_name); // Copy the image data into the map structure - msg_->info.width = img->w; - msg_->info.height = img->h; - msg_->info.resolution = loadParameters->resolution; - msg_->info.origin.position.x = loadParameters->origin[0]; - msg_->info.origin.position.y = loadParameters->origin[1]; - msg_->info.origin.position.z = 0.0; + msg.info.width = img.size().width(); + msg.info.height = img.size().height(); + msg.info.resolution = loadParameters.resolution; + msg.info.origin.position.x = loadParameters.origin[0]; + msg.info.origin.position.y = loadParameters.origin[1]; + msg.info.origin.position.z = 0.0; tf2::Quaternion q; - q.setRPY(0, 0, loadParameters->origin[2]); - msg_->info.origin.orientation.x = q.x(); - msg_->info.origin.orientation.y = q.y(); - msg_->info.origin.orientation.z = q.z(); - msg_->info.origin.orientation.w = q.w(); + q.setRPY(0, 0, loadParameters.origin[2]); + msg.info.origin.orientation.x = q.x(); + msg.info.origin.orientation.y = q.y(); + msg.info.origin.orientation.z = q.z(); + msg.info.origin.orientation.w = q.w(); // Allocate space to hold the data - msg_->data.resize(msg_->info.width * msg_->info.height); - - // Get values that we'll need to iterate through the pixels - int rowstride = img->pitch; - int n_channels = img->format->BytesPerPixel; - - // NOTE: Trinary mode still overrides here to preserve existing behavior. - // Alpha will be averaged in with color channels when using trinary mode. - int avg_channels; - if (loadParameters->mode == TRINARY || !img->format->Amask) { - avg_channels = n_channels; - } else { - avg_channels = n_channels - 1; - } + msg.data.resize(msg.info.width * msg.info.height); // Copy pixel data into the map structure - unsigned char * pixels = (unsigned char *)(img->pixels); - int color_sum; - for (unsigned int j = 0; j < msg_->info.height; j++) { - for (unsigned int i = 0; i < msg_->info.width; i++) { - // Compute mean of RGB for this pixel - unsigned char * p = pixels + j * rowstride + i * n_channels; - color_sum = 0; - for (int k = 0; k < avg_channels; k++) { - color_sum += *(p + (k)); + for (size_t y = 0; y < msg.info.height; y++) { + for (size_t x = 0; x < msg.info.width; x++) { + auto pixel = img.pixelColor(x, y); + + std::vector channels = {pixel.redQuantum(), pixel.greenQuantum(), + pixel.blueQuantum()}; + if (loadParameters.mode == MapMode::Trinary && img.matte()) { + // To preserve existing behavior, average in alpha with color channels in Trinary mode. + // CAREFUL. alpha is inverted from what you might expect. High = transparent, low = opaque + channels.push_back(MaxRGB - pixel.alphaQuantum()); } - double color_avg = color_sum / static_cast(avg_channels); - - int alpha; - if (n_channels == 1) { - alpha = 1; - } else { - alpha = *(p + n_channels - 1); - } - - if (loadParameters->negate) { - color_avg = 255 - color_avg; - } - -#define MAP_IDX(sx, i, j) ((sx) * (j) + (i)) - - unsigned char value; - if (loadParameters->mode == RAW) { - value = color_avg; - msg_->data[MAP_IDX(msg_->info.width, i, msg_->info.height - j - 1)] = value; - continue; + double sum = 0; + for (auto c : channels) { + sum += c; } + /// on a scale from 0.0 to 1.0 how bright is the pixel? + double shade = Magick::ColorGray::scaleQuantumToDouble(sum / channels.size()); // If negate is true, we consider blacker pixels free, and whiter - // pixels free. Otherwise, it's vice versa. - double occ = (255 - color_avg) / 255.0; - - // Apply thresholds to RGB means to determine occupancy values for - // map. Note that we invert the graphics-ordering of the pixels to - // produce a map with cell (0,0) in the lower-left corner. - if (occ > loadParameters->occupied_thresh) { - value = +100; - } else if (occ < loadParameters->free_thresh) { - value = 0; - } else if (loadParameters->mode == TRINARY || alpha < 1.0) { - value = -1; - } else { - double ratio = (occ - loadParameters->free_thresh) / - (loadParameters->occupied_thresh - loadParameters->free_thresh); - value = 99 * ratio; + // pixels occupied. Otherwise, it's vice versa. + /// on a scale from 0.0 to 1.0, how occupied is the map cell (before thresholding)? + double occ = (loadParameters.negate ? shade : 1.0 - shade); + + int8_t map_cell; + switch (loadParameters.mode) { + case MapMode::Trinary: + if (loadParameters.occupied_thresh < occ) { + map_cell = 100; + } else if (occ < loadParameters.free_thresh) { + map_cell = 0; + } else { + map_cell = -1; + } + break; + case MapMode::Scale: + if (pixel.alphaQuantum() != OpaqueOpacity) { + map_cell = -1; + } else if (loadParameters.occupied_thresh < occ) { + map_cell = 100; + } else if (occ < loadParameters.free_thresh) { + map_cell = 0; + } else { + map_cell = std::rint( + (occ - loadParameters.free_thresh) / + (loadParameters.occupied_thresh - loadParameters.free_thresh) * 100.0); + } + break; + case MapMode::Raw: { + double occ_percent = std::round(shade * 255); + if (0 <= occ_percent && occ_percent <= 100) { + map_cell = static_cast(occ_percent); + } else { + map_cell = -1; + } + break; + } + default: + throw std::runtime_error("Invalid map mode"); } - - msg_->data[MAP_IDX(msg_->info.width, i, msg_->info.height - j - 1)] = value; + msg.data[msg.info.width * (msg.info.height - y - 1) + x] = map_cell; } } - SDL_FreeSurface(img); + msg.info.map_load_time = node_->now(); + msg.header.frame_id = frame_id_; + msg.header.stamp = node_->now(); - msg_->info.map_load_time = node_->now(); - msg_->header.frame_id = frame_id_; - msg_->header.stamp = node_->now(); + RCLCPP_DEBUG( + node_->get_logger(), "Read map %s: %d X %d map @ %.3lf m/cell", + loadParameters.image_file_name.c_str(), msg.info.width, msg.info.height, msg.info.resolution); - RCLCPP_DEBUG(node_->get_logger(), "Read map %s: %d X %d map @ %.3lf m/cell", - map_name.c_str(), - msg_->info.width, - msg_->info.height, - msg_->info.resolution); + *msg_ = msg; } } // namespace nav2_map_server diff --git a/nav2_map_server/test/component/test_occ_grid_node.cpp b/nav2_map_server/test/component/test_occ_grid_node.cpp index fc7197622c..37918323aa 100644 --- a/nav2_map_server/test/component/test_occ_grid_node.cpp +++ b/nav2_map_server/test/component/test_occ_grid_node.cpp @@ -60,7 +60,8 @@ class TestNode : public ::testing::Test auto result = client->async_send_request(request); // Wait for the result - if (rclcpp::spin_until_future_complete(node, result) == + if ( + rclcpp::spin_until_future_complete(node, result) == rclcpp::executor::FutureReturnCode::SUCCESS) { return result.get(); diff --git a/nav2_map_server/test/unit/test_occ_grid.cpp b/nav2_map_server/test/unit/test_occ_grid.cpp index 4ccfe7a3aa..0b14e3682f 100644 --- a/nav2_map_server/test/unit/test_occ_grid.cpp +++ b/nav2_map_server/test/unit/test_occ_grid.cpp @@ -1,3 +1,5 @@ +// Copyright 2019 Rover Robotics + /* * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. @@ -129,29 +131,27 @@ class MapLoaderTest : public ::testing::Test // Try to load a valid PNG file. Succeeds if no exception is thrown, and if // the loaded image matches the known dimensions and content of the file. -// -// This test can fail on OS X, due to an apparent limitation of the -// underlying SDL_Image library. TEST_F(MapLoaderTest, loadValidPNG) { auto test_png = path(TEST_DIR) / path(g_valid_png_file); TestMapLoader::LoadParameters loadParameters; + loadParameters.image_file_name = test_png; loadParameters.resolution = g_valid_image_res; loadParameters.origin[0] = 0; loadParameters.origin[1] = 0; loadParameters.origin[2] = 0; loadParameters.free_thresh = 0.196; loadParameters.occupied_thresh = 0.65; - loadParameters.mode = TestMapLoader::TRINARY; + loadParameters.mode = nav2_map_server::MapMode::Trinary; loadParameters.negate = 0; // In order to loadMapFromFile without going through the Configure and Activate states, // the msg_ member must be initialized map_loader_->msg_ = std::make_unique(); - ASSERT_NO_THROW(map_loader_->loadMapFromFile(test_png.string(), &loadParameters)); + ASSERT_NO_THROW(map_loader_->loadMapFromFile(loadParameters)); nav_msgs::msg::OccupancyGrid map_msg = map_loader_->getOccupancyGrid(); EXPECT_FLOAT_EQ(map_msg.info.resolution, g_valid_image_res); @@ -170,20 +170,21 @@ TEST_F(MapLoaderTest, loadValidBMP) auto test_bmp = path(TEST_DIR) / path(g_valid_bmp_file); TestMapLoader::LoadParameters loadParameters; + loadParameters.image_file_name = test_bmp; loadParameters.resolution = g_valid_image_res; loadParameters.origin[0] = 0; loadParameters.origin[1] = 0; loadParameters.origin[2] = 0; loadParameters.free_thresh = 0.196; loadParameters.occupied_thresh = 0.65; - loadParameters.mode = TestMapLoader::TRINARY; + loadParameters.mode = nav2_map_server::MapMode::Trinary; loadParameters.negate = 0; // In order to loadMapFromFile without going through the Configure and Activate states, // the msg_ member must be initialized map_loader_->msg_ = std::make_unique(); - ASSERT_NO_THROW(map_loader_->loadMapFromFile(test_bmp.string(), &loadParameters)); + ASSERT_NO_THROW(map_loader_->loadMapFromFile(loadParameters)); nav_msgs::msg::OccupancyGrid map_msg = map_loader_->getOccupancyGrid(); EXPECT_FLOAT_EQ(map_msg.info.resolution, g_valid_image_res); @@ -201,15 +202,15 @@ TEST_F(MapLoaderTest, loadInvalidFile) auto test_invalid = path(TEST_DIR) / path("foo"); TestMapLoader::LoadParameters loadParameters; + loadParameters.image_file_name = test_invalid; loadParameters.resolution = g_valid_image_res; loadParameters.origin[0] = 0; loadParameters.origin[1] = 0; loadParameters.origin[2] = 0; loadParameters.free_thresh = 0.196; loadParameters.occupied_thresh = 0.65; - loadParameters.mode = TestMapLoader::TRINARY; + loadParameters.mode = nav2_map_server::MapMode::Trinary; loadParameters.negate = 0; - ASSERT_THROW(map_loader_->loadMapFromFile( - test_invalid.string(), &loadParameters), std::runtime_error); + ASSERT_ANY_THROW(map_loader_->loadMapFromFile(loadParameters)); }