25 #include <SimoxUtility/color/Color.h>
26 #include <SimoxUtility/color/GlasbeyLUT.h>
27 #include <SimoxUtility/color/cmaps/colormaps.h>
39 #include <armarx/navigation/core/aron/Graph.aron.generated.h>
40 #include <armarx/navigation/core/aron/Location.aron.generated.h>
42 #include <armarx/navigation/human/aron/Human.aron.generated.h>
44 #include <armarx/navigation/rooms/aron/Room.aron.generated.h>
57 locSegment(locSegment),
58 graphSegment(graphSegment),
59 costmapSegment(costmapSegment),
60 humanSegment(humanSegment),
61 roomsSegment(roomsSegment),
62 visu(
std::make_unique<graph::GraphVisu>())
72 const std::vector<ObjectInfo>& info,
75 using namespace armem::server;
77 std::map<armem::MemoryID, location::arondto::Location> locations;
86 locations[entity.id()].fromAron(instance->data());
91 for (
auto& [
id, location] : locations)
94 fromAron(location.framedPose, framedPose);
97 if (res.pose.has_value())
99 visu->vertex->draw(layer,
id.
str(), res.pose.value());
106 const std::vector<ObjectInfo>& info,
107 std::vector<viz::Layer>& layers,
110 using namespace armem::server;
112 std::map<armem::MemoryID, core::Graph> graphs;
117 [&](
const wm::Entity& entity)
124 navigation::core::arondto::Graph aron;
125 aron.fromAron(instance->data());
126 fromAron(aron, graph);
133 for (
auto& [
id, graph] : graphs)
139 visu->draw(layer, graph, {objects, info});
152 const std::vector<ObjectInfo> info;
159 std::vector<viz::Layer>& layers,
169 const std::vector<ObjectInfo> info = objectFinder.findAllObjects();
179 const std::vector<ObjectInfo> info;
185 std::vector<viz::Layer>& layers,
192 const std::vector<ObjectInfo> info = objectFinder.findAllObjects();
204 const Eigen::Translation3f human_T_mmm(Eigen::Vector3f{0, 0, 1000});
206 ARMARX_INFO <<
"Visualizing " << humans.size() <<
" humans";
207 for (
const auto& human : humans)
221 mmm.file(
"RobotAPI",
"RobotAPI/robots/MMM/mmm.xml");
228 if (human.linearVelocity != Eigen::Vector2f::Zero())
231 vel3d.translation().head<2>() += human.linearVelocity * 2;
233 .fromTo(human3d.translation(), vel3d.translation())
250 std::map<std::string, std::vector<std::pair<std::string, algorithms::Costmap>>>
251 namedProviderCostmaps;
256 using namespace armem::server;
263 navigation::algorithms::Costmap costmap =
264 algorithms::fromAron(*instance);
266 namedProviderCostmaps[instance->id().providerSegmentName].emplace_back(
267 instance->id().entityName, std::move(costmap));
272 for (
const auto& [providerName, namedCostmaps] : namedProviderCostmaps)
275 for (
const auto& [name, costmap] : namedCostmaps)
285 using namespace armem::server;
287 std::map<armem::MemoryID, rooms::Room>
rooms;
299 navigation::rooms::arondto::Room aron;
300 aron.fromAron(instance->data());
301 fromAron(aron, room);
309 for (
auto& [
id, room] :
rooms)
314 const auto color = simox::color::GlasbeyLUT::at(i++);
316 drawRoom(layer, room, color);
328 for (
const auto& point : room.
polygon)
337 polygon.color(color.with_alpha(50));
338 polygon.lineColor(simox::Color::black());
339 polygon.lineWidth(0);
349 const bool visuTransparent,
357 std::map<std::string, navigation::human::Humans> namedProviderHumans;
363 using namespace armem::server;
368 namedProviderHumans[entity.
id().providerSegmentName];
370 [&namedProviderHumans, ×tamp, &maxAge](
374 timestamp - instance.
metadata().referencedTime;
376 if (dtToNow < maxAge and dtToNow.
isPositive())
378 const auto dto = navigation::human::arondto::Human::FromAron(
381 navigation::human::Human human;
382 fromAron(dto, human);
384 namedProviderHumans[instance.id().providerSegmentName]
385 .emplace_back(std::move(human));
391 for (
const auto& [providerName, humans] : namedProviderHumans)
394 visualize(humans, layer, visuTransparent);