├── CHANGELOG.rst ├── CMakeLists.txt ├── ReadMe.md ├── include └── map_server │ └── image_loader.h ├── package.xml ├── scripts ├── crop_map └── crop_map.py ├── src ├── image_loader.cpp ├── main.cpp ├── map_saver.cpp └── map_server.dox └── test ├── consumer.py ├── rtest.cpp ├── rtest.xml ├── test_constants.cpp ├── test_constants.h ├── testmap.bmp ├── testmap.png ├── testmap.yaml └── utest.cpp /CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package map_server 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.12.13 (2016-08-15) 6 | -------------------- 7 | 8 | 1.12.12 (2016-06-24) 9 | -------------------- 10 | 11 | 1.12.11 (2016-06-08) 12 | -------------------- 13 | 14 | 1.12.10 (2016-05-27) 15 | -------------------- 16 | 17 | 1.12.9 (2016-05-26) 18 | ------------------- 19 | 20 | 1.12.8 (2016-05-16) 21 | ------------------- 22 | * Corrections to alpha channel detection and usage. 23 | * Removing some trailing whitespace. 24 | * Use enum to control map interpretation 25 | * Contributors: Aaron Hoy, David Lu 26 | 27 | 1.12.7 (2016-01-05) 28 | ------------------- 29 | 30 | 1.12.6 (2016-01-02) 31 | ------------------- 32 | 33 | 1.12.5 (2015-10-29) 34 | ------------------- 35 | 36 | 1.12.4 (2015-06-03) 37 | ------------------- 38 | 39 | 1.12.3 (2015-04-30) 40 | ------------------- 41 | 42 | 1.12.2 (2015-03-31) 43 | ------------------- 44 | 45 | 1.12.1 (2015-03-14) 46 | ------------------- 47 | 48 | 1.12.0 (2015-02-04) 49 | ------------------- 50 | * update maintainer email 51 | * Contributors: Michael Ferguson 52 | 53 | 1.11.15 (2015-02-03) 54 | -------------------- 55 | 56 | 1.11.14 (2014-12-05) 57 | -------------------- 58 | * prevent inf loop 59 | * Contributors: Jeremie Deray 60 | 61 | 1.11.13 (2014-10-02) 62 | -------------------- 63 | 64 | 1.11.12 (2014-10-01) 65 | -------------------- 66 | * map_server: [style] alphabetize dependencies 67 | * map_server: remove vestigial export line 68 | the removed line does not do anything in catkin 69 | * Contributors: William Woodall 70 | 71 | 1.11.11 (2014-07-23) 72 | -------------------- 73 | 74 | 1.11.10 (2014-06-25) 75 | -------------------- 76 | 77 | 1.11.9 (2014-06-10) 78 | ------------------- 79 | 80 | 1.11.8 (2014-05-21) 81 | ------------------- 82 | * fix build, was broken by `#175 `_ 83 | * Contributors: Michael Ferguson 84 | 85 | 1.11.7 (2014-05-21) 86 | ------------------- 87 | * make rostest in CMakeLists optional 88 | * Contributors: Lukas Bulwahn 89 | 90 | 1.11.5 (2014-01-30) 91 | ------------------- 92 | * install crop map 93 | * removing .py from executable script 94 | * Map Server can serve maps with non-lethal values 95 | * Added support for YAML-CPP 0.5+. 96 | The new yaml-cpp API removes the "node >> outputvar;" operator, and 97 | it has a new way of loading documents. There's no version hint in the 98 | library's headers, so I'm getting the version number from pkg-config. 99 | * check for CATKIN_ENABLE_TESTING 100 | * Change maintainer from Hersh to Lu 101 | 102 | 1.11.4 (2013-09-27) 103 | ------------------- 104 | * prefix utest target to not collide with other targets 105 | * Package URL Updates 106 | * unique target names to avoid conflicts (e.g. with map-store) 107 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(map_server) 3 | 4 | find_package(catkin REQUIRED 5 | COMPONENTS 6 | roscpp 7 | tf 8 | nav_msgs 9 | ) 10 | 11 | find_package(Boost REQUIRED COMPONENTS system) 12 | 13 | find_package(PkgConfig) 14 | pkg_check_modules(NEW_YAMLCPP yaml-cpp>=0.5) 15 | if(NEW_YAMLCPP_FOUND) 16 | add_definitions(-DHAVE_NEW_YAMLCPP) 17 | endif(NEW_YAMLCPP_FOUND) 18 | 19 | catkin_package( 20 | INCLUDE_DIRS 21 | include 22 | LIBRARIES 23 | image_loader 24 | CATKIN_DEPENDS 25 | roscpp 26 | tf 27 | nav_msgs 28 | ) 29 | 30 | include_directories( include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} ) 31 | add_library(image_loader src/image_loader.cpp) 32 | target_link_libraries(image_loader SDL SDL_image ${Boost_LIBRARIES}) 33 | 34 | add_executable(map_server src/main.cpp) 35 | target_link_libraries(map_server 36 | image_loader 37 | yaml-cpp 38 | ${catkin_LIBRARIES} 39 | ) 40 | 41 | add_executable(map_server-map_saver src/map_saver.cpp) 42 | set_target_properties(map_server-map_saver PROPERTIES OUTPUT_NAME map_saver) 43 | target_link_libraries(map_server-map_saver 44 | ${catkin_LIBRARIES} 45 | ) 46 | 47 | # copy test data to same place as tests are run 48 | function(copy_test_data) 49 | cmake_parse_arguments(PROJECT "" "" "FILES" ${ARGN}) 50 | foreach(datafile ${PROJECT_FILES}) 51 | file(COPY ${datafile} DESTINATION ${PROJECT_BINARY_DIR}/test) 52 | endforeach() 53 | endfunction() 54 | 55 | ## Tests 56 | if(CATKIN_ENABLE_TESTING) 57 | copy_test_data( FILES 58 | test/testmap.bmp 59 | test/testmap.png ) 60 | catkin_add_gtest(${PROJECT_NAME}_utest test/utest.cpp test/test_constants.cpp) 61 | target_link_libraries(${PROJECT_NAME}_utest image_loader SDL SDL_image) 62 | 63 | add_executable(rtest test/rtest.cpp test/test_constants.cpp) 64 | target_link_libraries( rtest 65 | gtest 66 | ${catkin_LIBRARIES} 67 | ) 68 | add_dependencies(rtest nav_msgs_gencpp) 69 | 70 | # This has to be done after we've already built targets, or catkin variables get borked 71 | find_package(rostest) 72 | add_rostest(test/rtest.xml) 73 | endif() 74 | 75 | ## Install executables and/or libraries 76 | install(TARGETS map_server-map_saver map_server image_loader 77 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 78 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 79 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 80 | 81 | ## Install project namespaced headers 82 | install(DIRECTORY include/${PROJECT_NAME}/ 83 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 84 | FILES_MATCHING PATTERN "*.h" 85 | PATTERN ".svn" EXCLUDE) 86 | 87 | ## Install excutable python script 88 | install( 89 | PROGRAMS 90 | scripts/crop_map 91 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 92 | -------------------------------------------------------------------------------- /ReadMe.md: -------------------------------------------------------------------------------- 1 | The map_server was a part of [Navigation Stack](http://wiki.ros.org/map_server?distro=melodic) 2 | 3 | It provides the map_saver to save the map topic(nav_msgs/OccupancyGrid) to disk in `.pgm` + `.yaml` form. 4 | 5 | While Cartographer's map topic is different from other's like GMapping, so we need to change some source code. 6 | 7 | | point type | GMapping value | Cartographer value | 8 | |:----:|:----:|:----:| 9 | | free | 0 | 0-N | 10 | | occupied | 100 | M-100 | 11 | | unknown | -1 | -1 & N-M | 12 | 13 | This package change the map_saver.cpp in function `void mapCallback(const nav_msgs::OccupancyGridConstPtr& map)` by 14 | 15 | ```cpp 16 | for(unsigned int y = 0; y < map->info.height; y++) { 17 | for(unsigned int x = 0; x < map->info.width; x++) { 18 | unsigned int i = x + (map->info.height - y - 1) * map->info.width; 19 | if (map->data[i] >= 0 && map->data[i] < 40) { //occ [0,0.1) 20 | fputc(254, out); 21 | } else if (map->data[i] > +50) { //occ (0.65,1] 22 | fputc(000, out); 23 | } else { //occ [0.1,0.65] 24 | fputc(205, out); 25 | } 26 | } 27 | } 28 | ``` 29 | 30 | You can change the value by your need. 31 | 32 | You can add this package to your workspace, make it and run it by `rosrun map_server map_saver [-f mapname]` 33 | 34 | If your workspace can not work with Cartographer, you can find `map_saver` in `your_ws/devel/lib/map_server` and run directly by `./map_saver [-f mapname]`. 35 | 36 | It should save the map in your current path or in the specific path if you point out by mapname 37 | -------------------------------------------------------------------------------- /include/map_server/image_loader.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2008, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | #ifndef MAP_SERVER_MAP_SERVER_H 30 | #define MAP_SERVER_MAP_SERVER_H 31 | 32 | /* 33 | * Author: Brian Gerkey 34 | */ 35 | 36 | #include "nav_msgs/GetMap.h" 37 | 38 | /** Map mode 39 | * Default: TRINARY - 40 | * value >= occ_th - Occupied (100) 41 | * value <= free_th - Free (0) 42 | * otherwise - Unknown 43 | * SCALE - 44 | * alpha < 1.0 - Unknown 45 | * value >= occ_th - Occupied (100) 46 | * value <= free_th - Free (0) 47 | * otherwise - f( (free_th, occ_th) ) = (0, 100) 48 | * (linearly map in between values to (0,100) 49 | * RAW - 50 | * value = value 51 | */ 52 | enum MapMode {TRINARY, SCALE, RAW}; 53 | 54 | namespace map_server 55 | { 56 | 57 | /** Read the image from file and fill out the resp object, for later 58 | * use when our services are requested. 59 | * 60 | * @param resp The map wil be written into here 61 | * @param fname The image file to read from 62 | * @param res The resolution of the map (gets stored in resp) 63 | * @param negate If true, then whiter pixels are occupied, and blacker 64 | * pixels are free 65 | * @param occ_th Threshold above which pixels are occupied 66 | * @param free_th Threshold below which pixels are free 67 | * @param origin Triple specifying 2-D pose of lower-left corner of image 68 | * @param mode Map mode 69 | * @throws std::runtime_error If the image file can't be loaded 70 | * */ 71 | void loadMapFromFile(nav_msgs::GetMap::Response* resp, 72 | const char* fname, double res, bool negate, 73 | double occ_th, double free_th, double* origin, 74 | MapMode mode=TRINARY); 75 | } 76 | 77 | #endif 78 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | map_server 3 | 1.12.13 4 | 5 | 6 | map_server provides the map_server ROS Node, which offers map data as a ROS Service. It also provides the map_saver command-line utility, which allows dynamically generated maps to be saved to file. 7 | 8 | 9 | Brian Gerkey, Tony Pratkanis 10 | contradict@gmail.com 11 | David V. Lu!! 12 | Michael Ferguson 13 | http://wiki.ros.org/map_server 14 | BSD 15 | 16 | catkin 17 | 18 | nav_msgs 19 | roscpp 20 | rostest 21 | sdl-image 22 | tf 23 | yaml-cpp 24 | 25 | nav_msgs 26 | roscpp 27 | rostest 28 | sdl-image 29 | tf 30 | yaml-cpp 31 | 32 | rospy 33 | 34 | -------------------------------------------------------------------------------- /scripts/crop_map: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import sys 4 | import yaml 5 | from PIL import Image 6 | import math 7 | 8 | def find_bounds(map_image): 9 | x_min = map_image.size[0] 10 | x_end = 0 11 | y_min = map_image.size[1] 12 | y_end = 0 13 | pix = map_image.load() 14 | for x in range(map_image.size[0]): 15 | for y in range(map_image.size[1]): 16 | val = pix[x, y] 17 | if val != 205: # not unknown 18 | x_min = min(x, x_min) 19 | x_end = max(x, x_end) 20 | y_min = min(y, y_min) 21 | y_end = max(y, y_end) 22 | return x_min, x_end, y_min, y_end 23 | 24 | def computed_cropped_origin(map_image, bounds, resolution, origin): 25 | """ Compute the image for the cropped map when map_image is cropped by bounds and had origin before. """ 26 | ox = origin[0] 27 | oy = origin[1] 28 | oth = origin[2] 29 | 30 | # First figure out the delta we have to translate from the lower left corner (which is the origin) 31 | # in the image system 32 | dx = bounds[0] * resolution 33 | dy = (map_image.size[1] - bounds[3]) * resolution 34 | 35 | # Next rotate this by the theta and add to the old origin 36 | 37 | new_ox = ox + dx * math.cos(oth) - dy * math.sin(oth) 38 | new_oy = oy + dx * math.sin(oth) + dy * math.cos(oth) 39 | 40 | return [new_ox, new_oy, oth] 41 | 42 | if __name__ == "__main__": 43 | if len(sys.argv) < 2: 44 | print >> sys.stderr, "Usage: %s map.yaml [cropped.yaml]" % sys.argv[0] 45 | sys.exit(1) 46 | 47 | with open(sys.argv[1]) as f: 48 | map_data = yaml.safe_load(f) 49 | 50 | if len(sys.argv) > 2: 51 | crop_name = sys.argv[2] 52 | if crop_name.endswith(".yaml"): 53 | crop_name = crop_name[:-5] 54 | crop_yaml = crop_name + ".yaml" 55 | crop_image = crop_name + ".pgm" 56 | else: 57 | crop_yaml = "cropped.yaml" 58 | crop_image = "cropped.pgm" 59 | 60 | map_image_file = map_data["image"] 61 | resolution = map_data["resolution"] 62 | origin = map_data["origin"] 63 | 64 | map_image = Image.open(map_image_file) 65 | 66 | bounds = find_bounds(map_image) 67 | 68 | # left, upper, right, lower 69 | cropped_image = map_image.crop((bounds[0], bounds[2], bounds[1] + 1, bounds[3] + 1)) 70 | 71 | cropped_image.save(crop_image) 72 | map_data["image"] = crop_image 73 | map_data["origin"] = computed_cropped_origin(map_image, bounds, resolution, origin) 74 | with open(crop_yaml, "w") as f: 75 | yaml.dump(map_data, f) 76 | 77 | -------------------------------------------------------------------------------- /scripts/crop_map.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import sys 4 | import yaml 5 | from PIL import Image 6 | import math 7 | 8 | def find_bounds(map_image): 9 | x_min = map_image.size[0] 10 | x_end = 0 11 | y_min = map_image.size[1] 12 | y_end = 0 13 | pix = map_image.load() 14 | for x in range(map_image.size[0]): 15 | for y in range(map_image.size[1]): 16 | val = pix[x, y] 17 | if val != 205: # not unknown 18 | x_min = min(x, x_min) 19 | x_end = max(x, x_end) 20 | y_min = min(y, y_min) 21 | y_end = max(y, y_end) 22 | return x_min, x_end, y_min, y_end 23 | 24 | def computed_cropped_origin(map_image, bounds, resolution, origin): 25 | """ Compute the image for the cropped map when map_image is cropped by bounds and had origin before. """ 26 | ox = origin[0] 27 | oy = origin[1] 28 | oth = origin[2] 29 | 30 | # First figure out the delta we have to translate from the lower left corner (which is the origin) 31 | # in the image system 32 | dx = bounds[0] * resolution 33 | dy = (map_image.size[1] - bounds[3]) * resolution 34 | 35 | # Next rotate this by the theta and add to the old origin 36 | 37 | new_ox = ox + dx * math.cos(oth) - dy * math.sin(oth) 38 | new_oy = oy + dx * math.sin(oth) + dy * math.cos(oth) 39 | 40 | return [new_ox, new_oy, oth] 41 | 42 | if __name__ == "__main__": 43 | if len(sys.argv) < 2: 44 | print >> sys.stderr, "Usage: %s map.yaml [cropped.yaml]" % sys.argv[0] 45 | sys.exit(1) 46 | 47 | with open(sys.argv[1]) as f: 48 | map_data = yaml.safe_load(f) 49 | 50 | if len(sys.argv) > 2: 51 | crop_name = sys.argv[2] 52 | if crop_name.endswith(".yaml"): 53 | crop_name = crop_name[:-5] 54 | crop_yaml = crop_name + ".yaml" 55 | crop_image = crop_name + ".pgm" 56 | else: 57 | crop_yaml = "cropped.yaml" 58 | crop_image = "cropped.pgm" 59 | 60 | map_image_file = map_data["image"] 61 | resolution = map_data["resolution"] 62 | origin = map_data["origin"] 63 | 64 | map_image = Image.open(map_image_file) 65 | 66 | bounds = find_bounds(map_image) 67 | 68 | # left, upper, right, lower 69 | cropped_image = map_image.crop((bounds[0], bounds[2], bounds[1] + 1, bounds[3] + 1)) 70 | 71 | cropped_image.save(crop_image) 72 | map_data["image"] = crop_image 73 | map_data["origin"] = computed_cropped_origin(map_image, bounds, resolution, origin) 74 | with open(crop_yaml, "w") as f: 75 | yaml.dump(map_data, f) 76 | 77 | -------------------------------------------------------------------------------- /src/image_loader.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2008, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | /* 31 | * This file contains helper functions for loading images as maps. 32 | * 33 | * Author: Brian Gerkey 34 | */ 35 | 36 | #include 37 | #include 38 | 39 | #include 40 | #include 41 | 42 | // We use SDL_image to load the image from disk 43 | #include 44 | 45 | #include "map_server/image_loader.h" 46 | #include 47 | 48 | // compute linear index for given map coords 49 | #define MAP_IDX(sx, i, j) ((sx) * (j) + (i)) 50 | 51 | namespace map_server 52 | { 53 | 54 | void 55 | loadMapFromFile(nav_msgs::GetMap::Response* resp, 56 | const char* fname, double res, bool negate, 57 | double occ_th, double free_th, double* origin, 58 | MapMode mode) 59 | { 60 | SDL_Surface* img; 61 | 62 | unsigned char* pixels; 63 | unsigned char* p; 64 | unsigned char value; 65 | int rowstride, n_channels, avg_channels; 66 | unsigned int i,j; 67 | int k; 68 | double occ; 69 | int alpha; 70 | int color_sum; 71 | double color_avg; 72 | 73 | // Load the image using SDL. If we get NULL back, the image load failed. 74 | if(!(img = IMG_Load(fname))) 75 | { 76 | std::string errmsg = std::string("failed to open image file \"") + 77 | std::string(fname) + std::string("\""); 78 | throw std::runtime_error(errmsg); 79 | } 80 | 81 | // Copy the image data into the map structure 82 | resp->map.info.width = img->w; 83 | resp->map.info.height = img->h; 84 | resp->map.info.resolution = res; 85 | resp->map.info.origin.position.x = *(origin); 86 | resp->map.info.origin.position.y = *(origin+1); 87 | resp->map.info.origin.position.z = 0.0; 88 | tf::Quaternion q; 89 | q.setRPY(0,0, *(origin+2)); 90 | resp->map.info.origin.orientation.x = q.x(); 91 | resp->map.info.origin.orientation.y = q.y(); 92 | resp->map.info.origin.orientation.z = q.z(); 93 | resp->map.info.origin.orientation.w = q.w(); 94 | 95 | // Allocate space to hold the data 96 | resp->map.data.resize(resp->map.info.width * resp->map.info.height); 97 | 98 | // Get values that we'll need to iterate through the pixels 99 | rowstride = img->pitch; 100 | n_channels = img->format->BytesPerPixel; 101 | 102 | // NOTE: Trinary mode still overrides here to preserve existing behavior. 103 | // Alpha will be averaged in with color channels when using trinary mode. 104 | if (mode==TRINARY || !img->format->Amask) 105 | avg_channels = n_channels; 106 | else 107 | avg_channels = n_channels - 1; 108 | 109 | // Copy pixel data into the map structure 110 | pixels = (unsigned char*)(img->pixels); 111 | for(j = 0; j < resp->map.info.height; j++) 112 | { 113 | for (i = 0; i < resp->map.info.width; i++) 114 | { 115 | // Compute mean of RGB for this pixel 116 | p = pixels + j*rowstride + i*n_channels; 117 | color_sum = 0; 118 | for(k=0;kmap.data[MAP_IDX(resp->map.info.width,i,resp->map.info.height - j - 1)] = value; 133 | continue; 134 | } 135 | 136 | 137 | // If negate is true, we consider blacker pixels free, and whiter 138 | // pixels free. Otherwise, it's vice versa. 139 | occ = (255 - color_avg) / 255.0; 140 | 141 | // Apply thresholds to RGB means to determine occupancy values for 142 | // map. Note that we invert the graphics-ordering of the pixels to 143 | // produce a map with cell (0,0) in the lower-left corner. 144 | if(occ > occ_th) 145 | value = +100; 146 | else if(occ < free_th) 147 | value = 0; 148 | else if(mode==TRINARY || alpha < 1.0) 149 | value = -1; 150 | else { 151 | double ratio = (occ - free_th) / (occ_th - free_th); 152 | value = 99 * ratio; 153 | } 154 | 155 | resp->map.data[MAP_IDX(resp->map.info.width,i,resp->map.info.height - j - 1)] = value; 156 | } 157 | } 158 | 159 | SDL_FreeSurface(img); 160 | } 161 | 162 | } 163 | -------------------------------------------------------------------------------- /src/main.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2008, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | /* Author: Brian Gerkey */ 31 | 32 | #define USAGE "\nUSAGE: map_server \n" \ 33 | " map.yaml: map description file\n" \ 34 | "DEPRECATED USAGE: map_server \n" \ 35 | " map: image file to load\n"\ 36 | " resolution: map resolution [meters/pixel]" 37 | 38 | #include 39 | #include 40 | #include 41 | #include 42 | 43 | #include "ros/ros.h" 44 | #include "ros/console.h" 45 | #include "map_server/image_loader.h" 46 | #include "nav_msgs/MapMetaData.h" 47 | #include "yaml-cpp/yaml.h" 48 | 49 | #ifdef HAVE_NEW_YAMLCPP 50 | // The >> operator disappeared in yaml-cpp 0.5, so this function is 51 | // added to provide support for code written under the yaml-cpp 0.3 API. 52 | template 53 | void operator >> (const YAML::Node& node, T& i) 54 | { 55 | i = node.as(); 56 | } 57 | #endif 58 | 59 | class MapServer 60 | { 61 | public: 62 | /** Trivial constructor */ 63 | MapServer(const std::string& fname, double res) 64 | { 65 | std::string mapfname = ""; 66 | double origin[3]; 67 | int negate; 68 | double occ_th, free_th; 69 | MapMode mode = TRINARY; 70 | std::string frame_id; 71 | ros::NodeHandle private_nh("~"); 72 | private_nh.param("frame_id", frame_id, std::string("map")); 73 | deprecated = (res != 0); 74 | if (!deprecated) { 75 | //mapfname = fname + ".pgm"; 76 | //std::ifstream fin((fname + ".yaml").c_str()); 77 | std::ifstream fin(fname.c_str()); 78 | if (fin.fail()) { 79 | ROS_ERROR("Map_server could not open %s.", fname.c_str()); 80 | exit(-1); 81 | } 82 | #ifdef HAVE_NEW_YAMLCPP 83 | // The document loading process changed in yaml-cpp 0.5. 84 | YAML::Node doc = YAML::Load(fin); 85 | #else 86 | YAML::Parser parser(fin); 87 | YAML::Node doc; 88 | parser.GetNextDocument(doc); 89 | #endif 90 | try { 91 | doc["resolution"] >> res; 92 | } catch (YAML::InvalidScalar) { 93 | ROS_ERROR("The map does not contain a resolution tag or it is invalid."); 94 | exit(-1); 95 | } 96 | try { 97 | doc["negate"] >> negate; 98 | } catch (YAML::InvalidScalar) { 99 | ROS_ERROR("The map does not contain a negate tag or it is invalid."); 100 | exit(-1); 101 | } 102 | try { 103 | doc["occupied_thresh"] >> occ_th; 104 | } catch (YAML::InvalidScalar) { 105 | ROS_ERROR("The map does not contain an occupied_thresh tag or it is invalid."); 106 | exit(-1); 107 | } 108 | try { 109 | doc["free_thresh"] >> free_th; 110 | } catch (YAML::InvalidScalar) { 111 | ROS_ERROR("The map does not contain a free_thresh tag or it is invalid."); 112 | exit(-1); 113 | } 114 | try { 115 | std::string modeS = ""; 116 | doc["mode"] >> modeS; 117 | 118 | if(modeS=="trinary") 119 | mode = TRINARY; 120 | else if(modeS=="scale") 121 | mode = SCALE; 122 | else if(modeS=="raw") 123 | mode = RAW; 124 | else{ 125 | ROS_ERROR("Invalid mode tag \"%s\".", modeS.c_str()); 126 | exit(-1); 127 | } 128 | } catch (YAML::Exception) { 129 | ROS_DEBUG("The map does not contain a mode tag or it is invalid... assuming Trinary"); 130 | mode = TRINARY; 131 | } 132 | try { 133 | doc["origin"][0] >> origin[0]; 134 | doc["origin"][1] >> origin[1]; 135 | doc["origin"][2] >> origin[2]; 136 | } catch (YAML::InvalidScalar) { 137 | ROS_ERROR("The map does not contain an origin tag or it is invalid."); 138 | exit(-1); 139 | } 140 | try { 141 | doc["image"] >> mapfname; 142 | // TODO: make this path-handling more robust 143 | if(mapfname.size() == 0) 144 | { 145 | ROS_ERROR("The image tag cannot be an empty string."); 146 | exit(-1); 147 | } 148 | if(mapfname[0] != '/') 149 | { 150 | // dirname can modify what you pass it 151 | char* fname_copy = strdup(fname.c_str()); 152 | mapfname = std::string(dirname(fname_copy)) + '/' + mapfname; 153 | free(fname_copy); 154 | } 155 | } catch (YAML::InvalidScalar) { 156 | ROS_ERROR("The map does not contain an image tag or it is invalid."); 157 | exit(-1); 158 | } 159 | } else { 160 | private_nh.param("negate", negate, 0); 161 | private_nh.param("occupied_thresh", occ_th, 0.65); 162 | private_nh.param("free_thresh", free_th, 0.196); 163 | mapfname = fname; 164 | origin[0] = origin[1] = origin[2] = 0.0; 165 | } 166 | 167 | ROS_INFO("Loading map from image \"%s\"", mapfname.c_str()); 168 | map_server::loadMapFromFile(&map_resp_,mapfname.c_str(),res,negate,occ_th,free_th, origin, mode); 169 | map_resp_.map.info.map_load_time = ros::Time::now(); 170 | map_resp_.map.header.frame_id = frame_id; 171 | map_resp_.map.header.stamp = ros::Time::now(); 172 | ROS_INFO("Read a %d X %d map @ %.3lf m/cell", 173 | map_resp_.map.info.width, 174 | map_resp_.map.info.height, 175 | map_resp_.map.info.resolution); 176 | meta_data_message_ = map_resp_.map.info; 177 | 178 | service = n.advertiseService("static_map", &MapServer::mapCallback, this); 179 | //pub = n.advertise("map_metadata", 1, 180 | 181 | // Latched publisher for metadata 182 | metadata_pub= n.advertise("map_metadata", 1, true); 183 | metadata_pub.publish( meta_data_message_ ); 184 | 185 | // Latched publisher for data 186 | map_pub = n.advertise("map", 1, true); 187 | map_pub.publish( map_resp_.map ); 188 | } 189 | 190 | private: 191 | ros::NodeHandle n; 192 | ros::Publisher map_pub; 193 | ros::Publisher metadata_pub; 194 | ros::ServiceServer service; 195 | bool deprecated; 196 | 197 | /** Callback invoked when someone requests our service */ 198 | bool mapCallback(nav_msgs::GetMap::Request &req, 199 | nav_msgs::GetMap::Response &res ) 200 | { 201 | // request is empty; we ignore it 202 | 203 | // = operator is overloaded to make deep copy (tricky!) 204 | res = map_resp_; 205 | ROS_INFO("Sending map"); 206 | 207 | return true; 208 | } 209 | 210 | /** The map data is cached here, to be sent out to service callers 211 | */ 212 | nav_msgs::MapMetaData meta_data_message_; 213 | nav_msgs::GetMap::Response map_resp_; 214 | 215 | /* 216 | void metadataSubscriptionCallback(const ros::SingleSubscriberPublisher& pub) 217 | { 218 | pub.publish( meta_data_message_ ); 219 | } 220 | */ 221 | 222 | }; 223 | 224 | int main(int argc, char **argv) 225 | { 226 | ros::init(argc, argv, "map_server", ros::init_options::AnonymousName); 227 | if(argc != 3 && argc != 2) 228 | { 229 | ROS_ERROR("%s", USAGE); 230 | exit(-1); 231 | } 232 | if (argc != 2) { 233 | ROS_WARN("Using deprecated map server interface. Please switch to new interface."); 234 | } 235 | std::string fname(argv[1]); 236 | double res = (argc == 2) ? 0.0 : atof(argv[2]); 237 | 238 | try 239 | { 240 | MapServer ms(fname, res); 241 | ros::spin(); 242 | } 243 | catch(std::runtime_error& e) 244 | { 245 | ROS_ERROR("map_server exception: %s", e.what()); 246 | return -1; 247 | } 248 | 249 | return 0; 250 | } 251 | 252 | -------------------------------------------------------------------------------- /src/map_saver.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * map_saver 3 | * Copyright (c) 2008, Willow Garage, Inc. 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 9 | * * Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * * Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in the 13 | * documentation and/or other materials provided with the distribution. 14 | * * Neither the name of the nor the names of its 15 | * contributors may be used to endorse or promote products derived from 16 | * this software without specific prior written permission. 17 | * 18 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 22 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | * POSSIBILITY OF SUCH DAMAGE. 29 | */ 30 | 31 | #include 32 | #include "ros/ros.h" 33 | #include "ros/console.h" 34 | #include "nav_msgs/GetMap.h" 35 | #include "tf/LinearMath/Matrix3x3.h" 36 | #include "geometry_msgs/Quaternion.h" 37 | 38 | using namespace std; 39 | 40 | /** 41 | * @brief Map generation node. 42 | */ 43 | class MapGenerator 44 | { 45 | 46 | public: 47 | MapGenerator(const std::string& mapname) : mapname_(mapname), saved_map_(false) 48 | { 49 | ros::NodeHandle n; 50 | ROS_INFO("Waiting for the map"); 51 | map_sub_ = n.subscribe("map", 1, &MapGenerator::mapCallback, this); 52 | } 53 | 54 | void mapCallback(const nav_msgs::OccupancyGridConstPtr& map) 55 | { 56 | ROS_INFO("Received a %d X %d map @ %.3f m/pix", 57 | map->info.width, 58 | map->info.height, 59 | map->info.resolution); 60 | 61 | 62 | std::string mapdatafile = mapname_ + ".pgm"; 63 | ROS_INFO("Writing map occupancy data to %s", mapdatafile.c_str()); 64 | FILE* out = fopen(mapdatafile.c_str(), "w"); 65 | if (!out) 66 | { 67 | ROS_ERROR("Couldn't save map file to %s", mapdatafile.c_str()); 68 | return; 69 | } 70 | 71 | fprintf(out, "P5\n# CREATOR: Map_generator.cpp %.3f m/pix\n%d %d\n255\n", 72 | map->info.resolution, map->info.width, map->info.height); 73 | // for(unsigned int y = 0; y < map->info.height; y++) { 74 | // for(unsigned int x = 0; x < map->info.width; x++) { 75 | // unsigned int i = x + (map->info.height - y - 1) * map->info.width; 76 | // if (map->data[i] == 0) { //occ [0,0.1) 77 | // fputc(254, out); 78 | // } else if (map->data[i] == +100) { //occ (0.65,1] 79 | // fputc(000, out); 80 | // } else { //occ [0.1,0.65] 81 | // fputc(205, out); 82 | // } 83 | // } 84 | // } 85 | 86 | // for(unsigned int y = 0; y < map->info.height; y++) { 87 | // for(unsigned int x = 0; x < map->info.width; x++) { 88 | // unsigned int i = x + (map->info.height - y - 1) * map->info.width; 89 | // int pixel = (double)(100.0-map->data[i]) *2.54; 90 | // fputc(pixel, out); 91 | // } 92 | // } 93 | 94 | for(unsigned int y = 0; y < map->info.height; y++) { 95 | for(unsigned int x = 0; x < map->info.width; x++) { 96 | unsigned int i = x + (map->info.height - y - 1) * map->info.width; 97 | if (map->data[i] >= 0 && map->data[i] < 40) { //occ [0,0.1) 98 | fputc(254, out); 99 | } else if (map->data[i] > +50) { //occ (0.65,1] 100 | fputc(000, out); 101 | } else { //occ [0.1,0.65] 102 | fputc(205, out); 103 | } 104 | } 105 | } 106 | 107 | fclose(out); 108 | 109 | 110 | std::string mapmetadatafile = mapname_ + ".yaml"; 111 | ROS_INFO("Writing map occupancy data to %s", mapmetadatafile.c_str()); 112 | FILE* yaml = fopen(mapmetadatafile.c_str(), "w"); 113 | 114 | 115 | /* 116 | resolution: 0.100000 117 | origin: [0.000000, 0.000000, 0.000000] 118 | # 119 | negate: 0 120 | occupied_thresh: 0.65 121 | free_thresh: 0.196 122 | 123 | */ 124 | 125 | geometry_msgs::Quaternion orientation = map->info.origin.orientation; 126 | tf::Matrix3x3 mat(tf::Quaternion(orientation.x, orientation.y, orientation.z, orientation.w)); 127 | double yaw, pitch, roll; 128 | mat.getEulerYPR(yaw, pitch, roll); 129 | 130 | fprintf(yaml, "image: %s\nresolution: %f\norigin: [%f, %f, %f]\nnegate: 0\noccupied_thresh: 0.65\nfree_thresh: 0.196\n\n", 131 | mapdatafile.c_str(), map->info.resolution, map->info.origin.position.x, map->info.origin.position.y, yaw); 132 | 133 | fclose(yaml); 134 | 135 | ROS_INFO("Done\n"); 136 | saved_map_ = true; 137 | } 138 | 139 | std::string mapname_; 140 | ros::Subscriber map_sub_; 141 | bool saved_map_; 142 | 143 | }; 144 | 145 | #define USAGE "Usage: \n" \ 146 | " map_saver -h\n"\ 147 | " map_saver [-f ] [ROS remapping args]" 148 | 149 | int main(int argc, char** argv) 150 | { 151 | ros::init(argc, argv, "map_saver"); 152 | std::string mapname = "map"; 153 | 154 | for(int i=1; i 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | 39 | #include "test_constants.h" 40 | 41 | int g_argc; 42 | char** g_argv; 43 | 44 | class MapClientTest : public testing::Test 45 | { 46 | public: 47 | // A node is needed to make a service call 48 | ros::NodeHandle* n_; 49 | 50 | MapClientTest() 51 | { 52 | ros::init(g_argc, g_argv, "map_client_test"); 53 | n_ = new ros::NodeHandle(); 54 | got_map_ = false; 55 | got_map_metadata_ = false; 56 | } 57 | 58 | ~MapClientTest() 59 | { 60 | delete n_; 61 | } 62 | 63 | bool got_map_; 64 | boost::shared_ptr map_; 65 | void mapCallback(const boost::shared_ptr& map) 66 | { 67 | map_ = map; 68 | got_map_ = true; 69 | } 70 | 71 | bool got_map_metadata_; 72 | boost::shared_ptr map_metadata_; 73 | void mapMetaDataCallback(const boost::shared_ptr& map_metadata) 74 | { 75 | map_metadata_ = map_metadata; 76 | got_map_metadata_ = true; 77 | } 78 | }; 79 | 80 | /* Try to retrieve the map via service, and compare to ground truth */ 81 | TEST_F(MapClientTest, call_service) 82 | { 83 | nav_msgs::GetMap::Request req; 84 | nav_msgs::GetMap::Response resp; 85 | ASSERT_TRUE(ros::service::waitForService("static_map", 5000)); 86 | ASSERT_TRUE(ros::service::call("static_map", req, resp)); 87 | ASSERT_FLOAT_EQ(resp.map.info.resolution, g_valid_image_res); 88 | ASSERT_EQ(resp.map.info.width, g_valid_image_width); 89 | ASSERT_EQ(resp.map.info.height, g_valid_image_height); 90 | ASSERT_STREQ(resp.map.header.frame_id.c_str(), "map"); 91 | for(unsigned int i=0; i < resp.map.info.width * resp.map.info.height; i++) 92 | ASSERT_EQ(g_valid_image_content[i], resp.map.data[i]); 93 | } 94 | 95 | /* Try to retrieve the map via topic, and compare to ground truth */ 96 | TEST_F(MapClientTest, subscribe_topic) 97 | { 98 | ros::Subscriber sub = n_->subscribe("map", 1, boost::bind(&MapClientTest::mapCallback, this, _1)); 99 | 100 | // Try a few times, because the server may not be up yet. 101 | int i=20; 102 | while(!got_map_ && i > 0) 103 | { 104 | ros::spinOnce(); 105 | ros::Duration d = ros::Duration().fromSec(0.25); 106 | d.sleep(); 107 | i--; 108 | } 109 | ASSERT_TRUE(got_map_); 110 | ASSERT_FLOAT_EQ(map_->info.resolution, g_valid_image_res); 111 | ASSERT_EQ(map_->info.width, g_valid_image_width); 112 | ASSERT_EQ(map_->info.height, g_valid_image_height); 113 | ASSERT_STREQ(map_->header.frame_id.c_str(), "map"); 114 | for(unsigned int i=0; i < map_->info.width * map_->info.height; i++) 115 | ASSERT_EQ(g_valid_image_content[i], map_->data[i]); 116 | } 117 | 118 | /* Try to retrieve the metadata via topic, and compare to ground truth */ 119 | TEST_F(MapClientTest, subscribe_topic_metadata) 120 | { 121 | ros::Subscriber sub = n_->subscribe("map_metadata", 1, boost::bind(&MapClientTest::mapMetaDataCallback, this, _1)); 122 | 123 | // Try a few times, because the server may not be up yet. 124 | int i=20; 125 | while(!got_map_metadata_ && i > 0) 126 | { 127 | ros::spinOnce(); 128 | ros::Duration d = ros::Duration().fromSec(0.25); 129 | d.sleep(); 130 | i--; 131 | } 132 | ASSERT_TRUE(got_map_metadata_); 133 | ASSERT_FLOAT_EQ(map_metadata_->resolution, g_valid_image_res); 134 | ASSERT_EQ(map_metadata_->width, g_valid_image_width); 135 | ASSERT_EQ(map_metadata_->height, g_valid_image_height); 136 | } 137 | 138 | int main(int argc, char **argv) 139 | { 140 | testing::InitGoogleTest(&argc, argv); 141 | 142 | g_argc = argc; 143 | g_argv = argv; 144 | 145 | return RUN_ALL_TESTS(); 146 | } 147 | -------------------------------------------------------------------------------- /test/rtest.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /test/test_constants.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2008, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | /* Author: Brian Gerkey */ 31 | 32 | /* This file contains global constants shared among tests */ 33 | 34 | /* Note that these must be changed if the test image changes */ 35 | 36 | #include "test_constants.h" 37 | 38 | const unsigned int g_valid_image_width = 10; 39 | const unsigned int g_valid_image_height = 10; 40 | // Note that the image content is given in row-major order, with the 41 | // lower-left pixel first. This is different from a graphics coordinate 42 | // system, which starts with the upper-left pixel. The loadMapFromFile 43 | // call converts from the latter to the former when it loads the image, and 44 | // we want to compare against the result of that conversion. 45 | const char g_valid_image_content[] = { 46 | 0,0,0,0,0,0,0,0,0,0, 47 | 100,100,100,100,0,0,100,100,100,0, 48 | 100,100,100,100,0,0,100,100,100,0, 49 | 100,0,0,0,0,0,0,0,0,0, 50 | 100,0,0,0,0,0,0,0,0,0, 51 | 100,0,0,0,0,0,100,100,0,0, 52 | 100,0,0,0,0,0,100,100,0,0, 53 | 100,0,0,0,0,0,100,100,0,0, 54 | 100,0,0,0,0,0,100,100,0,0, 55 | 100,0,0,0,0,0,0,0,0,0, 56 | }; 57 | 58 | const char* g_valid_png_file = "test/testmap.png"; 59 | const char* g_valid_bmp_file = "test/testmap.bmp"; 60 | 61 | const float g_valid_image_res = 0.1; 62 | 63 | -------------------------------------------------------------------------------- /test/test_constants.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2008, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | #ifndef MAP_SERVER_TEST_CONSTANTS_H 30 | #define MAP_SERVER_TEST_CONSTANTS_H 31 | 32 | /* Author: Brian Gerkey */ 33 | 34 | /* This file externs global constants shared among tests */ 35 | 36 | extern const unsigned int g_valid_image_width; 37 | extern const unsigned int g_valid_image_height; 38 | extern const char g_valid_image_content[]; 39 | extern const char* g_valid_png_file; 40 | extern const char* g_valid_bmp_file; 41 | extern const float g_valid_image_res; 42 | 43 | #endif 44 | -------------------------------------------------------------------------------- /test/testmap.bmp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HaoQChen/map_server/e766feacab7879df4f1035c3b99f019b4ba32616/test/testmap.bmp -------------------------------------------------------------------------------- /test/testmap.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HaoQChen/map_server/e766feacab7879df4f1035c3b99f019b4ba32616/test/testmap.png -------------------------------------------------------------------------------- /test/testmap.yaml: -------------------------------------------------------------------------------- 1 | image: testmap.png 2 | resolution: 0.1 3 | origin: [2.0, 3.0, 1.0] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | -------------------------------------------------------------------------------- /test/utest.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2008, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | /* Author: Brian Gerkey */ 31 | 32 | #include // for std::runtime_error 33 | #include 34 | #include "map_server/image_loader.h" 35 | #include "test_constants.h" 36 | 37 | /* Try to load a valid PNG file. Succeeds if no exception is thrown, and if 38 | * the loaded image matches the known dimensions and content of the file. 39 | * 40 | * This test can fail on OS X, due to an apparent limitation of the 41 | * underlying SDL_Image library. */ 42 | TEST(MapServer, loadValidPNG) 43 | { 44 | try 45 | { 46 | nav_msgs::GetMap::Response map_resp; 47 | double origin[3] = { 0.0, 0.0, 0.0 }; 48 | map_server::loadMapFromFile(&map_resp, g_valid_png_file, g_valid_image_res, false, 0.65, 0.1, origin); 49 | EXPECT_FLOAT_EQ(map_resp.map.info.resolution, g_valid_image_res); 50 | EXPECT_EQ(map_resp.map.info.width, g_valid_image_width); 51 | EXPECT_EQ(map_resp.map.info.height, g_valid_image_height); 52 | for(unsigned int i=0; i < map_resp.map.info.width * map_resp.map.info.height; i++) 53 | EXPECT_EQ(g_valid_image_content[i], map_resp.map.data[i]); 54 | } 55 | catch(...) 56 | { 57 | ADD_FAILURE() << "Uncaught exception : " << "This is OK on OS X"; 58 | } 59 | } 60 | 61 | /* Try to load a valid BMP file. Succeeds if no exception is thrown, and if 62 | * the loaded image matches the known dimensions and content of the file. */ 63 | TEST(MapServer, loadValidBMP) 64 | { 65 | try 66 | { 67 | nav_msgs::GetMap::Response map_resp; 68 | double origin[3] = { 0.0, 0.0, 0.0 }; 69 | map_server::loadMapFromFile(&map_resp, g_valid_bmp_file, g_valid_image_res, false, 0.65, 0.1, origin); 70 | EXPECT_FLOAT_EQ(map_resp.map.info.resolution, g_valid_image_res); 71 | EXPECT_EQ(map_resp.map.info.width, g_valid_image_width); 72 | EXPECT_EQ(map_resp.map.info.height, g_valid_image_height); 73 | for(unsigned int i=0; i < map_resp.map.info.width * map_resp.map.info.height; i++) 74 | EXPECT_EQ(g_valid_image_content[i], map_resp.map.data[i]); 75 | } 76 | catch(...) 77 | { 78 | ADD_FAILURE() << "Uncaught exception"; 79 | } 80 | } 81 | 82 | /* Try to load an invalid file. Succeeds if a std::runtime_error exception 83 | * is thrown. */ 84 | TEST(MapServer, loadInvalidFile) 85 | { 86 | try 87 | { 88 | nav_msgs::GetMap::Response map_resp; 89 | double origin[3] = { 0.0, 0.0, 0.0 }; 90 | map_server::loadMapFromFile(&map_resp, "foo", 0.1, false, 0.65, 0.1, origin); 91 | } 92 | catch(std::runtime_error &e) 93 | { 94 | SUCCEED(); 95 | return; 96 | } 97 | catch(...) 98 | { 99 | FAIL() << "Uncaught exception"; 100 | } 101 | ADD_FAILURE() << "Didn't throw exception as expected"; 102 | } 103 | 104 | int main(int argc, char **argv) 105 | { 106 | testing::InitGoogleTest(&argc, argv); 107 | return RUN_ALL_TESTS(); 108 | } 109 | --------------------------------------------------------------------------------