From 872c4c50c2ef3100cd85f61f1b7a076c2f40f163 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Wed, 17 Aug 2022 09:27:21 +0200
Subject: [PATCH] navigation memory: new core segments + visu
---
.../NavigationMemory/CMakeLists.txt | 1 +
.../NavigationMemory/NavigationMemory.cpp | 12 +++-
.../NavigationMemory/NavigationMemory.h | 1 +
.../components/NavigationMemory/Visu.cpp | 68 ++++++++++++++++++-
.../components/NavigationMemory/Visu.h | 6 +-
5 files changed, 85 insertions(+), 3 deletions(-)
diff --git a/source/armarx/navigation/components/NavigationMemory/CMakeLists.txt b/source/armarx/navigation/components/NavigationMemory/CMakeLists.txt
index 3f8da95e..5f88e9cf 100644
--- a/source/armarx/navigation/components/NavigationMemory/CMakeLists.txt
+++ b/source/armarx/navigation/components/NavigationMemory/CMakeLists.txt
@@ -18,6 +18,7 @@ armarx_add_component(navigation_memory
armarx_navigation::graph
armarx_navigation::location
armarx_navigation::algorithms
+ armarx_navigation::teb_human
SOURCES
NavigationMemory.cpp
diff --git a/source/armarx/navigation/components/NavigationMemory/NavigationMemory.cpp b/source/armarx/navigation/components/NavigationMemory/NavigationMemory.cpp
index a18764b0..97d8fa36 100644
--- a/source/armarx/navigation/components/NavigationMemory/NavigationMemory.cpp
+++ b/source/armarx/navigation/components/NavigationMemory/NavigationMemory.cpp
@@ -48,6 +48,7 @@
#include <armarx/navigation/core/aron/Location.aron.generated.h>
#include <armarx/navigation/core/aron/Trajectory.aron.generated.h>
#include <armarx/navigation/core/aron/Twist.aron.generated.h>
+#include <armarx/navigation/human/aron/Human.aron.generated.h>
#include <armarx/navigation/graph/constants.h>
#include <armarx/navigation/location/constants.h>
@@ -127,6 +128,11 @@ namespace armarx::navigation
workingMemory().addCoreSegment(navigation::graph::coreSegmentID.coreSegmentName,
navigation::core::arondto::Graph::ToAronType());
+ workingMemory().addCoreSegment(memory::constants::HumanCoreSegmentName,
+ navigation::human::arondto::Human::ToAronType());
+
+ // workingMemory().addCoreSegment(memory::constants::HumanGroupCoreSegmentName,
+ // navigation::human::arondto::Human::ToAronType());
if (not properties.snapshotToLoad.empty())
{
@@ -432,7 +438,8 @@ namespace armarx::navigation
memory::Visu visu{arviz,
workingMemory().getCoreSegment(navigation::location::coreSegmentID),
workingMemory().getCoreSegment(navigation::graph::coreSegmentID),
- workingMemory().getCoreSegment(memory::constants::CostmapCoreSegmentName)};
+ workingMemory().getCoreSegment(memory::constants::CostmapCoreSegmentName),
+ workingMemory().getCoreSegment(memory::constants::HumanCoreSegmentName)};
Properties::LocationGraph p;
@@ -462,6 +469,9 @@ namespace armarx::navigation
// Costmaps
visu.drawCostmaps(layers, p.visuCostmaps);
+ // Humans
+ visu.drawHumans(layers, p.visuHumans);
+
arviz.commit(layers);
metronome.waitForNextTick();
diff --git a/source/armarx/navigation/components/NavigationMemory/NavigationMemory.h b/source/armarx/navigation/components/NavigationMemory/NavigationMemory.h
index c67da7fc..e34aa21e 100644
--- a/source/armarx/navigation/components/NavigationMemory/NavigationMemory.h
+++ b/source/armarx/navigation/components/NavigationMemory/NavigationMemory.h
@@ -97,6 +97,7 @@ namespace armarx::navigation
bool visuLocations = true;
bool visuGraphEdges = true;
bool visuCostmaps = true;
+ bool visuHumans = true;
float visuFrequency = 2;
};
diff --git a/source/armarx/navigation/components/NavigationMemory/Visu.cpp b/source/armarx/navigation/components/NavigationMemory/Visu.cpp
index 73df72de..f3ae9cbc 100644
--- a/source/armarx/navigation/components/NavigationMemory/Visu.cpp
+++ b/source/armarx/navigation/components/NavigationMemory/Visu.cpp
@@ -22,18 +22,24 @@
#include "Visu.h"
+#include <SimoxUtility/color/Color.h>
#include <SimoxUtility/color/cmaps/colormaps.h>
+#include "RobotAPI/components/ArViz/Client/Elements.h"
#include "RobotAPI/components/ArViz/Client/Layer.h"
#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
#include "armarx/navigation/conversions/eigen.h"
+#include "armarx/navigation/human/aron/Human.aron.generated.h"
+#include "armarx/navigation/human/aron_conversions.h"
+#include "armarx/navigation/human/types.h"
#include <armarx/navigation/algorithms/aron/Costmap.aron.generated.h>
#include <armarx/navigation/algorithms/aron_conversions.h>
#include <armarx/navigation/core/Graph.h>
#include <armarx/navigation/core/aron/Graph.aron.generated.h>
#include <armarx/navigation/core/aron/Location.aron.generated.h>
#include <armarx/navigation/graph/Visu.h>
+#include <armarx/navigation/human/aron/Human.aron.generated.h>
namespace armarx::navigation::memory
@@ -42,11 +48,13 @@ namespace armarx::navigation::memory
Visu::Visu(viz::Client arviz,
const armem::server::wm::CoreSegment& locSegment,
const armem::server::wm::CoreSegment& graphSegment,
- const armem::server::wm::CoreSegment& costmapSegment) :
+ const armem::server::wm::CoreSegment& costmapSegment,
+ const armem::server::wm::CoreSegment& humanSegment) :
arviz(arviz),
locSegment(locSegment),
graphSegment(graphSegment),
costmapSegment(costmapSegment),
+ humanSegment(humanSegment),
visu(std::make_unique<graph::GraphVisu>())
{
}
@@ -165,6 +173,24 @@ namespace armarx::navigation::memory
layer.add(mesh);
}
+ void
+ visualize(const human::Humans& humans, viz::Layer& layer)
+ {
+ ARMARX_INFO << "Visualizing " << humans.size() << " humans";
+ for (const auto& human : humans)
+ {
+ viz::Cylinder cylinder(std::to_string(layer.size()));
+ cylinder.fromTo(conv::to3D(human.pose.translation()),
+ conv::to3D(human.pose.translation()) + Eigen::Vector3f{0, 0, 10});
+
+
+ cylinder.color(simox::Color::orange());
+ cylinder.radius(300);
+
+ layer.add(cylinder);
+ }
+ }
+
} // namespace
void
@@ -207,5 +233,45 @@ namespace armarx::navigation::memory
}
}
+ void
+ Visu::drawHumans(std::vector<viz::Layer>& layers, bool enabled)
+ {
+ if (not enabled)
+ {
+ return;
+ }
+
+ std::map<std::string, navigation::human::Humans> namedProviderHumans;
+
+ humanSegment.doLocked(
+ [&]()
+ {
+ using namespace armem::server;
+
+ humanSegment.forEachEntity(
+ [&](const wm::Entity& entity)
+ {
+ entity.getLatestSnapshot().forEachInstance(
+ [&namedProviderHumans](const armarx::armem::wm::EntityInstance& instance)
+ {
+ const auto dto =
+ navigation::human::arondto::Human::FromAron(instance.data());
+
+ navigation::human::Human human;
+ fromAron(dto, human);
+
+ namedProviderHumans[instance.id().providerSegmentName]
+ .emplace_back(std::move(human));
+ });
+ });
+ });
+
+ for (const auto& [providerName, humans] : namedProviderHumans)
+ {
+ viz::Layer& layer = layers.emplace_back(arviz.layer("humans_" + providerName));
+ visualize(humans, layer);
+ }
+ }
+
} // namespace armarx::navigation::memory
diff --git a/source/armarx/navigation/components/NavigationMemory/Visu.h b/source/armarx/navigation/components/NavigationMemory/Visu.h
index cfd732ad..123a865d 100644
--- a/source/armarx/navigation/components/NavigationMemory/Visu.h
+++ b/source/armarx/navigation/components/NavigationMemory/Visu.h
@@ -28,6 +28,7 @@
#include "RobotAPI/libraries/armem/server/wm/memory_definitions.h"
#include <RobotAPI/components/ArViz/Client/Client.h>
#include <RobotAPI/libraries/armem/core/forward_declarations.h>
+
#include "armarx/navigation/algorithms/Costmap.h"
@@ -45,13 +46,15 @@ namespace armarx::navigation::memory
Visu(viz::Client arviz,
const armem::server::wm::CoreSegment& locSegment,
const armem::server::wm::CoreSegment& graphSegment,
- const armem::server::wm::CoreSegment& costmapSegment);
+ const armem::server::wm::CoreSegment& costmapSegment,
+ const armem::server::wm::CoreSegment& humanSegment);
~Visu();
void drawLocations(std::vector<viz::Layer>& layers, bool enabled);
void drawGraphs(std::vector<viz::Layer>& layers, bool enabled);
void drawCostmaps(std::vector<viz::Layer>& layers, bool enabled);
+ void drawHumans(std::vector<viz::Layer>& layers, bool enabled);
public:
@@ -60,6 +63,7 @@ namespace armarx::navigation::memory
const armem::server::wm::CoreSegment& locSegment;
const armem::server::wm::CoreSegment& graphSegment;
const armem::server::wm::CoreSegment& costmapSegment;
+ const armem::server::wm::CoreSegment& humanSegment;
std::unique_ptr<navigation::graph::GraphVisu> visu;
};
--
GitLab