33 std::string path = p.parent_path().string();
34 if (path.empty())
return path;
36 path.push_back (std::filesystem::path::preferred_separator);
53 if (m_maps_storage.size() == 0)
56 return ReturnValue::return_code::return_value_error_method_failed;
67 return ReturnValue::return_code::return_value_error_method_failed;
72 for (
auto&
it : m_maps_storage)
87 return ReturnValue::return_code::return_value_error_method_failed;
101 return ReturnValue::return_code::return_value_error_method_failed;
107 std::getline(file,
buffer);
113 if (dummy ==
"mapfile:")
127 auto p = m_maps_storage.find(map_name);
128 if (p == m_maps_storage.end())
131 map.
crop(-1, -1, -1, -1);
133 m_maps_storage[map_name] = map;
137 yCError(
MAP2DSTORAGE) <<
"A map with the same name '" << map_name <<
"'was found, skipping...";
157 return ReturnValue::return_code::return_value_error_method_failed;
169 yCInfo(
MAP2DSTORAGE) <<
"Parameter mapCollectionContext not given. No maps/locations will be loaded.";
178 if (m_locations_startup_file_with_path==
"")
185 if (ret) {
yCInfo(
MAP2DSTORAGE) <<
"Location file" << m_locations_startup_file_with_path <<
"successfully loaded."; }
186 else {
yCError(
MAP2DSTORAGE) <<
"Problems opening file" << m_locations_startup_file_with_path; }
189 if (m_collection_startup_file_with_path==
"")
196 yCInfo(
MAP2DSTORAGE) <<
"Map collection file:" << m_collection_startup_file_with_path <<
"successfully loaded.";
199 if (m_maps_storage.size() > 0)
202 for (
auto&
it : m_maps_storage)
213 if (m_locations_storage.size() > 0)
216 for (
auto&
it : m_locations_storage)
227 if (m_objects_storage.size() > 0)
230 for (
auto&
it : m_objects_storage)
241 if (m_areas_storage.size() > 0)
244 for (
auto&
it : m_areas_storage)
255 if (m_paths_storage.size() > 0)
258 for (
auto&
it : m_paths_storage)
270 yCError(
MAP2DSTORAGE) <<
"Unable to load map collection file:" << m_collection_startup_file_with_path;
292bool Map2DStorage::priv_load_locations_and_areas_v1(std::ifstream& file)
295 std::getline(file,
buffer);
296 if (
buffer !=
"Locations:")
304 std::getline(file,
buffer);
327 m_locations_storage[name] =
location;
339 std::getline(file,
buffer);
361 m_areas_storage[name] = area;
366bool Map2DStorage::priv_load_locations_and_areas_v2(std::ifstream& file)
369 std::getline(file,
buffer);
370 if (
buffer !=
"Locations:")
378 std::getline(file,
buffer);
401 m_locations_storage[name] =
location;
413 std::getline(file,
buffer);
438 m_areas_storage[name] = area;
450 std::getline(file,
buffer);
466 if (
ll &&
ll->size() == 4)
470 loc.
x =
ll->get(1).asFloat64();
471 loc.
y =
ll->get(2).asFloat64();
472 loc.
theta =
ll->get(3).asFloat64();
487 m_paths_storage[name] = path;
493bool Map2DStorage::priv_load_locations_and_areas_v3(std::ifstream& file)
496 std::getline(file,
buffer);
500 if (
buffer !=
"Locations:")
508 std::getline(file,
buffer);
532 m_locations_storage[name] =
location;
547 std::getline(file,
buffer);
573 m_areas_storage[name] = area;
588 std::getline(file,
buffer);
604 if (
ll &&
ll->size() == 4)
608 loc.
x =
ll->get(1).asFloat64();
609 loc.
y =
ll->get(2).asFloat64();
610 loc.
theta =
ll->get(3).asFloat64();
626 m_paths_storage[name] = path;
633bool Map2DStorage::priv_load_locations_and_areas_v4(std::ifstream& file)
636 std::getline(file,
buffer);
640 if (
buffer !=
"Locations:")
648 std::getline(file,
buffer);
649 if (
buffer ==
"Objects:") {
674 auto it = m_locations_storage.find(name);
675 if (
it != m_locations_storage.end())
677 yCError(
MAP2DSTORAGE) <<
"The file contains multiple locations with the same name: " << name;
697 std::getline(file,
buffer);
726 auto it = m_objects_storage.find(name);
727 if (
it != m_objects_storage.end())
729 yCError(
MAP2DSTORAGE) <<
"The file contains multiple objects with the same name: " << name;
750 std::getline(file,
buffer);
778 auto it = m_areas_storage.find(name);
779 if (
it != m_areas_storage.end())
786 m_areas_storage[name] = area;
802 std::getline(file,
buffer);
818 if (
ll &&
ll->size() == 4)
822 loc.
x =
ll->get(1).asFloat64();
823 loc.
y =
ll->get(2).asFloat64();
824 loc.
theta =
ll->get(3).asFloat64();
842 auto it = m_paths_storage.find(name);
843 if (
it != m_paths_storage.end())
850 m_paths_storage[name] = path;
866 return ReturnValue::return_code::return_value_error_method_failed;
871 std::getline(file,
buffer);
876 return ReturnValue::return_code::return_value_error_method_failed;
878 std::getline(file,
buffer);
883 if (!priv_load_locations_and_areas_v1(file))
887 return ReturnValue::return_code::return_value_error_method_failed;
890 else if (version == 2)
892 if (!priv_load_locations_and_areas_v2(file))
896 return ReturnValue::return_code::return_value_error_method_failed;
899 else if (version == 3)
901 if (!priv_load_locations_and_areas_v3(file))
905 return ReturnValue::return_code::return_value_error_method_failed;
908 else if (version == 4)
910 if (!priv_load_locations_and_areas_v4(file))
914 return ReturnValue::return_code::return_value_error_method_failed;
921 return ReturnValue::return_code::return_value_error_method_failed;
927 << m_locations_storage.size() <<
"locations and "
928 << m_areas_storage.size() <<
" areas and "
929 << m_paths_storage.size() <<
" paths available";
941 return ReturnValue::return_code::return_value_error_method_failed;
945 std::string q =
"\"";
947 file <<
"Version:\n";
952 file <<
"Locations:\n";
953 std::map<std::string, Map2DLocation>::iterator
it;
954 for (
it = m_locations_storage.begin();
it != m_locations_storage.end(); ++
it)
958 file << q <<
tmp_loc.description << q <<
"\n";
964 file <<
"Objects:\n";
965 std::map<std::string, Map2DObject>::iterator
it;
966 for (
it = m_objects_storage.begin();
it != m_objects_storage.end(); ++
it)
971 file << q <<
tmp_loc.description << q <<
"\n";
978 std::map<std::string, Map2DArea>::iterator
it2;
979 for (
it2 = m_areas_storage.begin();
it2 != m_areas_storage.end(); ++
it2)
983 for (
size_t i = 0; i <
tmp_area.points.size(); i++)
988 file << q <<
tmp_area.description << q;
997 std::map<std::string, Map2DPath>::iterator
it3;
998 for (
it3 = m_paths_storage.begin();
it3 != m_paths_storage.end(); ++
it3)
1001 file <<
it3->first <<
" ";
1002 for (
size_t i=0; i<
it3->second.size(); i++)
1007 file << q <<
tmp_path.description << q;
1020 m_maps_storage.clear();
1027 auto it = m_maps_storage.find(map_name);
1028 if (
it == m_maps_storage.end())
1031 m_maps_storage[map_name] = map;
1036 m_maps_storage[map_name] = map;
1043 auto it = m_maps_storage.find(map_name);
1044 if (
it != m_maps_storage.end())
1049 return ReturnValue::return_code::return_value_error_method_failed;
1055 for (
auto&
it : m_maps_storage)
1057 map_names.push_back(
it.first);
1064 size_t rem = m_maps_storage.erase(map_name);
1067 return ReturnValue::return_code::return_value_error_method_failed;
1077 m_locations_storage.insert(std::pair<std::string, Map2DLocation>(location_name, loc));
1083 m_areas_storage.insert(std::pair<std::string, Map2DArea>(area_name, area));
1089 m_paths_storage.insert(std::pair<std::string, Map2DPath>(path_name, path));
1095 auto it = m_locations_storage.find(location_name);
1096 if (
it != m_locations_storage.end())
1101 return ReturnValue::return_code::return_value_error_method_failed;
1106 auto it = m_areas_storage.find(area_name);
1107 if (
it != m_areas_storage.end())
1112 return ReturnValue::return_code::return_value_error_method_failed;
1117 auto it = m_paths_storage.find(path_name);
1118 if (
it != m_paths_storage.end())
1123 return ReturnValue::return_code::return_value_error_method_failed;
1129 for (
auto&
it : m_locations_storage)
1131 locations.push_back(
it.first);
1139 for (
auto&
it : m_areas_storage)
1141 areas.push_back(
it.first);
1149 for (
auto&
it : m_paths_storage)
1151 paths.push_back(
it.first);
1159 for (
auto&
it : m_locations_storage)
1161 locations.push_back(
it.second);
1168 for (
auto&
it : m_areas_storage)
1170 areas.push_back(
it.second);
1178 for (
auto&
it : m_paths_storage)
1180 paths.push_back(
it.second);
1187 std::map<std::string, Map2DLocation>::iterator
orig_it;
1188 orig_it = m_locations_storage.find(original_name);
1189 std::map<std::string, Map2DLocation>::iterator
new_it;
1190 new_it = m_locations_storage.find(new_name);
1192 if (
orig_it != m_locations_storage.end() &&
1193 new_it == m_locations_storage.end())
1196 m_locations_storage.erase(
orig_it);
1197 m_locations_storage.insert(std::pair<std::string, Map2DLocation>(new_name, loc));
1200 return ReturnValue::return_code::return_value_error_method_failed;
1205 std::map<std::string, Map2DLocation>::iterator
it;
1206 it = m_locations_storage.find(location_name);
1207 if (
it != m_locations_storage.end())
1209 m_locations_storage.erase(
it);
1212 return ReturnValue::return_code::return_value_error_method_failed;
1217 std::map<std::string, Map2DArea>::iterator
it;
1218 it = m_areas_storage.find(area_name);
1219 if (
it != m_areas_storage.end())
1221 m_areas_storage.erase(
it);
1224 return ReturnValue::return_code::return_value_error_method_failed;
1229 std::map<std::string, Map2DPath>::iterator
it;
1230 it = m_paths_storage.find(path_name);
1231 if (
it != m_paths_storage.end())
1233 m_paths_storage.erase(
it);
1236 return ReturnValue::return_code::return_value_error_method_failed;
1242 std::map<std::string, Map2DArea>::iterator
orig_it;
1243 orig_it = m_areas_storage.find(original_name);
1244 std::map<std::string, Map2DArea>::iterator
new_it;
1245 new_it = m_areas_storage.find(new_name);
1247 if (
orig_it != m_areas_storage.end() &&
1248 new_it == m_areas_storage.end())
1251 m_areas_storage.erase(
orig_it);
1252 m_areas_storage.insert(std::pair<std::string, Map2DArea>(new_name, area));
1255 return ReturnValue::return_code::return_value_error_method_failed;
1261 std::map<std::string, Map2DPath>::iterator
orig_it;
1262 orig_it = m_paths_storage.find(original_name);
1263 std::map<std::string, Map2DPath>::iterator
new_it;
1264 new_it = m_paths_storage.find(new_name);
1266 if (
orig_it != m_paths_storage.end() &&
1267 new_it == m_paths_storage.end())
1270 m_paths_storage.erase(
orig_it);
1271 m_paths_storage.insert(std::pair<std::string, Map2DPath>(new_name, area));
1274 return ReturnValue::return_code::return_value_error_method_failed;
1281 m_locations_storage.clear();
1287 m_areas_storage.clear();
1293 m_paths_storage.clear();
1299 for (
auto it = m_maps_storage.begin();
it != m_maps_storage.end();
it++)
1301 it->second.clearMapTemporaryFlags();
1308 auto it = m_maps_storage.find(map_name);
1309 if (
it != m_maps_storage.end())
1311 it->second.clearMapTemporaryFlags();
1316 return ReturnValue::return_code::return_value_error_method_failed;
1324 std::lock_guard<std::mutex> lock(m_mutex);
1327 bool ok = in.
read(connection);
1346 auto p = m_maps_storage.find(map_name);
1347 if (p == m_maps_storage.end())
1350 return ReturnValue::return_code::return_value_error_method_failed;
1354 bool b = p->second.saveToFile(file_name);
1362 yCError(
MAP2DSTORAGE) <<
"save_map failed: unable to save " << map_name <<
" to " << file_name;
1363 return ReturnValue::return_code::return_value_error_method_failed;
1376 auto p = m_maps_storage.find(map_name);
1377 if (p == m_maps_storage.end())
1379 m_maps_storage[map_name] = map;
1386 return ReturnValue::return_code::return_value_error_method_failed;
1392 return ReturnValue::return_code::return_value_error_method_failed;
1400 for (
auto it = m_maps_storage.begin();
it != m_maps_storage.end();
it++)
1402 b &=
it->second.enable_map_compression_over_network(enable);
1412 return ReturnValue::return_code::return_value_error_method_failed;
1417 std::lock_guard<std::mutex> lock(m_mutex);
1418 m_objects_storage[object_name] = obj;
1419 return yarp::dev::ReturnValue_ok;
1424 std::lock_guard<std::mutex> lock(m_mutex);
1425 auto it = m_objects_storage.find(object_name);
1426 if (
it != m_objects_storage.end())
1429 return yarp::dev::ReturnValue_ok;
1436 std::lock_guard<std::mutex> lock(m_mutex);
1438 for (
auto&
it : m_objects_storage)
1440 objects.push_back(
it.first);
1442 return yarp::dev::ReturnValue_ok;
1447 std::lock_guard<std::mutex> lock(m_mutex);
1449 for (
auto&
it : m_objects_storage)
1451 objects.push_back(
it.second);
1453 return yarp::dev::ReturnValue_ok;
1458 std::lock_guard<std::mutex> lock(m_mutex);
1459 auto it = m_objects_storage.find(original_name);
1460 if (
it != m_objects_storage.end())
1462 auto obj =
it->second;
1463 m_objects_storage.erase(
it);
1464 m_objects_storage[new_name] = obj;
1465 return yarp::dev::ReturnValue_ok;
1472 std::lock_guard<std::mutex> lock(m_mutex);
1473 auto it = m_objects_storage.find(object_name);
1474 if (
it != m_objects_storage.end())
1476 m_objects_storage.erase(
it);
1477 return yarp::dev::ReturnValue_ok;
1484 std::lock_guard<std::mutex> lock(m_mutex);
1485 m_objects_storage.clear();
1486 return yarp::dev::ReturnValue_ok;
static std::string extractPathFromFile(std::string full_filename)
static std::string extractPathFromFile(std::string full_filename)
std::string m_mapLocationsFile
bool parseParams(const yarp::os::Searchable &config) override
Parse the DeviceDriver parameters.
std::string m_mapCollectionContext
std::string m_mapCollectionFile
yarp::dev::ReturnValue store_map(const yarp::dev::Nav2D::MapGrid2D &map) override
Stores a map into the map server.
yarp::dev::ReturnValue getObjectsList(std::vector< std::string > &locations) override
Get a list of the names of all stored objects.
yarp::dev::ReturnValue saveMapsCollection(std::string maps_collection_file) override
Save a collection of maps to disk.
yarp::dev::ReturnValue deleteLocation(std::string location_name) override
Delete a location.
yarp::dev::ReturnValue getArea(std::string area_name, yarp::dev::Nav2D::Map2DArea &area) override
Retrieves an area.
yarp::dev::ReturnValue getAllObjects(std::vector< yarp::dev::Nav2D::Map2DObject > &obj) override
Get a list of all stored objects.
bool close() override
Close the DeviceDriver.
yarp::dev::ReturnValue getAllPaths(std::vector< yarp::dev::Nav2D::Map2DPath > &paths) override
Get a list of all stored paths.
yarp::dev::ReturnValue storeObject(std::string object_name, yarp::dev::Nav2D::Map2DObject obj) override
Store a object specified by the user in the world reference frame.
yarp::dev::ReturnValue getLocation(std::string location_name, yarp::dev::Nav2D::Map2DLocation &loc) override
Retrieves a location specified by the user in the world reference frame.
yarp::dev::ReturnValue renameObject(std::string original_name, std::string new_name) override
Searches for a object and renames it.
yarp::dev::ReturnValue getObject(std::string object_name, yarp::dev::Nav2D::Map2DObject &obj) override
Retrieves a object specified by the user in the world reference frame.
yarp::dev::ReturnValue reloadMapsCollection() override
Reloads the last opened file containing the maps from disk.
yarp::dev::ReturnValue enableMapsCompression(bool enable) override
Enable/disables maps compression over the network.
yarp::dev::ReturnValue storeLocation(std::string location_name, yarp::dev::Nav2D::Map2DLocation loc) override
Store a location specified by the user in the world reference frame.
yarp::dev::ReturnValue storeArea(std::string area_name, yarp::dev::Nav2D::Map2DArea area) override
Store an area.
yarp::dev::ReturnValue getAreasList(std::vector< std::string > &areas) override
Get a list of the names of all stored areas.
yarp::dev::ReturnValue clearAllPaths() override
Delete all stored paths.
yarp::dev::ReturnValue storePath(std::string path_name, yarp::dev::Nav2D::Map2DPath path) override
Store a path.
yarp::dev::ReturnValue clearMapTemporaryFlags(std::string map_name) override
Clear all temporary flags from a specific map.
Map2DStorage()
Map2DStorage.
yarp::dev::ReturnValue get_map_names(std::vector< std::string > &map_names) override
Gets a list containing the names of all registered maps.
yarp::dev::ReturnValue getPathsList(std::vector< std::string > &paths) override
Get a list of the names of all stored paths.
yarp::dev::ReturnValue saveLocationsAndExtras(std::string locations_file) override
Save a collection of locations/area/paths etc to disk.
yarp::dev::ReturnValue loadMapsCollection(std::string maps_collection_file) override
Load a collection of maps from disk.
yarp::dev::ReturnValue deletePath(std::string path_name) override
Delete a path.
yarp::dev::ReturnValue clearAllLocations() override
Delete all stored locations.
yarp::dev::ReturnValue getAllAreas(std::vector< yarp::dev::Nav2D::Map2DArea > &areas) override
Get a list of all stored areas.
yarp::dev::ReturnValue remove_map(std::string map_name) override
Removes a map from the map server.
yarp::dev::ReturnValue renameArea(std::string original_name, std::string new_name) override
Searches for an area and renames it.
yarp::dev::ReturnValue loadMapFromDisk(std::string file_name) override
Load a map from disk.
yarp::dev::ReturnValue clearAllMapsTemporaryFlags() override
Clear all temporary flags from all stored maps.
yarp::dev::ReturnValue deleteArea(std::string area_name) override
Delete an area.
yarp::dev::ReturnValue getAllLocations(std::vector< yarp::dev::Nav2D::Map2DLocation > &locations) override
Get a list of all stored locations.
yarp::dev::ReturnValue renamePath(std::string original_name, std::string new_name) override
Searches for a path and renames it.
yarp::dev::ReturnValue get_map(std::string map_name, yarp::dev::Nav2D::MapGrid2D &map) override
Gets a map from the map server.
yarp::dev::ReturnValue saveMapToDisk(std::string map_name, std::string file_name) override
Save a map to disk.
yarp::dev::ReturnValue clearAllAreas() override
Delete all stored areas.
yarp::dev::ReturnValue getPath(std::string path_name, yarp::dev::Nav2D::Map2DPath &path) override
Retrieves a path.
yarp::dev::ReturnValue deleteObject(std::string object_name) override
Delete an object.
yarp::dev::ReturnValue renameLocation(std::string original_name, std::string new_name) override
Searches for a location and renames it.
yarp::dev::ReturnValue reloadLocationsAndExtras() override
Reloads the last opened file containing the collection of locations/areas/paths etc from disk.
yarp::dev::ReturnValue loadLocationsAndExtras(std::string locations_file) override
Load a collection of locations/areas/paths etc from disk.
yarp::dev::ReturnValue clearAllObjects() override
Delete all stored objects.
yarp::dev::ReturnValue getLocationsList(std::vector< std::string > &locations) override
Get a list of the names of all stored locations.
bool open(yarp::os::Searchable ¶ms) override
Open the DeviceDriver.
yarp::dev::ReturnValue clearAllMaps() override
Removes all the registered maps from the server.
std::string description
user defined string
std::string map_id
name of the map
std::vector< yarp::math::Vec2D< double > > points
list of points which define the vertices of the area
std::string map_id
name of the map
double theta
orientation [deg] in the map reference frame
double y
y position of the location [m], expressed in the map reference frame
double x
x position of the location [m], expressed in the map reference frame
std::string description
user defined string
void push_back(yarp::dev::Nav2D::Map2DLocation loc)
Inserts a new location into the path @loc the location to be inserted.
std::string getMapName() const
Retrieves the map name.
bool crop(int left, int top, int right, int bottom)
Modifies the map, cropping pixels at the boundaries.
bool loadFromFile(std::string map_filename)
Loads a yarp map file from disk.
@ return_value_error_method_failed
Method is deprecated.
A simple collection of objects that can be described and transmitted in a portable way.
void fromString(const std::string &text)
Initializes bottle from a string.
size_type size() const
Gets the number of elements in the bottle.
bool read(ConnectionReader &reader) override
Set the bottle's value based on input from a network connection.
Value & get(size_type index) const
Reads a Value v from a certain part of the list.
bool write(ConnectionWriter &writer) const override
Output a representation of the bottle to a network connection.
A mini-server for performing network communication in the background.
void close() override
Stop port activity.
An interface for reading from a network connection.
virtual ConnectionWriter * getWriter()=0
Gets a way to reply to the message, if possible.
An interface for writing to a network connection.
bool setDefaultContext(const std::string &contextName)
Sets the context for the current ResourceFinder object.
std::string findFile(const std::string &name)
Find the full path to a file.
A base class for nested structures that can be searched.
virtual yarp::conf::float64_t asFloat64() const
Get 64-bit floating point value.
virtual std::int32_t asInt32() const
Get 32-bit integer value.
virtual bool isList() const
Checks if value is a list.
virtual Bottle * asList() const
Get list value.
virtual std::string asString() const
Get string value.
#define yCInfo(component,...)
#define yCError(component,...)
#define yCTrace(component,...)
#define yCWarning(component,...)
#define yCDebug(component,...)
#define YARP_LOG_COMPONENT(name,...)
For streams capable of holding different kinds of content, check what they actually have.
An interface to the operating system, including Port based communication.