30 #include <SimoxUtility/algorithm/string/string_tools.h>
52 #include <armarx/navigation/algorithms/aron/Costmap.aron.generated.h>
54 #include <armarx/navigation/core/aron/Graph.aron.generated.h>
55 #include <armarx/navigation/core/aron/Location.aron.generated.h>
56 #include <armarx/navigation/core/aron/Trajectory.aron.generated.h>
57 #include <armarx/navigation/core/aron/Twist.aron.generated.h>
59 #include <armarx/navigation/human/aron/Human.aron.generated.h>
61 #include <armarx/navigation/memory/aron/LaserScannerFeatures.aron.generated.h>
63 #include <armarx/navigation/rooms/aron/Room.aron.generated.h>
89 def->optional(properties.snapshotToLoad,
91 "Snapshot to load at start up \n"
92 "(e.g. 'R003' to load 'PriorKnowledgeData/navigation-graphs/R003').");
94 def->optional(properties.packageToLoad,
96 "Package to load snapshot at start up \n"
97 "(e.g. 'PriorKnowledgeData').");
99 def->optional(properties.locationGraph.visuLocations,
100 "p.locationGraph.visuLocation",
101 "Enable visualization of locations.");
104 properties.locationGraph.visuRelativeLocations,
105 "p.locationGraph.visuRelativeLocation",
106 "Enable visualization of relative locations (currently supported: robots, objects).");
108 def->optional(properties.locationGraph.visuGraphEdges,
109 "p.locationGraph.visuGraphEdges",
110 "Enable visualization of navigation graph edges.");
113 properties.visuCostmaps,
"p.visuCostmaps",
"Enable visualization of costmaps.");
114 def->optional(properties.zOffsetCostmap,
116 "Adjust the height of the visualized costmap.");
118 def->optional(properties.visuHumans,
"p.visuHumans",
"Enable visualization of humans.");
120 def->optional(properties.visuTransparent,
122 "Enable visualization of humans a bit transparent.");
124 def->optional(properties.visuHumanMaxAgeMs,
125 "p.visuHumanMaxAgeMs",
126 "The maximum age of humans to be drawn in ms.");
128 def->optional(properties.visuRooms,
"p.visuRooms",
"Enable visualization of rooms.");
130 def->optional(properties.visuFrequency,
132 "Visualization frequeny of locations and graph edges [Hz].");
155 algorithms::arondto::Costmap::ToAronType());
158 navigation::core::arondto::GlobalTrajectory::ToAronType());
160 navigation::core::arondto::LocalTrajectory::ToAronType());
172 navigation::location::arondto::Location::ToAronType());
174 navigation::core::arondto::Graph::ToAronType());
177 navigation::human::arondto::Human::ToAronType());
181 navigation::memory::arondto::LaserScannerFeatures::ToAronType())
182 .setMaxHistorySize(properties.laserScannerFeaturesMaxHistorySize);
187 navigation::rooms::arondto::Room::ToAronType());
196 if (properties.locationGraph.visuRelativeLocations)
208 tasks.visuTask->start();
230 return "navigation_memory";
239 tab.locationGraph.setup(*
this);
249 tab.locationGraph.update(*
this);
259 reloadSnapshot =
Button();
260 reloadSnapshot.setLabel(
"Reload from Snapshot '" + owner.properties.snapshotToLoad +
263 visuLocations.setValue(owner.properties.locationGraph.visuLocations);
264 visuGraphEdges.setValue(owner.properties.locationGraph.visuLocations);
266 grid.
add(reloadSnapshot, {row, 0}, {1, 2});
269 grid.
add(
Label(
"Visualize Locations"), {row, 0}).add(visuLocations, {row, 1});
272 grid.
add(
Label(
"Visualize Graph Edges"), {row, 0}).add(visuGraphEdges, {row, 1});
283 if (reloadSnapshot.wasClicked())
285 owner.loadSnapshot();
287 if (visuLocations.hasValueChanged() or visuGraphEdges.hasValueChanged())
289 std::scoped_lock lock(owner.propertiesMutex);
290 owner.properties.locationGraph.visuLocations = visuLocations.getValue();
291 owner.properties.locationGraph.visuGraphEdges = visuGraphEdges.getValue();
308 std::scoped_lock lock(propertiesMutex);
309 pp = properties.locationGraph;
314 while (tasks.visuTask and not tasks.visuTask->isStopped())
319 std::scoped_lock lock(propertiesMutex);
320 pp = properties.locationGraph;
323 std::vector<viz::Layer> layers;
327 if (properties.locationGraph.visuRelativeLocations)
337 if (properties.locationGraph.visuRelativeLocations)
347 visu.drawCostmaps(layers, p.visuCostmaps, p.zOffsetCostmap);
350 visu.drawHumans(layers,
355 visu.drawRooms(layers, p.visuRooms);
359 metronome.waitForNextTick();
364 Component::loadSnapshot()
366 if (properties.snapshotToLoad.empty())
369 <<
"Not loading any predefined locations or graphs as the params was empty ...";
373 const std::vector<std::string> snapshotsToLoad =
376 for (
const auto& snapshotToLoad : snapshotsToLoad)
379 ARMARX_INFO <<
"Loading snapshots from snapshot '" << snapshotToLoad
380 <<
"' in package '" << properties.packageToLoad <<
"' ...";
383 properties.packageToLoad,
387 if (
auto graph = f.find(snapshotToLoad); graph.has_value())
389 const std::string graphId = graph->getID();
390 ARMARX_DEBUG <<
"Found graph " << graphId <<
" with path: " << graph->getFullPath();
398 std::filesystem::path locationsFilePath =
399 graph->getFullPath() /
"locations.json";
400 if (not std::filesystem::is_regular_file(locationsFilePath))
402 ARMARX_WARNING <<
"Found no locations file for graph '" << graphId <<
"'.";
416 std::ifstream ifs(locationsFilePath);
417 const std::string content((std::istreambuf_iterator<char>(ifs)),
418 (std::istreambuf_iterator<char>()));
421 nlohmann::json js = nlohmann::json::parse(content);
427 for (
const auto& location : locations)
430 if (location->type !=
434 <<
"Navigation memory received location '" + location->id.name +
435 "' which is not of type "
436 "'armarx::priorknowledge::util::LocationType:"
437 ":FRAMED_LOCATION' (e.g. you specified a "
438 "FRAMED_BOXED_LOCATION?). Only FramedLocations (i.e. "
439 "poses in 3D space) are navigatable. Ignoring this "
441 "but please check the location.json file.";
445 auto& framedLocation =
449 navigation::location::arondto::Location loc;
452 loc.framedPose.header.
frame = framedLocation.frame;
453 loc.framedPose.header.agent = framedLocation.agent;
454 loc.framedPose.pose = framedLocation.pose;
455 toAron(loc.names, framedLocation.names);
460 up.referencedTime = now;
462 up.arrivedTime = now;
464 up.instancesData = {loc.toAron()};
476 std::filesystem::path graphFilesPath = graph->getFullPath() /
"Graph";
478 if (not std::filesystem::is_directory(graphFilesPath))
480 ARMARX_INFO <<
"Found no `Graph/` folder for graph '" << graphId <<
"'.";
494 up.referencedTime = now;
495 up.arrivedTime = now;
498 up.instancesData = {g.toAron()};
500 ARMARX_DEBUG <<
"Sending empty graph `" << snapshotToLoad
506 ARMARX_DEBUG <<
"Loading graphs from " << graphFilesPath;
515 for (
const auto& graphFile : std::filesystem::directory_iterator(
518 if (std::filesystem::is_regular_file(graphFile.path()) and
522 const std::string graphName = graphFile.path().stem();
525 ARMARX_DEBUG <<
"Loading graph `" << graphName <<
" `from file `"
526 << graphFile.path() <<
"`.";
530 std::ifstream ifs(graphFile.path());
531 const std::string content((std::istreambuf_iterator<char>(ifs)),
532 (std::istreambuf_iterator<char>()));
535 const nlohmann::json j = nlohmann::json::parse(content);
537 const auto& jEdges = j.at(
"edges");
538 const auto& jVertices = j.at(
"vertices");
540 for (
const auto& jVertex : jVertices)
543 jVertex.at(
"locationID"));
545 armarx::navigation::core::arondto::Vertex
v;
546 v.vertexID = jVertex.at(
"vertexID");
547 toAron(
v.locationID, vertexLocationMemoryId);
549 v.locationID.instanceIndex = 0;
551 g.vertices.push_back(
v);
554 for (
const auto& jEdge : jEdges)
556 std::pair<std::int64_t, std::int64_t> edgeSourceTargetPair;
557 jEdge.get_to(edgeSourceTargetPair);
559 armarx::navigation::core::arondto::Edge e;
560 e.sourceVertexID = edgeSourceTargetPair.first;
561 e.targetVertexID = edgeSourceTargetPair.second;
562 g.edges.push_back(e);
567 up.referencedTime = now;
568 up.arrivedTime = now;
571 up.instancesData = {g.toAron()};
582 std::filesystem::path roomsFilePath = graph->getFullPath() /
"rooms.json";
583 if (not std::filesystem::is_regular_file(roomsFilePath))
585 ARMARX_INFO <<
"Found no rooms file for graph '" << graphId <<
"'.";
600 std::ifstream ifs(roomsFilePath);
601 const std::string content((std::istreambuf_iterator<char>(ifs)),
602 (std::istreambuf_iterator<char>()));
605 const nlohmann::json js = nlohmann::json::parse(content);
608 js.at(
"rooms").get<std::map<std::string, nlohmann::json>>();
609 ARMARX_INFO <<
"Found " << rooms.size() <<
" rooms.";
610 for (
const auto& [roomId, j] : rooms)
612 auto roomMemoryId = providerID;
615 const std::string roomMemoryIdStr = roomMemoryId.str();
617 if (j.find(
"polygon") == j.end())
620 <<
"' has no 'polygon' member. Skipping "
625 if (j.find(
"height") == j.end())
628 <<
"' has no 'height' member. "
629 "Skipping this entity.";
633 navigation::rooms::arondto::Room room;
634 j.at(
"polygon").get_to(room.polygon);
635 j.at(
"height").get_to(room.height);
639 for (
auto& point : room.polygon)
647 up.referencedTime = now;
649 up.arrivedTime = now;
651 up.instancesData = {room.toAron()};
658 ARMARX_INFO <<
"Loaded NavigationMemory '" << snapshotToLoad
659 <<
"' --> graph: '" << graphId <<
"'";
663 ARMARX_WARNING <<
"Could not load NavigationMemory '" << snapshotToLoad
664 <<
"'. Continue with empty memory.";
670 store(
const std::map<armem::MemoryID, core::Graph>& graphs,
671 const std::filesystem::path& baseDirectory)
673 ARMARX_INFO <<
"Creating export directory `" << baseDirectory <<
"`.";
674 std::filesystem::create_directories(baseDirectory);
676 for (
const auto& [memoryId, graph] : graphs)
678 const std::filesystem::path nestedSubDir =
679 std::filesystem::path(memoryId.providerSegmentName) / memoryId.coreSegmentName;
680 const std::filesystem::path dir = baseDirectory / nestedSubDir;
682 std::filesystem::create_directories(dir);
687 std::vector<std::pair<std::size_t, std::size_t>> outgoingEdges;
690 std::back_inserter(outgoingEdges),
692 { return std::make_pair(edge.m_source, edge.m_target); });
694 j[
"edges"] = outgoingEdges;
697 for (
const auto& vertex : graph.m_vertices)
701 fromAron(vertex.m_property.aron.locationID, locationId);
703 nlohmann::json jVertex;
705 jVertex[
"vertexID"] = vertex.m_property.aron.vertexID;
707 j[
"vertices"].push_back(jVertex);
711 const std::filesystem::path
filename = dir / (memoryId.entityName +
".json");
714 ofs << std::setw(4) << j;
721 store(
const std::map<armem::MemoryID, location::arondto::Location>& locations,
722 const std::filesystem::path& baseDirectory)
724 ARMARX_INFO <<
"Creating export directory `" << baseDirectory <<
"`.";
725 std::filesystem::create_directories(baseDirectory);
728 std::map<std::string, nlohmann::json> js;
730 for (
const auto& [memoryId, location] : locations)
732 auto& j = js[memoryId.providerSegmentName];
734 if (j.count(
"locations") == 0)
740 nlohmann::json framedPose;
742 framedPose[
"agent"] = location.framedPose.header.agent;
743 framedPose[
"frame"] = location.framedPose.header.frame;
746 std::vector<std::vector<float>> poseAsVec;
748 framedPose[
"pose"] = poseAsVec;
750 jLoc[
"framedPose"] = framedPose;
752 auto entityId = memoryId.getEntityID();
753 j[
"locations"][entityId.entityName] = jLoc;
757 for (
const auto& [providerId, j] : js)
759 const std::filesystem::path subDir = std::filesystem::path(providerId);
760 const std::filesystem::path dir = baseDirectory / subDir;
762 if (not std::filesystem::exists(dir))
764 std::filesystem::create_directories(dir);
767 const std::filesystem::path
filename = dir /
"locations.json";
770 ofs << std::setw(4) << j;
779 const Ice::Current& current)
788 const std::map<armem::MemoryID, location::arondto::Location> locations =
789 [&locationCoreSegment]()
791 std::map<armem::MemoryID, location::arondto::Location> locations;
801 locations[entity.id()].fromAron(instance->data());
813 const auto graphs = [&graphCoreSegment]()
815 std::map<armem::MemoryID, core::Graph> graphs;
827 aron.fromAron(instance->data());
843 store(locations, baseDirectory);