From 3fde33b2fb36b9f90cb331292a8d1db7b38f91d2 Mon Sep 17 00:00:00 2001 From: Dan Rose Date: Thu, 15 Aug 2019 13:36:35 -0500 Subject: [PATCH] Rewrite nav2_map_server Add GraphicsMagick API and use it to implement map_saver, map_loader Removes dependency on SDL, creates installation dependency on libgraphicsmagick++; apt package libgraphicsmagick++1-dev Allows saving in trinary, raw, or scale mode. Added configurable image format pgm, png, etc. Add warning when using scale mode with opaque format. Remove SDL from map server Extract repeated MapMode to shared enum class Switch writing YAML to same library that reads it (yaml-cpp) Add --help alias to map saver -h Extract map yaml loading to its own function, with better error messages. Also fixed crash due to misuse of std::string constructor. --- nav2_map_server/CMakeLists.txt | 55 ++- .../cmake_modules/FindGRAPHICSMAGICKCPP.cmake | 34 ++ .../include/nav2_map_server/map_mode.hpp | 55 +++ .../{map_generator.hpp => map_saver.hpp} | 27 +- .../nav2_map_server/occ_grid_loader.hpp | 23 +- nav2_map_server/package.xml | 6 +- nav2_map_server/src/map_generator.cpp | 118 ------ nav2_map_server/src/map_mode.cpp | 52 +++ nav2_map_server/src/map_saver.cpp | 266 +++++++++---- nav2_map_server/src/map_saver_cli.cpp | 108 ++++++ nav2_map_server/src/occ_grid_loader.cpp | 351 ++++++++---------- .../test/component/test_occ_grid_node.cpp | 3 +- nav2_map_server/test/unit/test_occ_grid.cpp | 21 +- 13 files changed, 682 insertions(+), 437 deletions(-) create mode 100644 nav2_map_server/cmake_modules/FindGRAPHICSMAGICKCPP.cmake create mode 100644 nav2_map_server/include/nav2_map_server/map_mode.hpp rename nav2_map_server/include/nav2_map_server/{map_generator.hpp => map_saver.hpp} (64%) delete mode 100644 nav2_map_server/src/map_generator.cpp create mode 100644 nav2_map_server/src/map_mode.cpp create mode 100644 nav2_map_server/src/map_saver_cli.cpp 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)); }