Skip to content

Commit 89ece96

Browse files
AlexeyMerzlyakovAlexeyMerzlyakov
committed
Fixing review comments
* Rename mapio -> map_io * Move all OccGridLoader functionality into MapServer. Remove OccGridLoader * Switch all thresholds to be floating-point * Switch loadMapFromYaml() returning type to LOAD_MAP_STATUS and remove duplicating code from loadMapResponseFromYaml() * Make mapCallback() to be lambda-function * Make saveMapCallback() to be class method * Utilize local rclcpp_node_ from LifecycleNode instead of map_listener_. Remove map_listener_ and got_map_msg_ variables. * Rename load_map_callback() -> loadMapCallback() and make it to be class method * Rename handle_occ_callback() -> getMapCallback() and make it to be class method * Force saveMapTopicToFile() and saveMapToFile() to work with constant arguments * map_saver_cli: move arguments parsing code into new parse_arguments() function * Rename test_occ_grid_node -> test_map_server_node and fix test * Rename test_occ_grid -> test_occ_grid and fix test * Fix copyrights * Fix comments * Update README
1 parent 8256121 commit 89ece96

20 files changed

Lines changed: 532 additions & 741 deletions

‎nav2_map_server/CMakeLists.txt‎

Lines changed: 11 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -34,20 +34,19 @@ add_executable(${map_saver_cli_executable}
3434
add_executable(${map_saver_server_executable}
3535
src/map_saver_server_main.cpp)
3636

37-
set(mapio_library_name mapio)
37+
set(map_io_library_name map_io)
3838

3939
set(library_name ${map_server_executable}_core)
4040

41-
add_library(${mapio_library_name} SHARED
41+
add_library(${map_io_library_name} SHARED
4242
src/map_mode.cpp
43-
src/mapio.cpp)
43+
src/map_io.cpp)
4444

4545
add_library(${library_name} SHARED
46-
src/occ_grid_loader.cpp
4746
src/map_server.cpp
4847
src/map_saver.cpp)
4948

50-
set(mapio_dependencies
49+
set(map_io_dependencies
5150
yaml_cpp_vendor
5251
nav_msgs
5352
nav2_util
@@ -80,11 +79,11 @@ ament_target_dependencies(${map_saver_server_executable}
8079
ament_target_dependencies(${library_name}
8180
${map_server_dependencies})
8281

83-
ament_target_dependencies(${mapio_library_name}
84-
${mapio_dependencies})
82+
ament_target_dependencies(${map_io_library_name}
83+
${map_io_dependencies})
8584

8685
target_link_libraries(${library_name}
87-
${mapio_library_name})
86+
${map_io_library_name})
8887

8988
target_link_libraries(${map_server_executable}
9089
${library_name})
@@ -95,13 +94,13 @@ target_link_libraries(${map_saver_cli_executable}
9594
target_link_libraries(${map_saver_server_executable}
9695
${library_name})
9796

98-
target_include_directories(${mapio_library_name} SYSTEM PRIVATE
97+
target_include_directories(${map_io_library_name} SYSTEM PRIVATE
9998
${GRAPHICSMAGICKCPP_INCLUDE_DIRS})
10099

101-
target_link_libraries(${mapio_library_name}
100+
target_link_libraries(${map_io_library_name}
102101
${GRAPHICSMAGICKCPP_LIBRARIES})
103102

104-
install(TARGETS ${map_server_executable} ${library_name} ${mapio_library_name}
103+
install(TARGETS ${map_server_executable} ${library_name} ${map_io_library_name}
105104
${map_saver_cli_executable} ${map_saver_server_executable}
106105
ARCHIVE DESTINATION lib
107106
LIBRARY DESTINATION lib
@@ -125,6 +124,6 @@ endif()
125124
ament_export_include_directories(include)
126125
ament_export_libraries(
127126
${library_name}
128-
${mapio_library_name}
127+
${map_io_library_name}
129128
)
130129
ament_package()

‎nav2_map_server/README.md‎

Lines changed: 8 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -21,25 +21,20 @@ Currently map server divides into tree parts:
2121

2222
- `map_server`
2323
- `map_saver`
24-
- `mapio` library
24+
- `map_io` library
2525

2626
`map_server` is responsible for loading the map from a file through command-line interface
27-
or by using serice requests. The main concept is to have `MapLoader` abstract base class
28-
and type-specific map loaders which derive from this class. There is currently one such
29-
derived class, the `OccGridLoader`, which converts an input image to an OccupancyGrid and
30-
makes this available via topic and service interfaces. The `MapServer` class is a ROS2 node
31-
that uses the appropriate loader, based on an input parameter.
27+
or by using serice requests.
3228

3329
`map_saver` saves the map into a file. Like `map_server`, it has an ability to save the map from
3430
command-line or by calling a service.
3531

36-
`mapio` - is a map input-output library. The library is designed to be an object-independent
32+
`map_io` - is a map input-output library. The library is designed to be an object-independent
3733
in order to allow easily save/load map from external code just by calling necessary function.
3834
This library is also used by `map_loader` and `map_saver` to work. Currently it contains
3935
OccupancyGrid saving/loading functions moved from the rest part of map server code.
40-
Like `OccGridLoader`, it also designed to be replaceble for a new IO library
41-
(e.g. for library with new map encoding method or any other library supporting costmaps,
42-
multifloor maps, etc...).
36+
It is designed to be replaceble for a new IO library (e.g. for library with new map encoding
37+
method or any other library supporting costmaps, multifloor maps, etc...).
4338

4439
### CLI-usage
4540

@@ -116,11 +111,11 @@ $ ros2 run nav2_map_server map_saver_cli [arguments] [--ros-args ROS remapping a
116111

117112
## Currently Supported Map Types
118113

119-
- Occupancy grid (nav_msgs/msg/OccupancyGrid), via the OccGridLoader
114+
- Occupancy grid (nav_msgs/msg/OccupancyGrid)
120115

121116
## MapIO library
122117

123-
`MapIO` library contains following API functions declared in `mapio.hpp` to work with
118+
`MapIO` library contains following API functions declared in `map_io.hpp` to work with
124119
OccupancyGrid maps:
125120

126121
- loadMapYaml(): Load and parse the given YAML file
@@ -144,6 +139,6 @@ Service usage examples:
144139

145140
```
146141
$ ros2 service call /map_server/load_map nav2_msgs/srv/LoadMap "{map_url: /ros/maps/map.yaml}"
147-
$ ros2 service call /map_saver/save_map nav2_msgs/srv/SaveMap "{map_topic: map, map_url: my_map, image_format: pgm, map_mode: trinary, free_thresh: 25, occupied_thresh: 65}"
142+
$ ros2 service call /map_saver/save_map nav2_msgs/srv/SaveMap "{map_topic: map, map_url: my_map, image_format: pgm, map_mode: trinary, free_thresh: 0.25, occupied_thresh: 0.65}"
148143
```
149144

nav2_map_server/include/nav2_map_server/mapio.hpp renamed to nav2_map_server/include/nav2_map_server/map_io.hpp

Lines changed: 16 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,3 @@
1-
// Copyright (c) 2020 Samsung R&D Institute Russia
21
// Copyright (c) 2018 Intel Corporation
32
//
43
// Licensed under the Apache License, Version 2.0 (the "License");
@@ -15,8 +14,8 @@
1514

1615
/* OccupancyGrid map input-output library */
1716

18-
#ifndef NAV2_MAP_SERVER__MAPIO_HPP_
19-
#define NAV2_MAP_SERVER__MAPIO_HPP_
17+
#ifndef NAV2_MAP_SERVER__MAP_IO_HPP_
18+
#define NAV2_MAP_SERVER__MAP_IO_HPP_
2019

2120
#include <string>
2221
#include <vector>
@@ -40,6 +39,14 @@ struct LoadParameters
4039
bool negate;
4140
};
4241

42+
typedef enum
43+
{
44+
LOAD_MAP_SUCCESS,
45+
MAP_DOES_NOT_EXIST,
46+
INVALID_MAP_METADATA,
47+
INVALID_MAP_DATA
48+
} LOAD_MAP_STATUS;
49+
4350
/**
4451
* @brief Load and parse the given YAML file
4552
* @param yaml_filename Name of the map file passed though parameter
@@ -63,9 +70,9 @@ void loadMapFromFile(
6370
* generate an OccupancyGrid
6471
* @param yaml_file Name of input YAML file
6572
* @param map Output loaded map
66-
* @return true or false
73+
* @return status of map loaded
6774
*/
68-
bool loadMapFromYaml(
75+
LOAD_MAP_STATUS loadMapFromYaml(
6976
const std::string & yaml_file,
7077
nav_msgs::msg::OccupancyGrid & map);
7178

@@ -76,22 +83,21 @@ struct SaveParameters
7683
{
7784
std::string map_file_name{""};
7885
std::string image_format{""};
79-
int free_thresh{0};
80-
int occupied_thresh{0};
86+
double free_thresh{0.0};
87+
double occupied_thresh{0.0};
8188
MapMode mode{MapMode::Trinary};
8289
};
8390

8491
/**
8592
* @brief Write OccupancyGrid map to file
8693
* @param map OccupancyGrid map data
8794
* @param save_parameters Map saving parameters.
88-
* NOTE: save_parameters could be updated during function execution.
8995
* @return true or false
9096
*/
9197
bool saveMapToFile(
9298
const nav_msgs::msg::OccupancyGrid & map,
93-
SaveParameters & save_parameters);
99+
const SaveParameters & save_parameters);
94100

95101
} // namespace nav2_map_server
96102

97-
#endif // NAV2_MAP_SERVER__MAPIO_HPP_
103+
#endif // NAV2_MAP_SERVER__MAP_IO_HPP_

‎nav2_map_server/include/nav2_map_server/map_saver.hpp‎

Lines changed: 45 additions & 20 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,4 @@
1-
// Copyright (c) 2020 Samsung R&D Institute Russia
2-
// Copyright (c) 2018 Intel Corporation
1+
// Copyright (c) 2020 Samsung Research Russia
32
//
43
// Licensed under the Apache License, Version 2.0 (the "License");
54
// you may not use this file except in compliance with the License.
@@ -23,7 +22,7 @@
2322
#include "nav2_util/lifecycle_node.hpp"
2423
#include "nav2_msgs/srv/save_map.hpp"
2524

26-
#include "mapio.hpp"
25+
#include "map_io.hpp"
2726

2827
namespace nav2_map_server
2928
{
@@ -48,46 +47,72 @@ class MapSaver : public nav2_util::LifecycleNode
4847
/**
4948
* @brief Read a message from incoming map topic and save map to a file
5049
* @param map_topic Incoming map topic name
51-
* NOTE: map_topic could be updated during function execution.
5250
* @param save_parameters Map saving parameters.
53-
* NOTE: save_parameters could be updated during function execution.
5451
* @return true of false
5552
*/
56-
bool saveMapTopicToFile(std::string & map_topic, SaveParameters & save_parameters);
53+
bool saveMapTopicToFile(
54+
const std::string & map_topic,
55+
const SaveParameters & save_parameters);
5756

5857
protected:
59-
// Lifecycle interfaces
58+
/**
59+
* @brief Sets up map saving service
60+
* @param state Lifecycle Node's state
61+
* @return Success or Failure
62+
*/
6063
nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override;
64+
/**
65+
* @brief Called when node switched to active state
66+
* @param state Lifecycle Node's state
67+
* @return Success or Failure
68+
*/
6169
nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override;
70+
/**
71+
* @brief Called when node switched to inactive state
72+
* @param state Lifecycle Node's state
73+
* @return Success or Failure
74+
*/
6275
nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override;
76+
/**
77+
* @brief Called when it is required node clean-up
78+
* @param state Lifecycle Node's state
79+
* @return Success or Failure
80+
*/
6381
nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override;
82+
/**
83+
* @brief Called when in Shutdown state
84+
* @param state Lifecycle Node's state
85+
* @return Success or Failure
86+
*/
6487
nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override;
88+
/**
89+
* @brief Called when Error is raised
90+
* @param state Lifecycle Node's state
91+
* @return Success or Failure
92+
*/
6593
nav2_util::CallbackReturn on_error(const rclcpp_lifecycle::State & state) override;
6694

6795
/**
68-
* @brief A callback function that receives map message from subscribed topic
69-
* @param map Occupancy Grid message data
96+
* @brief Map saving service callback
97+
* @param request_header Service request header
98+
* @param request Service request
99+
* @param response Service response
70100
*/
71-
void mapCallback(const nav_msgs::msg::OccupancyGrid::SharedPtr msg);
101+
void saveMapCallback(
102+
const std::shared_ptr<rmw_request_id_t> request_header,
103+
const std::shared_ptr<nav2_msgs::srv::SaveMap::Request> request,
104+
std::shared_ptr<nav2_msgs::srv::SaveMap::Response> response);
72105

73106
// The timeout for saving the map in service
74107
std::shared_ptr<rclcpp::Duration> save_map_timeout_;
75108
// Default values for map thresholds
76-
int free_thresh_default_;
77-
int occupied_thresh_default_;
109+
double free_thresh_default_;
110+
double occupied_thresh_default_;
78111

79112
// The name of the service for saving a map from topic
80113
const std::string save_map_service_name_{"save_map"};
81114
// A service to save the map to a file at run time (SaveMap)
82115
rclcpp::Service<nav2_msgs::srv::SaveMap>::SharedPtr save_map_service_;
83-
84-
// Map topic listener node
85-
rclcpp::Node::SharedPtr map_listener_;
86-
87-
// Pointer to map message received in the subscription callback
88-
nav_msgs::msg::OccupancyGrid::SharedPtr msg_;
89-
// Indicator that map message was receiced
90-
bool got_map_msg_;
91116
};
92117

93118
} // namespace nav2_map_server

0 commit comments

Comments
 (0)