diff --git a/examples/components/example_client/Component.cpp b/examples/components/example_client/Component.cpp
index 1110c342da3693c7868daa14a741bca2e0f67711..8d776c194c6d6ecafc9522a77211d0dd54330f24 100644
--- a/examples/components/example_client/Component.cpp
+++ b/examples/components/example_client/Component.cpp
@@ -189,6 +189,7 @@ namespace armarx::navigation::components::example_client
client::NavigationStackConfig()
.general({} /*{.maxVel = VelocityLimits{.linear = 400 , .angular = 0.1}}*/)
.globalPlanner(global_planning::SPFAParams())
+ .localPlanner(local_planning::TimedElasticBandsParams())
.trajectoryController(
traj_ctrl::local::TrajectoryFollowingControllerParams())); // FIXME
@@ -289,6 +290,7 @@ namespace armarx::navigation::components::example_client
client::NavigationStackConfig()
.general({} /*{.maxVel = VelocityLimits{.linear = 400 , .angular = 0.1}}*/)
.globalPlanner(global_planning::SPFAParams())
+ .localPlanner(local_planning::TimedElasticBandsParams())
.trajectoryController(
traj_ctrl::local::TrajectoryFollowingControllerParams())); // FIXME
@@ -366,6 +368,7 @@ namespace armarx::navigation::components::example_client
client::NavigationStackConfig()
.general({} /*{.maxVel = VelocityLimits{.linear = 400 , .angular = 0.1}}*/)
.globalPlanner(global_planning::Point2PointParams())
+ .localPlanner(local_planning::TimedElasticBandsParams())
.trajectoryController(
traj_ctrl::local::TrajectoryFollowingControllerParams())); // FIXME
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp b/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
index 9ddc696aba96611d4b09cbc79757c80da3f62c00..ed8a9f439046eb2983a5baffdc81fb2da5faeef1 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
+++ b/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
@@ -37,17 +37,17 @@
#include "armarx/navigation/components/dynamic_scene_provider/ArVizDrawer.h"
#include <armarx/navigation/core/basic_types.h>
+#include <armarx/navigation/human/types.h>
#include <armarx/navigation/memory/client/costmap/Reader.h>
#include <armarx/navigation/util/util.h>
-
namespace armarx::navigation::components::dynamic_scene_provider
{
const std::string Component::defaultName = "dynamic_scene_provider";
- Component::Component()
+ Component::Component() : humanGrouper(HumanGrouper::GroupingSettings())
{
addPlugin(humanPoseReaderPlugin);
addPlugin(laserScannerFeaturesReaderPlugin);
@@ -55,6 +55,7 @@ namespace armarx::navigation::components::dynamic_scene_provider
addPlugin(costmapReaderPlugin);
addPlugin(occupancyGridReaderPlugin);
addPlugin(humanWriterPlugin);
+ addPlugin(humanGroupWriterPlugin);
}
armarx::PropertyDefinitionsPtr
@@ -189,6 +190,54 @@ namespace armarx::navigation::components::dynamic_scene_provider
return Component::defaultName;
}
+ using Human = armarx::navigation::human::Human;
+
+ void
+ armarx::navigation::components::dynamic_scene_provider::Component::addTestHumans(
+ std::vector<Human>& humans)
+ {
+ DateTime timestamp = Clock::Now();
+
+ ARMARX_INFO << "For testing purposes, we will add another 3 humans!";
+ core::Pose2D p1 = core::Pose2D::Identity();
+ p1.linear() = Eigen::Rotation2Df(1.25 * M_PI).toRotationMatrix();
+ p1.translation() = Eigen::Vector2f(1000, 500);
+ Human h1 = {
+ .pose = p1, .linearVelocity = Eigen::Vector2f(0, 0), .detectionTime = timestamp};
+ core::Pose2D p2 = core::Pose2D::Identity();
+ p2.linear() = Eigen::Rotation2Df(-0.25 * M_PI).toRotationMatrix();
+ p2.translation() = Eigen::Vector2f(1000, -500);
+ Human h2 = {
+ .pose = p2, .linearVelocity = Eigen::Vector2f(0, 0), .detectionTime = timestamp};
+
+
+ // Create third human that swings in a sine wave
+
+ double f_1_s = 0.25f;
+ double omega_1_s = 2 * M_PI * f_1_s;
+ double t_s = timestamp.toDurationSinceEpoch().toSecondsDouble();
+ double sinArg = std::fmod(t_s * omega_1_s, 2 * M_PI);
+ double amplitude_mm = 1000;
+
+ core::Pose2D p3 = core::Pose2D::Identity();
+ p3.linear() = Eigen::Rotation2Df(M_PI_2).toRotationMatrix();
+ // in mm
+ p3.translation() = Eigen::Vector2f(2500 + amplitude_mm * sin(sinArg), 0);
+ Human h3 = {.pose = p3,
+ // linear velocity in mm/s
+ .linearVelocity = Eigen::Vector2f(amplitude_mm * omega_1_s * cos(sinArg), 0),
+ .detectionTime = timestamp};
+
+ humans.emplace_back(h1);
+ humans.emplace_back(h2);
+
+ // periodically toggle third human for showing group of 2 vs group of 3
+ // if ((timestamp.toSecondsSinceEpoch() / 5) % 2 == 0)
+ // {
+ humans.emplace_back(h3);
+ // }
+ }
+
void
Component::runPeriodically()
{
@@ -242,6 +291,9 @@ namespace armarx::navigation::components::dynamic_scene_provider
// Laser scanner features
//
+ ARMARX_INFO << "Not querying laser scanner features for testing purposes (no "
+ "robot available)";
+
/*
TIMING_START(READ_LASERSCANNER_FROM_MEMORY);
@@ -251,13 +303,13 @@ namespace armarx::navigation::components::dynamic_scene_provider
.name = properties.laserScannerFeatures.name,
.timestamp = timestamp};
- const armem::vision::laser_scanner_features::client::Reader::Result laserFeaturesResult =
- laserScannerFeaturesReaderPlugin->get().queryData(laserFeaturesQuery);
+ const armem::vision::laser_scanner_features::client::Reader::Result laserFeaturesResult =
+ laserScannerFeaturesReaderPlugin->get().queryData(laserFeaturesQuery);
- ARMARX_CHECK_EQUAL(laserFeaturesResult.status,
- armem::vision::laser_scanner_features::client::Reader::Result::Success);
+ ARMARX_CHECK_EQUAL(laserFeaturesResult.status,
+ armem::vision::laser_scanner_features::client::Reader::Result::Success);
- ARMARX_INFO << laserFeaturesResult.features.size() << " clusters/features";
+ ARMARX_INFO << laserFeaturesResult.features.size() << " clusters/features";
TIMING_END_COMMENT_STREAM(
READ_LASERSCANNER_FROM_MEMORY, "Timer: reading laserscanner from memory", ARMARX_VERBOSE);
@@ -394,6 +446,9 @@ namespace armarx::navigation::components::dynamic_scene_provider
ARMARX_VERBOSE);
+ ARMARX_INFO << "Not running tracker with lasersensors because they are not available for "
+ "the current tests.";
+
/*
ARMARX_INFO << "Running human tracker with lasersensor measurements";
@@ -425,18 +480,40 @@ namespace armarx::navigation::components::dynamic_scene_provider
//TODO use clusters for obstacle creation
- std::vector<human::Human> humans = humanTracker.getTrackedHumans();
+ humanTracker.update(human::HumanTracker::CameraMeasurement{
+ .detectionTime = timestamp, .humanPoses = humanPoseResult.humanPoses});
+ using Human = armarx::navigation::human::Human;
+ std::vector<Human> humans = humanTracker.getTrackedHumans();
+
+ // Add some testing humans
+ addTestHumans(humans);
+ humanWriterPlugin->get().store(humans, getName(), timestamp);
- if(not humans.empty())
+ if (not humans.empty())
{
ARMARX_INFO << "Detected " << humans.size() << " humans";
+ for (Human& human : humans)
+ {
+ human.proxemicZones = proxemics.createProxemicZones(human);
+ }
humanWriterPlugin->get().store(humans, getName(), timestamp);
}
+ ARMARX_INFO << "Providing grouper with new humans";
+
+ humanGrouper.updateHumans(humans);
+
+ ARMARX_INFO << "Running human grouper";
+ using HumanGroup = armarx::navigation::human::HumanGroup;
+ std::vector<HumanGroup> groups = humanGrouper.getCurrentGroups();
+
+ ARMARX_INFO << "Detected " << groups.size() << " human groups";
+
+ humanGroupWriterPlugin->get().store(groups, getName(), timestamp);
TIMING_END_COMMENT_STREAM(
- WRITE_BACK_HUMANS, "Timer: write humans to memory", ARMARX_VERBOSE);
+ WRITE_BACK_HUMANS, "Timer: write humans and human groups to memory", ARMARX_VERBOSE);
TIMING_END_COMMENT_STREAM(
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/Component.h b/source/armarx/navigation/components/dynamic_scene_provider/Component.h
index aec652dd6438b528ed939e80379715d34e795f60..42a109c6555a3ba6e7384c9ac6165996bf4dc6ed 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/Component.h
+++ b/source/armarx/navigation/components/dynamic_scene_provider/Component.h
@@ -51,6 +51,9 @@
#include "armarx/navigation/memory/client/costmap/Reader.h"
#include "armarx/navigation/memory/client/human/Writer.h"
#include <armarx/navigation/components/dynamic_scene_provider/ComponentInterface.h>
+#include <armarx/navigation/human/HumanGrouper.h>
+#include <armarx/navigation/human/ProxemicZoneCreator.h>
+#include <armarx/navigation/memory/client/human_group/Writer.h>
namespace armarx::navigation::components::dynamic_scene_provider
@@ -119,6 +122,8 @@ namespace armarx::navigation::components::dynamic_scene_provider
VirtualRobot::RobotPtr robot = nullptr;
+ using Human = armarx::navigation::human::Human;
+ void addTestHumans(std::vector<Human>& humans);
private:
static const std::string defaultName;
@@ -204,8 +209,13 @@ namespace armarx::navigation::components::dynamic_scene_provider
ReaderWriterPlugin<memory::client::human::Writer>* humanWriterPlugin = nullptr;
+ ReaderWriterPlugin<memory::client::human_group::Writer>* humanGroupWriterPlugin = nullptr;
+
human::HumanTracker humanTracker;
+ human::ProxemicZoneCreator proxemics;
+ using HumanGrouper = human::HumanGrouper;
+ HumanGrouper humanGrouper;
};
} // namespace armarx::navigation::components::dynamic_scene_provider
diff --git a/source/armarx/navigation/components/navigation_memory/Component.cpp b/source/armarx/navigation/components/navigation_memory/Component.cpp
index dcf4b7ad14653de4fe1dbdc83e56fd69fa591452..e2cc5737e3991c3b9fbd4898ad144e22e93abc02 100644
--- a/source/armarx/navigation/components/navigation_memory/Component.cpp
+++ b/source/armarx/navigation/components/navigation_memory/Component.cpp
@@ -51,8 +51,8 @@
#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/human/aron/Human.aron.generated.h>
#include <armarx/navigation/location/constants.h>
#include <armarx/navigation/memory/constants.h>
@@ -133,8 +133,8 @@ namespace armarx::navigation::components::navigation_memory
workingMemory().addCoreSegment(memory::constants::HumanCoreSegmentName,
navigation::human::arondto::Human::ToAronType());
- // workingMemory().addCoreSegment(memory::constants::HumanGroupCoreSegmentName,
- // navigation::human::arondto::Human::ToAronType());
+ workingMemory().addCoreSegment(memory::constants::HumanGroupCoreSegmentName,
+ navigation::human::arondto::HumanGroup::ToAronType());
if (not properties.snapshotToLoad.empty())
{
@@ -421,11 +421,13 @@ namespace armarx::navigation::components::navigation_memory
void
Component::visuRun()
{
- memory::Visu visu{arviz,
- workingMemory().getCoreSegment(navigation::location::coreSegmentID),
- workingMemory().getCoreSegment(navigation::graph::coreSegmentID),
- workingMemory().getCoreSegment(memory::constants::CostmapCoreSegmentName),
- workingMemory().getCoreSegment(memory::constants::HumanCoreSegmentName)};
+ memory::Visu visu{
+ arviz,
+ workingMemory().getCoreSegment(navigation::location::coreSegmentID),
+ workingMemory().getCoreSegment(navigation::graph::coreSegmentID),
+ workingMemory().getCoreSegment(memory::constants::CostmapCoreSegmentName),
+ workingMemory().getCoreSegment(memory::constants::HumanCoreSegmentName),
+ workingMemory().getCoreSegment(memory::constants::HumanGroupCoreSegmentName)};
Properties::LocationGraph p;
@@ -458,6 +460,9 @@ namespace armarx::navigation::components::navigation_memory
// Humans
visu.drawHumans(layers, p.visuHumans, p.visuTransparent);
+ // Groups
+ visu.drawHumanGroups(layers, p.visuHumanGroups);
+
arviz.commit(layers);
metronome.waitForNextTick();
@@ -580,7 +585,7 @@ namespace armarx::navigation::components::navigation_memory
bool
Component::storeLocationGraph(const armarx::data::PackagePath& packagePath,
- const Ice::Current& current)
+ const Ice::Current& current)
{
armem::server::wm::CoreSegment& locationCoreSegment =
workingMemory().getCoreSegment(navigation::location::coreSegmentID);
@@ -651,4 +656,4 @@ namespace armarx::navigation::components::navigation_memory
}
-} // namespace armarx::navigation::components::navigation_memory
+} // namespace armarx::navigation::components::navigation_memory
diff --git a/source/armarx/navigation/components/navigation_memory/Component.h b/source/armarx/navigation/components/navigation_memory/Component.h
index b1b1a1b682e645c33ab1daef65c0cb10587a0796..619be519aef6dd3be5ed205ce3042d10aa975a8c 100644
--- a/source/armarx/navigation/components/navigation_memory/Component.h
+++ b/source/armarx/navigation/components/navigation_memory/Component.h
@@ -105,6 +105,7 @@ namespace armarx::navigation::components::navigation_memory
bool visuGraphEdges = true;
bool visuCostmaps = true;
bool visuHumans = true;
+ bool visuHumanGroups = true;
bool visuTransparent = false;
diff --git a/source/armarx/navigation/components/navigation_memory/Visu.cpp b/source/armarx/navigation/components/navigation_memory/Visu.cpp
index 882deb59ae697593a36325bd0b3b8fff5625f9eb..7a8390d151d13ab837f22fd8a3debd8c5c1856e4 100644
--- a/source/armarx/navigation/components/navigation_memory/Visu.cpp
+++ b/source/armarx/navigation/components/navigation_memory/Visu.cpp
@@ -15,6 +15,7 @@
*
* @package Navigation::ArmarXObjects::NavigationMemory
* @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @author Timo Weberruß ( timo dot weberruss at student dot kit dot edu )
* @date 2021
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
* GNU General Public License
@@ -53,12 +54,14 @@ 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) :
+ const armem::server::wm::CoreSegment& humanSegment,
+ const armem::server::wm::CoreSegment& humanGroupSegment) :
arviz(arviz),
locSegment(locSegment),
graphSegment(graphSegment),
costmapSegment(costmapSegment),
humanSegment(humanSegment),
+ humanGroupSegment(humanGroupSegment),
visu(std::make_unique<graph::GraphVisu>())
{
}
@@ -137,94 +140,124 @@ namespace armarx::navigation::memory
}
}
- namespace
+
+ void
+ Visu::visualize(const algorithms::Costmap& costmap, viz::Layer& layer, const std::string& name)
{
+ const auto cmap = simox::color::cmaps::viridis();
+ const float vmax = costmap.getGrid().array().maxCoeff();
- void
- visualize(const algorithms::Costmap& costmap, viz::Layer& layer, const std::string& name)
+ const auto asColor = [&cmap, &vmax](const float distance) -> viz::data::Color
{
- const auto cmap = simox::color::cmaps::viridis();
- const float vmax = costmap.getGrid().array().maxCoeff();
+ const auto color = cmap.at(distance, 0.F, vmax);
+ return {color.a, color.r, color.g, color.b};
+ };
- const auto asColor = [&cmap, &vmax](const float distance) -> viz::data::Color
- {
- const auto color = cmap.at(distance, 0.F, vmax);
- return {color.a, color.r, color.g, color.b};
- };
+ const std::int64_t cols = costmap.getGrid().cols();
+ const std::int64_t rows = costmap.getGrid().rows();
- const std::int64_t cols = costmap.getGrid().cols();
- const std::int64_t rows = costmap.getGrid().rows();
+ auto mesh = viz::Mesh(name);
- auto mesh = viz::Mesh(name);
+ std::vector<std::vector<Eigen::Vector3f>> vertices;
+ std::vector<std::vector<viz::data::Color>> colors;
- std::vector<std::vector<Eigen::Vector3f>> vertices;
- std::vector<std::vector<viz::data::Color>> colors;
+ for (int r = 0; r < rows; r++)
+ {
+ auto& verticesRow = vertices.emplace_back(cols);
+ auto& colorsRow = colors.emplace_back(cols);
- for (int r = 0; r < rows; r++)
+ for (int c = 0; c < cols; c++)
{
- auto& verticesRow = vertices.emplace_back(cols);
- auto& colorsRow = colors.emplace_back(cols);
-
- for (int c = 0; c < cols; c++)
- {
- verticesRow.at(c) = conv::to3D(costmap.toPositionGlobal({r, c}));
- colorsRow.at(c) = asColor(costmap.getGrid()(r, c));
- }
+ verticesRow.at(c) = conv::to3D(costmap.toPositionGlobal({r, c}));
+ colorsRow.at(c) = asColor(costmap.getGrid()(r, c));
}
+ }
- mesh.grid2D(vertices, colors);
+ mesh.grid2D(vertices, colors);
- layer.add(mesh);
- }
+ layer.add(mesh);
+ }
- void
- visualize(const human::Humans& humans, viz::Layer& layer, const bool visuTransparent)
- {
+ void
+ Visu::visualize(const human::Humans& humans, viz::Layer& layer, const bool visuTransparent)
+ {
- const Eigen::Translation3f human_T_mmm(Eigen::Vector3f{0, 0, 1000});
+ const Eigen::Translation3f human_T_mmm(Eigen::Vector3f{0, 0, 1000});
+
+ ARMARX_INFO << "Visualizing " << humans.size() << " humans";
+ for (const auto& human : humans)
+ {
+ viz::Robot mmm(std::to_string(layer.size()));
+ mmm.file("RobotAPI", "RobotAPI/robots/MMM/mmm.xml");
+ mmm.pose(conv::to3D(human.pose) * human_T_mmm);
+ mmm.scale(1.7); // 1.7m
+ mmm.overrideColor(viz::Color::orange(255, visuTransparent ? 100 : 255));
+ layer.add(mmm);
+
+ core::Pose pose3d = conv::to3D(human.pose);
+ pose3d.translation() += Eigen::Vector3f{0, 0, 1000};
+ auto arrow = viz::Arrow(std::to_string(layer.size()))
+ .pose(pose3d)
+ .length(200)
+ .color(simox::Color::red());
+ layer.add(arrow);
- 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});
+ auto arrow = viz::Arrow(std::to_string(layer.size()))
+ .fromTo(pose3d.translation(),
+ pose3d.translation() + conv::to3D(human.linearVelocity))
+ .color(simox::Color::blue());
+ arrow.width(10);
+ layer.add(arrow);
+ }
+ }
+ }
+ void
+ Visu::visualizeProxemicZones(const human::Humans& humans, viz::Layer& layer)
+ {
+ for (const auto& human : humans)
+ {
+ auto proxemicZones = human.proxemicZones;
- // cylinder.color(simox::Color::orange());
- // cylinder.radius(300);
- // layer.add(cylinder);
+ int i = 0;
+ for (const auto& proxemicZone : proxemicZones)
+ {
+ const Eigen::Vector3f axisLength(
+ proxemicZone.shape.a, proxemicZone.shape.b, 10.f - i);
+ const core::Pose pose3d = conv::to3D(proxemicZone.pose);
+ viz::Ellipsoid zoneEllipse("proxemicZone_" + std::to_string(visualizationIndex));
+ zoneEllipse.pose(pose3d);
+ zoneEllipse.axisLengths(axisLength);
+ zoneEllipse.color(PROXEMIC_ZONE_COLOR[i % PROXEMIC_ZONE_COLOR.size()]);
- viz::Robot mmm(std::to_string(layer.size()));
- mmm.file("RobotAPI", "RobotAPI/robots/MMM/mmm.xml");
- mmm.pose(conv::to3D(human.pose) * human_T_mmm);
- mmm.scale(1.7); // 1.7m
- mmm.overrideColor(viz::Color::orange(255, visuTransparent ? 100 : 255));
- layer.add(mmm);
+ layer.add(zoneEllipse);
+ i++;
+ }
+ }
+ }
- core::Pose pose3d = conv::to3D(human.pose);
- pose3d.translation() += Eigen::Vector3f{0, 0, 1000};
- auto arrow = viz::Arrow(std::to_string(layer.size()))
- .pose(pose3d)
- .length(200)
- .color(simox::Color::red());
- layer.add(arrow);
+ void
+ Visu::visualize(const human::HumanGroups& humanGroups, viz::Layer& layer)
+ {
+ for (const auto& humanGroup : humanGroups)
+ {
+ viz::Polygon polygon(std::to_string(layer.size()));
- {
- auto arrow =
- viz::Arrow(std::to_string(layer.size()))
- .fromTo(pose3d.translation(),
- pose3d.translation() + conv::to3D(human.linearVelocity))
- .color(simox::Color::blue());
- arrow.width(10);
- layer.add(arrow);
- }
+ const std::vector<Eigen::Vector2f> verts2D = humanGroup.shape.vertices;
+ std::vector<Eigen::Vector3f> verts3D;
+ for (const auto& v2d : verts2D)
+ {
+ verts3D.emplace_back(Eigen::Vector3f(v2d.x(), v2d.y(), 50));
}
+ polygon.points(verts3D);
+ polygon.color(simox::Color::kit_yellow());
+ polygon.lineWidth(10);
+ layer.add(polygon);
}
-
- } // namespace
+ }
void
Visu::drawCostmaps(std::vector<viz::Layer>& layers, bool enabled)
@@ -292,11 +325,13 @@ namespace armarx::navigation::memory
[&namedProviderHumans](
const armarx::armem::wm::EntityInstance& instance)
{
+ ARMARX_TRACE;
const auto dto =
navigation::human::arondto::Human::FromAron(instance.data());
-
+ ARMARX_TRACE;
navigation::human::Human human;
fromAron(dto, human);
+ ARMARX_TRACE;
namedProviderHumans[instance.id().providerSegmentName].emplace_back(
std::move(human));
@@ -307,7 +342,52 @@ namespace armarx::navigation::memory
for (const auto& [providerName, humans] : namedProviderHumans)
{
viz::Layer& layer = layers.emplace_back(arviz.layer("humans_" + providerName));
+ viz::Layer& proxZoneLayer =
+ layers.emplace_back(arviz.layer("proxemic_zones_" + providerName));
visualize(humans, layer, visuTransparent);
+ visualizeProxemicZones(humans, proxZoneLayer);
+ }
+ }
+
+ void
+ Visu::drawHumanGroups(std::vector<viz::Layer>& layers, bool enabled)
+ {
+ if (not enabled)
+ {
+ return;
+ }
+
+ std::map<std::string, navigation::human::HumanGroups> namedProviderHumanGroups;
+
+ humanGroupSegment.doLocked(
+ [&]()
+ {
+ using namespace armem::server;
+
+ humanGroupSegment.forEachEntity(
+ [&](const wm::Entity& entity)
+ {
+ namedProviderHumanGroups[entity.id().providerSegmentName];
+ entity.getLatestSnapshot().forEachInstance(
+ [&namedProviderHumanGroups](
+ const armarx::armem::wm::EntityInstance& instance)
+ {
+ const auto dto = navigation::human::arondto::HumanGroup::FromAron(
+ instance.data());
+
+ navigation::human::HumanGroup humanGroup;
+ fromAron(dto, humanGroup);
+
+ namedProviderHumanGroups[instance.id().providerSegmentName]
+ .emplace_back(std::move(humanGroup));
+ });
+ });
+ });
+
+ for (const auto& [providerName, humanGroups] : namedProviderHumanGroups)
+ {
+ viz::Layer& layer = layers.emplace_back(arviz.layer("humanGroups_" + providerName));
+ visualize(humanGroups, layer);
}
}
diff --git a/source/armarx/navigation/components/navigation_memory/Visu.h b/source/armarx/navigation/components/navigation_memory/Visu.h
index d965feccd52b78b895ded9ebaf1683ff64c95319..835823d9868337c65d98ab1ec146f73a9817eac0 100644
--- a/source/armarx/navigation/components/navigation_memory/Visu.h
+++ b/source/armarx/navigation/components/navigation_memory/Visu.h
@@ -31,6 +31,7 @@
#include <RobotAPI/libraries/armem/core/forward_declarations.h>
#include "armarx/navigation/algorithms/Costmap.h"
+#include "armarx/navigation/human/types.h"
namespace armarx::navigation::graph
@@ -48,7 +49,8 @@ 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);
+ const armem::server::wm::CoreSegment& humanSegment,
+ const armem::server::wm::CoreSegment& humanGroupSegment);
~Visu();
@@ -56,6 +58,7 @@ namespace armarx::navigation::memory
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, bool visuTransparent);
+ void drawHumanGroups(std::vector<viz::Layer>& layers, bool enabled);
viz::ScopedClient arviz;
@@ -64,8 +67,21 @@ namespace armarx::navigation::memory
const armem::server::wm::CoreSegment& graphSegment;
const armem::server::wm::CoreSegment& costmapSegment;
const armem::server::wm::CoreSegment& humanSegment;
+ const armem::server::wm::CoreSegment& humanGroupSegment;
std::unique_ptr<navigation::graph::GraphVisu> visu;
+
+ private:
+ const std::array<simox::Color, 2> PROXEMIC_ZONE_COLOR = {simox::Color::red(),
+ simox::Color::blue()};
+ int visualizationIndex;
+ void visualize(const navigation::human::Humans& humans,
+ viz::Layer& layer,
+ const bool visuTransparent);
+ void visualizeProxemicZones(const navigation::human::Humans& humans, viz::Layer& layer);
+ void visualize(const human::HumanGroups& humanGroups, viz::Layer& layer);
+ void
+ visualize(const algorithms::Costmap& costmap, viz::Layer& layer, const std::string& name);
};
diff --git a/source/armarx/navigation/human/CMakeLists.txt b/source/armarx/navigation/human/CMakeLists.txt
index 836826fb77312440fdc726cc105d91e2201f0fba..360b78b6c31bc8942cc4ce5ba597e798edfc8bf6 100644
--- a/source/armarx/navigation/human/CMakeLists.txt
+++ b/source/armarx/navigation/human/CMakeLists.txt
@@ -18,6 +18,12 @@ armarx_add_library(teb_human
types.cpp
aron_conversions.cpp
ProxemicZoneCreator.cpp
+ HumanGrouper.cpp
+ EuclideanDistance.cpp
+ OrientationDistance.cpp
+ CombinedDistance.cpp
+ ConvexHullGenerator.cpp
+ MovementDistance.cpp
HumanSystemModel.cpp
UnscentedKalmanFilter_impl.cpp
HumanTracker.cpp
@@ -27,6 +33,14 @@ armarx_add_library(teb_human
aron_conversions.h
shapes.h
ProxemicZoneCreator.h
+ HumanGrouper.h
+ DistanceFunction.h
+ EuclideanDistance.h
+ OrientationDistance.h
+ CombinedDistance.h
+ GroupShapeGenerator.h
+ ConvexHullGenerator.h
+ MovementDistance.h
HumanSystemModel.h
HumanTracker.h
HumanFilter.h
@@ -100,8 +114,24 @@ armarx_add_test(human_tracker_test
TEST_FILES
test/human_tracker_test.cpp
DEPENDENCIES
- ArmarXCore
- armarx_navigation::core
- armarx_navigation::teb_human
- range-v3::range-v3
+ PUBLIC
+ ArmarXCore
+ armarx_navigation::core
+ armarx_navigation::teb_human
+
+ PRIVATE
+ range-v3::range-v3
+)
+
+armarx_add_test(human_test
+ TEST_FILES
+ test/human_test.cpp
+ DEPENDENCIES
+ PUBLIC
+ ArmarXCore
+ armarx_navigation::core
+ armarx_navigation::teb_human
+
+ PRIVATE
+ range-v3::range-v3
)
diff --git a/source/armarx/navigation/human/CombinedDistance.cpp b/source/armarx/navigation/human/CombinedDistance.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..18330577f961ed5130511092d8704e6f3581a0c6
--- /dev/null
+++ b/source/armarx/navigation/human/CombinedDistance.cpp
@@ -0,0 +1,28 @@
+#include "CombinedDistance.h"
+
+namespace armarx::navigation::human
+{
+ CombinedDistance::CombinedDistance(const double maxOrientationInfluence,
+ const double movementInfluence) :
+ euclidean(EuclideanDistance()),
+ orientation(OrientationDistance()),
+ movement(MovementDistance()),
+ maxOrientationInfluence(maxOrientationInfluence),
+ movementInfluence(movementInfluence)
+ {
+ }
+
+ double
+ CombinedDistance::computeDistance(const Human& h1, const Human& h2) const
+ {
+ double d = euclidean.computeDistance(h1, h2);
+
+ // scales the euclidean distance by a factor in [1, maxOrientationInfluence]
+ d *= (1 + (maxOrientationInfluence - 1) * orientation.computeDistance(h1, h2));
+
+ // scales the euclidean distance by a factor in [1, inf) depending on influence
+ d *= (1 + movementInfluence * movement.computeDistance(h1, h2));
+
+ return d;
+ }
+} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/human/CombinedDistance.h b/source/armarx/navigation/human/CombinedDistance.h
new file mode 100644
index 0000000000000000000000000000000000000000..4cacc38d0971ce6499b0bcae046c12de84ad787e
--- /dev/null
+++ b/source/armarx/navigation/human/CombinedDistance.h
@@ -0,0 +1,67 @@
+/**
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Timo Weberruß ( timo dot weberruss at student dot kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include "DistanceFunction.h"
+#include "EuclideanDistance.h"
+#include "MovementDistance.h"
+#include "OrientationDistance.h"
+
+
+namespace armarx::navigation::human
+{
+ /**
+ * @brief The CombinedDistance class combines the standard euclidean distance multiplicatively
+ * with an orientation distance in such a way that the distance is always at least the euclidean
+ * distance but will be scaled by up to the maximum influence factor if the people face away
+ * from each other. The distance will then again be increased by the scaled difference in movement
+ * between the two people. The distance will always be at least the euclidean distance.
+ */
+ class CombinedDistance : public DistanceFunction
+ {
+ public:
+ /**
+ * @brief CombinedDistance Creates a new combined distance with the specified orientation scale factor
+ * @param maxOrientationInfluence the euclidean distance between the humans will be scaled
+ * by up to this factor if they face away from each other
+ * @param movementInfluence scales the difference in velocity of the humans before applying
+ * it to the total distance
+ */
+ CombinedDistance(double maxOrientationInfluence, double movementInfluence);
+
+ /**
+ * @brief computeDistance computes the distance value for the two given people. Should
+ * always be commutative, i.e. computeDistance(h1, h2) = computeDistance(h2, h1)
+ * @param h1 the first human
+ * @param h2 the second human
+ * @return a value specifying a distance between the two people
+ */
+ double computeDistance(const Human& h1, const Human& h2) const override;
+
+ private:
+ const EuclideanDistance euclidean;
+ const OrientationDistance orientation;
+ const MovementDistance movement;
+ const double maxOrientationInfluence;
+ const double movementInfluence;
+ };
+} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/human/ConvexHullGenerator.cpp b/source/armarx/navigation/human/ConvexHullGenerator.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2aff0df1f43d4aba9a1a1b76a32049c50ad371e7
--- /dev/null
+++ b/source/armarx/navigation/human/ConvexHullGenerator.cpp
@@ -0,0 +1,49 @@
+#include "ConvexHullGenerator.h"
+
+#include <boost/geometry.hpp>
+#include <boost/geometry/geometries/register/point.hpp>
+#include <boost/geometry/multi/geometries/register/multi_point.hpp>
+
+using Point = std::pair<double, double>;
+BOOST_GEOMETRY_REGISTER_POINT_2D(Point, double, boost::geometry::cs::cartesian, first, second)
+
+namespace armarx::navigation::human
+{
+
+ shapes::Polygon
+ ConvexHullGenerator::createShape(const HumanGroup& group) const
+ {
+ if (group.humans.empty())
+ {
+ throw InvalidArgumentException("Human group size has to be at least 1");
+ }
+
+ using BoostPoly = boost::geometry::model::polygon<Point>;
+
+ std::vector<Point> coordinates;
+ for (const Human& human : group.humans)
+ {
+ coordinates.emplace_back(human.pose.translation().x(), human.pose.translation().y());
+ }
+
+ BoostPoly poly;
+ BoostPoly hull;
+
+ poly.outer().assign(coordinates.begin(), coordinates.end());
+ boost::geometry::convex_hull(poly, hull);
+
+ std::vector<Eigen::Vector2f> vertices;
+
+ for (auto it = boost::begin(boost::geometry::exterior_ring(hull));
+ it != boost::end(boost::geometry::exterior_ring(hull));
+ ++it)
+ {
+ const double x = boost::geometry::get<0>(*it);
+ const double y = boost::geometry::get<1>(*it);
+
+ vertices.emplace_back(x, y);
+ }
+
+ return {.vertices = vertices};
+ }
+} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/human/ConvexHullGenerator.h b/source/armarx/navigation/human/ConvexHullGenerator.h
new file mode 100644
index 0000000000000000000000000000000000000000..d99d845dbafc308bc7acc86b5b23282374d8dfc6
--- /dev/null
+++ b/source/armarx/navigation/human/ConvexHullGenerator.h
@@ -0,0 +1,44 @@
+/**
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Timo Weberruß ( timo dot weberruss at student dot kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include "GroupShapeGenerator.h"
+#include <armarx/navigation/human/types.h>
+
+
+#pragma once
+
+namespace armarx::navigation::human
+{
+ /**
+ * @brief A GroupShapeGenerator that defines the group shape of a set of humans as the
+ * convex hull of their positions in 2D space.
+ */
+ class ConvexHullGenerator : public GroupShapeGenerator
+ {
+ public:
+ ConvexHullGenerator() = default;
+ virtual ~ConvexHullGenerator() = default;
+
+ shapes::Polygon createShape(const HumanGroup& group) const override;
+ };
+} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/human/DistanceFunction.h b/source/armarx/navigation/human/DistanceFunction.h
new file mode 100644
index 0000000000000000000000000000000000000000..070d897472f6efa4d209960fdde84ae9d09eb36f
--- /dev/null
+++ b/source/armarx/navigation/human/DistanceFunction.h
@@ -0,0 +1,49 @@
+/**
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Timo Weberruß ( timo dot weberruss at student dot kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <armarx/navigation/human/types.h>
+
+
+namespace armarx::navigation::human
+{
+ /**
+ * @brief A distance function is able to compute a distance between two humans.
+ * The exact semantics of this distance depends on the implementation. For example,
+ * a HumanGrouper uses distances two group humans together (the smaller the distance between
+ * two humans, the more likely it is for them to belong to the same social group).
+ */
+ class DistanceFunction
+ {
+ public:
+ DistanceFunction() = default;
+ virtual ~DistanceFunction() = default;
+
+ /**
+ * @brief computeDistance computes the (semantic or literal, depending on the implementation) distance between the given humans
+ * @param h1 the first human
+ * @param h2 the second human
+ * @return the distance between the humans (greater values correspond to humans that are further away)
+ */
+ virtual double computeDistance(const Human& h1, const Human& h2) const = 0;
+ };
+} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/human/EuclideanDistance.cpp b/source/armarx/navigation/human/EuclideanDistance.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..911d59a914634c00594bce7cd160af86b39b617e
--- /dev/null
+++ b/source/armarx/navigation/human/EuclideanDistance.cpp
@@ -0,0 +1,10 @@
+#include "EuclideanDistance.h"
+
+namespace armarx::navigation::human
+{
+ double
+ EuclideanDistance::computeDistance(const Human& h1, const Human& h2) const
+ {
+ return (h1.pose.translation() - h2.pose.translation()).norm();
+ }
+} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/human/EuclideanDistance.h b/source/armarx/navigation/human/EuclideanDistance.h
new file mode 100644
index 0000000000000000000000000000000000000000..c1d39ff10bfffdc3636d1a882b3ba3c59a50adbf
--- /dev/null
+++ b/source/armarx/navigation/human/EuclideanDistance.h
@@ -0,0 +1,39 @@
+/**
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Timo Weberruß ( timo dot weberruss at student dot kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include "DistanceFunction.h"
+
+
+namespace armarx::navigation::human
+{
+ /**
+ * @brief The EuclideanDistance class is an implementation of a distance function that computes
+ * the literal, euclidean distance between two human. (i.e. sqrt(dx²+dy²)).
+ */
+ class EuclideanDistance : public DistanceFunction
+ {
+ public:
+ EuclideanDistance() = default;
+ double computeDistance(const Human& h1, const Human& h2) const override;
+ };
+} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/human/GroupShapeGenerator.h b/source/armarx/navigation/human/GroupShapeGenerator.h
new file mode 100644
index 0000000000000000000000000000000000000000..34f777d9fb2bbdc17a42559b37125d32a7f0889f
--- /dev/null
+++ b/source/armarx/navigation/human/GroupShapeGenerator.h
@@ -0,0 +1,47 @@
+/**
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Timo Weberruß ( timo dot weberruss at student dot kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <armarx/navigation/human/types.h>
+
+
+namespace armarx::navigation::human
+{
+ /**
+ * @brief A strategy encapsuling a way to create a shape describing the special area occupied
+ * by a social human group
+ */
+ class GroupShapeGenerator
+ {
+ public:
+ GroupShapeGenerator() = default;
+ virtual ~GroupShapeGenerator() = default;
+
+ /**
+ * @brief createShape creates and returns the shape describing the given group
+ * @param group a social grouping of at least one human
+ * @return a polygon describing the proxemic zone occupied by the given group (not including
+ * the zones of the individual members of the group)
+ */
+ virtual shapes::Polygon createShape(const HumanGroup& group) const = 0;
+ };
+} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/human/HumanGrouper.cpp b/source/armarx/navigation/human/HumanGrouper.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..281a77d448668fc9bdb4422dbf2b1f09a46d256e
--- /dev/null
+++ b/source/armarx/navigation/human/HumanGrouper.cpp
@@ -0,0 +1,70 @@
+#include "HumanGrouper.h"
+
+#include <unordered_set>
+
+#include "CombinedDistance.h"
+#include "ConvexHullGenerator.h"
+
+namespace armarx::navigation::human
+{
+ HumanGrouper::HumanGrouper(const GroupingSettings& settings) :
+ distanceFunction(std::make_unique<CombinedDistance>(settings.maxOrientationInfluence,
+ settings.movementInfluence)),
+ shapeGenerator(std::make_unique<ConvexHullGenerator>()),
+ settings(settings)
+ {
+ }
+
+ void
+ HumanGrouper::updateHumans(const std::vector<human::Human>& newHumans)
+ {
+ currentHumans_ = newHumans;
+ }
+
+ std::vector<human::HumanGroup>
+ HumanGrouper::getCurrentGroups()
+ {
+ const DateTime now = DateTime::Now();
+
+ std::vector<human::HumanGroup> groups = {};
+
+ // maps index of human in currentHumans to index of group in groups
+ std::unordered_map<uint64_t, uint64_t> humanGroupMap;
+
+ for (uint64_t parentIdx = 0; parentIdx < currentHumans_.size(); parentIdx++)
+ {
+ const Human parent = currentHumans_.at(parentIdx);
+
+ // if no group exists yet for this human, create one just for them
+ if (humanGroupMap.count(parentIdx) == 0)
+ {
+ const human::HumanGroup group = {{{}}, {parent}, now};
+ groups.push_back(group);
+ humanGroupMap[parentIdx] = groups.size() - 1;
+ }
+
+ for (uint64_t childIdx = 0; childIdx < currentHumans_.size(); childIdx++)
+ {
+ const Human child = currentHumans_.at(childIdx);
+
+ if ((humanGroupMap.count(childIdx) == 0 ||
+ humanGroupMap[childIdx] != humanGroupMap[parentIdx]) &&
+ this->distanceFunction->computeDistance(parent, child) <
+ settings.groupingThreshold)
+ {
+ humanGroupMap[childIdx] = humanGroupMap[parentIdx];
+ groups.at(humanGroupMap[childIdx])
+ .humans.push_back(currentHumans_.at(childIdx));
+ }
+ }
+ }
+
+ for (human::HumanGroup& group : groups)
+ {
+ group.shape = shapeGenerator->createShape(group);
+ }
+
+ return groups;
+ }
+
+} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/human/HumanGrouper.h b/source/armarx/navigation/human/HumanGrouper.h
new file mode 100644
index 0000000000000000000000000000000000000000..fc3fe60dc8aab190d9f3cbe6d736db7915570560
--- /dev/null
+++ b/source/armarx/navigation/human/HumanGrouper.h
@@ -0,0 +1,91 @@
+/**
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Timo Weberruß ( timo dot weberruss at student dot kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include "DistanceFunction.h"
+#include "GroupShapeGenerator.h"
+#include "armarx/navigation/core/basic_types.h"
+#include "armarx/navigation/human/types.h"
+
+namespace armarx::navigation::human
+{
+ /**
+ * @brief Identifies social interaction groups in a collection of detected humans.
+ *
+ * Can identify social interaction groups in a collection of humans. The currently detected
+ * humans can be set by calling updateHumans. The grouper will also return groups of size 1,
+ * and every detected human will be in exactly one group.
+ */
+ class HumanGrouper
+ {
+ using Human = armarx::navigation::human::Human;
+
+ public:
+ /**
+ * @brief The GroupingSettings struct contains settings for group generation.
+ * This includes the influence of aligned orientation and how aggressively groups
+ * are generated.
+ */
+ struct GroupingSettings
+ {
+ // people strictly closer than this distance will be grouped
+ const double groupingThreshold = 2000;
+
+ // distances between humans will be scaled by up to this factor based on their orientation
+ const double maxOrientationInfluence = 1.5;
+
+ // distances between humans will be scaled by their movement difference depending on this factor
+ const double movementInfluence = 1.5;
+ };
+
+ /**
+ * @brief Creates a new HumanGrouper
+ * @param settings different parameters that will affect group generation
+ */
+ HumanGrouper(const GroupingSettings& settings);
+
+ /**
+ * @brief Sets the currently detected humans.
+ *
+ * Sets the currently detected humans. Only these humans will be assumed to currently exist
+ * when getCurrentGroups() is called the next time.
+ * @param newHumans a vector containing the most recently detected humans
+ */
+ void updateHumans(const std::vector<Human>& newHumans);
+
+ /**
+ * @brief Recognizes groups in the current humans.
+ *
+ * Identifies and returns social groups in the humans given to this instance by the last
+ * call to updateHumans. Will also return groups of size one for single people.
+ * @return a vector containing the recognized human groups
+ */
+ std::vector<human::HumanGroup> getCurrentGroups();
+
+ private:
+ const std::unique_ptr<DistanceFunction> distanceFunction;
+ const std::unique_ptr<GroupShapeGenerator> shapeGenerator;
+ const GroupingSettings settings;
+
+ std::vector<Human> currentHumans_;
+ };
+} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/human/MovementDistance.cpp b/source/armarx/navigation/human/MovementDistance.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a71bda4df7a3766904251abf9d4a1b5ff6667289
--- /dev/null
+++ b/source/armarx/navigation/human/MovementDistance.cpp
@@ -0,0 +1,12 @@
+#include "MovementDistance.h"
+
+namespace armarx::navigation::human
+{
+ MovementDistance::MovementDistance() = default;
+
+ double
+ MovementDistance::computeDistance(const Human& h1, const Human& h2) const
+ {
+ return (h1.linearVelocity - h2.linearVelocity).norm();
+ }
+} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/human/MovementDistance.h b/source/armarx/navigation/human/MovementDistance.h
new file mode 100644
index 0000000000000000000000000000000000000000..57fb065fb949a8f5fc571514e51ccb00144ed6ed
--- /dev/null
+++ b/source/armarx/navigation/human/MovementDistance.h
@@ -0,0 +1,40 @@
+/**
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Timo Weberruß ( timo dot weberruss at student dot kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include "DistanceFunction.h"
+
+
+namespace armarx::navigation::human
+{
+ /**
+ * @brief A DistanceFunction that defines the distance between two humans as the difference
+ * in velocity (that is, the norm of the linear velocity difference vector).
+ */
+ class MovementDistance : public DistanceFunction
+ {
+ public:
+ MovementDistance();
+
+ double computeDistance(const Human& h1, const Human& h2) const override;
+ };
+} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/human/OrientationDistance.cpp b/source/armarx/navigation/human/OrientationDistance.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..07f98cf2df9aeda6b63213d95b17b86f41c0d753
--- /dev/null
+++ b/source/armarx/navigation/human/OrientationDistance.cpp
@@ -0,0 +1,58 @@
+#include "OrientationDistance.h"
+
+#include <boost/math/constants/constants.hpp>
+#include <math.h>
+
+namespace armarx::navigation::human
+{
+ OrientationDistance::OrientationDistance(const double max, const double min) :
+ max(max), min(min)
+ {
+ }
+
+ double
+ OrientationDistance::computeDistance(const Human& h1, const Human& h2) const
+ {
+ // ranges from 0 to 1
+ const double factor = getOrientationFactor(h1, h2);
+
+ return min + factor * (max - min);
+ }
+
+
+ double
+ OrientationDistance::getOrientationFactor(const Human& h1, const Human& h2)
+ {
+ const double lineOrientation = getLineOrientation(h2, h1);
+ const double angleH1 = normOrientation(Eigen::Rotation2Dd(h1.pose.linear()).angle() / M_PI);
+ const double angleH2 = normOrientation(Eigen::Rotation2Dd(h2.pose.linear()).angle() / M_PI);
+
+ // assuming the angles to be in the interval [0, 2pi]
+ const double deviationFirst = std::abs(normOrientation(angleH1 - lineOrientation));
+ const double deviationSecond = std::abs(normOrientation(angleH2 - (lineOrientation + 1)));
+
+ // ranges from 0 (looking directly at each other) to 1 (back to back)
+ return (deviationFirst + deviationSecond) / 2;
+ }
+
+ double
+ OrientationDistance::getLineOrientation(const Human& h1, const Human& h2)
+ {
+ const double dx = (h1.pose.translation() - h2.pose.translation()).x();
+ const double dy = (h1.pose.translation() - h2.pose.translation()).y();
+ const double lineOrientation = atan2(dy, dx) / M_PI;
+
+ return lineOrientation;
+ }
+
+ // brings orientation to (-1, 1]
+ double
+ OrientationDistance::normOrientation(const double orientation)
+ {
+ // brings orientation to [0, 2)
+ double convertedOrientation = std::fmod(((std::fmod(orientation, 2)) + 2), 2);
+ // brings orientation to [-1, 1)
+ convertedOrientation -= static_cast<int>(convertedOrientation > 1) * 2;
+ return convertedOrientation;
+ }
+} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/human/OrientationDistance.h b/source/armarx/navigation/human/OrientationDistance.h
new file mode 100644
index 0000000000000000000000000000000000000000..3b8175e6c94e8b79cff9ec5d42c43342188a601e
--- /dev/null
+++ b/source/armarx/navigation/human/OrientationDistance.h
@@ -0,0 +1,60 @@
+/**
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Timo Weberruß ( timo dot weberruss at student dot kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include "DistanceFunction.h"
+
+
+namespace armarx::navigation::human
+{
+ /**
+ * @brief The OrientationDistance class is a DistanceFunction that computes the distance between
+ * the two humans based upon their orientation towards or away from each other.
+ *
+ * This distance function will return
+ * - the specified minimum distance (0 by default) if the humans are exactly facing each other
+ * - the specified maximum distance (1 by default) if the humans are oriented back to back
+ * - a linear interpolation between the two for anything inbetween (for instance, two people
+ * looking in the same direction will have a distance equal to the arithmetic mean of the
+ * minimum and maximum distance, 0.5 by default)
+ */
+ class OrientationDistance : public DistanceFunction
+ {
+ public:
+ /**
+ * @brief OrientationDistance Creates a new OrientationDistance with the specified range
+ * @param max the distance two people will have when faced back to back
+ * @param min the distance two people will have when oriented face to face
+ */
+ OrientationDistance(double max = 1, double min = 0);
+ double computeDistance(const Human& h1, const Human& h2) const override;
+
+ private:
+ const double max;
+ const double min;
+
+ // helper methods
+ static double getOrientationFactor(const Human& h1, const Human& h2);
+ static double getLineOrientation(const Human& h2, const Human& h1);
+ static double normOrientation(double orientation);
+ };
+} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/human/aron/Human.xml b/source/armarx/navigation/human/aron/Human.xml
index 57d104d858b8b346531c61b73069aba168ce374d..2f2b64b9fa6f0851e9b5bced33f3b87e816849a9 100644
--- a/source/armarx/navigation/human/aron/Human.xml
+++ b/source/armarx/navigation/human/aron/Human.xml
@@ -2,6 +2,40 @@
<AronTypeDefinition>
<GenerateTypes>
+ <Object name='armarx::navigation::human::arondto::ExponentialPenaltyModel'>
+ <ObjectChild key='minDistance'>
+ <Float />
+ </ObjectChild>
+ <ObjectChild key='epsilon'>
+ <Float />
+ </ObjectChild>
+ <ObjectChild key='exponent'>
+ <Float />
+ </ObjectChild>
+ </Object>
+
+ <Object name='armarx::navigation::human::arondto::ProxemicZone'>
+ <ObjectChild key='pose'>
+ <Pose />
+ </ObjectChild>
+ <ObjectChild key='ellipseA'>
+ <Float />
+ </ObjectChild>
+ <ObjectChild key='ellipseB'>
+ <Float />
+ </ObjectChild>
+ <ObjectChild key='penalty'>
+ <armarx::navigation::human::arondto::ExponentialPenaltyModel />
+ </ObjectChild>
+ <ObjectChild key='weight'>
+ <Float />
+ </ObjectChild>
+ <ObjectChild key='homotopicRelevance'>
+ <Bool />
+ </ObjectChild>
+ </Object>
+
+
<Object name='armarx::navigation::human::arondto::Human'>
<ObjectChild key='pose'>
<Pose />
@@ -12,6 +46,11 @@
<ObjectChild key='detectionTime'>
<Time />
</ObjectChild>
+ <ObjectChild key='proxemicZones'>
+ <List>
+ <armarx::navigation::human::arondto::ProxemicZone />
+ </List>
+ </ObjectChild>
</Object>
<Object name='armarx::navigation::human::arondto::HumanGroup'>
diff --git a/source/armarx/navigation/human/aron_conversions.cpp b/source/armarx/navigation/human/aron_conversions.cpp
index e09a3ca376d5f75790c8adada1038e627c968161..9afef2fe91017c8d6ce3eeec9e3df1c4d2a264e8 100644
--- a/source/armarx/navigation/human/aron_conversions.cpp
+++ b/source/armarx/navigation/human/aron_conversions.cpp
@@ -14,6 +14,15 @@ namespace armarx::navigation::human
dto.pose = conv::to3D(bo.pose).matrix();
dto.linearVelocity = conv::to3D(bo.linearVelocity);
dto.detectionTime = bo.detectionTime;
+ dto.proxemicZones = bo.proxemicZones |
+ ranges::views::transform(
+ [](const ProxemicZone& boZone) -> arondto::ProxemicZone
+ {
+ arondto::ProxemicZone dtoZone;
+ toAron(dtoZone, boZone);
+ return dtoZone;
+ }) |
+ ranges::to_vector;
}
void
@@ -22,6 +31,15 @@ namespace armarx::navigation::human
bo.pose = conv::to2D(core::Pose(dto.pose));
bo.linearVelocity = conv::to2D(dto.linearVelocity);
bo.detectionTime = dto.detectionTime;
+ bo.proxemicZones = dto.proxemicZones |
+ ranges::views::transform(
+ [](const arondto::ProxemicZone& dtoZone) -> ProxemicZone
+ {
+ ProxemicZone boZone;
+ fromAron(dtoZone, boZone);
+ return boZone;
+ }) |
+ ranges::to_vector;
}
@@ -66,4 +84,42 @@ namespace armarx::navigation::human
bo.detectionTime = dto.detectionTime;
}
+ void
+ toAron(arondto::ExponentialPenaltyModel& dto, const ExponentialPenaltyModel& bo)
+ {
+ dto.minDistance = bo.minDistance;
+ dto.epsilon = bo.epsilon;
+ dto.exponent = bo.exponent;
+ }
+
+ void
+ fromAron(const arondto::ExponentialPenaltyModel& dto, ExponentialPenaltyModel& bo)
+ {
+ bo.minDistance = dto.minDistance;
+ bo.epsilon = dto.epsilon;
+ bo.exponent = dto.exponent;
+ }
+
+ void
+ toAron(arondto::ProxemicZone& dto, const ProxemicZone& bo)
+ {
+ dto.pose = conv::to3D(bo.pose).matrix();
+ dto.ellipseA = bo.shape.a;
+ dto.ellipseB = bo.shape.b;
+ toAron(dto.penalty, bo.penalty);
+ dto.weight = bo.weight;
+ dto.homotopicRelevance = bo.homotopicRelevance;
+ }
+
+ void
+ fromAron(const arondto::ProxemicZone& dto, ProxemicZone& bo)
+ {
+ bo.pose = conv::to2D(core::Pose(dto.pose));
+ bo.shape = {.a = dto.ellipseA, .b = dto.ellipseB};
+ fromAron(dto.penalty, bo.penalty);
+ bo.weight = dto.weight;
+ bo.homotopicRelevance = dto.homotopicRelevance;
+ }
+
+
} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/human/aron_conversions.h b/source/armarx/navigation/human/aron_conversions.h
index 71159748a75ed5d44312650a530c80cc2e25e1ab..230a940333fcc086e036fbac9710a896db30631d 100644
--- a/source/armarx/navigation/human/aron_conversions.h
+++ b/source/armarx/navigation/human/aron_conversions.h
@@ -14,6 +14,7 @@
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*
* @author Tobias Gröger ( tobias dot groeger at student dot kit dot edu )
+ * @author Timo Weberruß ( timo dot weberruss at student dot kit dot edu )
* @date 2022
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
* GNU General Public License
@@ -25,11 +26,15 @@ namespace armarx::navigation::human
{
struct Human;
struct HumanGroup;
+ struct ExponentialPenaltyModel;
+ struct ProxemicZone;
namespace arondto
{
struct Human;
struct HumanGroup;
+ struct ExponentialPenaltyModel;
+ struct ProxemicZone;
} // namespace arondto
@@ -43,4 +48,10 @@ namespace armarx::navigation::human
void toAron(arondto::HumanGroup& dto, const HumanGroup& bo);
void fromAron(const arondto::HumanGroup& dto, HumanGroup& bo);
+ void toAron(arondto::ExponentialPenaltyModel& dto, const ExponentialPenaltyModel& bo);
+ void fromAron(const arondto::ExponentialPenaltyModel& dto, ExponentialPenaltyModel& bo);
+
+ void toAron(arondto::ProxemicZone& dto, const ProxemicZone& bo);
+ void fromAron(const arondto::ProxemicZone& dto, ProxemicZone& bo);
+
} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/human/test/human_test.cpp b/source/armarx/navigation/human/test/human_test.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..862c7e3b28abe6bd7e5fd1a10a8c0354aede4a27
--- /dev/null
+++ b/source/armarx/navigation/human/test/human_test.cpp
@@ -0,0 +1,308 @@
+/**
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Timo Weberruß ( timo dot weberruss at student dot kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include <math.h>
+
+#include <algorithm>
+#include <vector>
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/Logging.h>
+
+#include <SemanticObjectRelations/Shapes/Shape.h>
+#include <armarx/navigation/core/Trajectory.h>
+#include <armarx/navigation/human/EuclideanDistance.h>
+#include <armarx/navigation/human/OrientationDistance.h>
+#include <range/v3/view/zip.hpp>
+
+// test includes and other stuff
+#define BOOST_TEST_MODULE Navigation::ArmarXLibraries::core
+#define ARMARX_BOOST_TEST
+
+#include <armarx/navigation/Test.h>
+#include <armarx/navigation/core/basic_types.h>
+#include <armarx/navigation/human/CombinedDistance.h>
+#include <armarx/navigation/human/ConvexHullGenerator.h>
+#include <armarx/navigation/human/HumanGrouper.h>
+#include <armarx/navigation/human/MovementDistance.h>
+#include <armarx/navigation/human/types.h>
+
+using armarx::navigation::core::Pose2D;
+using armarx::navigation::human::CombinedDistance;
+using armarx::navigation::human::ConvexHullGenerator;
+using armarx::navigation::human::EuclideanDistance;
+using armarx::navigation::human::Human;
+using armarx::navigation::human::HumanGroup;
+using armarx::navigation::human::HumanGrouper;
+using armarx::navigation::human::MovementDistance;
+using armarx::navigation::human::OrientationDistance;
+using armarx::navigation::human::shapes::Polygon;
+
+BOOST_AUTO_TEST_CASE(testEuclideanDistance1_standardDistance)
+{
+ const EuclideanDistance distance = EuclideanDistance();
+ Pose2D pose1 = Pose2D::Identity();
+ pose1.translation() = Eigen::Vector2f(3, 4);
+ pose1.linear() = Eigen::Rotation2Df(1.1).toRotationMatrix();
+ const Human h1 = {.pose = pose1,
+ .linearVelocity = Eigen::Vector2f(0.2, 0.1),
+ .detectionTime = armarx::DateTime::Now()};
+
+ Pose2D pose2 = Pose2D::Identity();
+ pose2.translation() = Eigen::Vector2f(6, 8);
+ pose2.linear() = Eigen::Rotation2Df(2.3).toRotationMatrix();
+ const Human h2 = {.pose = pose2,
+ .linearVelocity = Eigen::Vector2f(0.5, 0.8),
+ .detectionTime = armarx::DateTime::Now()};
+
+ double d = distance.computeDistance(h1, h2);
+ BOOST_CHECK_CLOSE(d, 5, 0.001);
+}
+
+BOOST_AUTO_TEST_CASE(testOrientationDistance1_faceToFace)
+{
+ const OrientationDistance distance = OrientationDistance(1, 0);
+ Pose2D pose1 = Pose2D::Identity();
+ pose1.translation() = Eigen::Vector2f(0, 0);
+ pose1.linear() = Eigen::Rotation2Df(0).toRotationMatrix();
+ const Human h1 = {.pose = pose1,
+ .linearVelocity = Eigen::Vector2f(0.2, 0.1),
+ .detectionTime = armarx::DateTime::Now()};
+
+ Pose2D pose2 = Pose2D::Identity();
+ pose2.translation() = Eigen::Vector2f(1, 0);
+ pose2.linear() = Eigen::Rotation2Df(M_PI).toRotationMatrix();
+ const Human h2 = {.pose = pose2,
+ .linearVelocity = Eigen::Vector2f(0.5, 0.8),
+ .detectionTime = armarx::DateTime::Now()};
+
+ double d = distance.computeDistance(h1, h2);
+ BOOST_CHECK(d < 0.001);
+}
+
+BOOST_AUTO_TEST_CASE(testOrientationDistance2_backToBack)
+{
+ const OrientationDistance distance = OrientationDistance(7, 0);
+ Pose2D pose1 = Pose2D::Identity();
+ pose1.translation() = Eigen::Vector2f(2, 2);
+ pose1.linear() = Eigen::Rotation2Df(-M_PI_2).toRotationMatrix();
+ const Human h1 = {.pose = pose1,
+ .linearVelocity = Eigen::Vector2f(0.2, 0.1),
+ .detectionTime = armarx::DateTime::Now()};
+
+ Pose2D pose2 = Pose2D::Identity();
+ pose2.translation() = Eigen::Vector2f(2, 3);
+ pose2.linear() = Eigen::Rotation2Df(M_PI_2).toRotationMatrix();
+ const Human h2 = {.pose = pose2,
+ .linearVelocity = Eigen::Vector2f(0.5, 0.8),
+ .detectionTime = armarx::DateTime::Now()};
+
+ double d = distance.computeDistance(h1, h2);
+ BOOST_CHECK_CLOSE(d, 7, 0.001);
+}
+
+BOOST_AUTO_TEST_CASE(testMovementDistance1_roughlyMovingTogether)
+{
+ const MovementDistance distance = MovementDistance();
+ Pose2D pose1 = Pose2D::Identity();
+ pose1.translation() = Eigen::Vector2f(-2, 3);
+ pose1.linear() = Eigen::Rotation2Df(-M_PI_2).toRotationMatrix();
+ const Human h1 = {.pose = pose1,
+ .linearVelocity = Eigen::Vector2f(1.0, 1.0),
+ .detectionTime = armarx::DateTime::Now()};
+
+ Pose2D pose2 = Pose2D::Identity();
+ pose2.translation() = Eigen::Vector2f(2, 5);
+ pose2.linear() = Eigen::Rotation2Df(M_PI_2).toRotationMatrix();
+ const Human h2 = {.pose = pose2,
+ .linearVelocity = Eigen::Vector2f(1.3, 1.4),
+ .detectionTime = armarx::DateTime::Now()};
+
+ double d = distance.computeDistance(h1, h2);
+ BOOST_CHECK_CLOSE(d, 0.5, 0.001);
+}
+
+BOOST_AUTO_TEST_CASE(testCombinedDistance1_threeDistances)
+{
+ const CombinedDistance combined = CombinedDistance(2, 3);
+ Pose2D pose1 = Pose2D::Identity();
+ pose1.translation() = Eigen::Vector2f(0, 0);
+ pose1.linear() = Eigen::Rotation2Df(-M_PI_2).toRotationMatrix();
+ const Human h1 = {.pose = pose1,
+ .linearVelocity = Eigen::Vector2f(1.0, 1.0),
+ .detectionTime = armarx::DateTime::Now()};
+
+ Pose2D pose2 = Pose2D::Identity();
+ pose2.translation() = Eigen::Vector2f(2, 0);
+ pose2.linear() = Eigen::Rotation2Df(M_PI_2).toRotationMatrix();
+ const Human h2 = {.pose = pose2,
+ .linearVelocity = Eigen::Vector2f(1.3, 1.4),
+ .detectionTime = armarx::DateTime::Now()};
+
+ double d = combined.computeDistance(h1, h2);
+ BOOST_CHECK_CLOSE(d, 2 * 1.5 * 2.5, 0.001);
+}
+
+BOOST_AUTO_TEST_CASE(testConvexHullGenerator1_triangle)
+{
+ ConvexHullGenerator generator = ConvexHullGenerator();
+
+ Pose2D pose1 = Pose2D::Identity();
+ pose1.translation() = Eigen::Vector2f(-2, 0);
+ pose1.linear() = Eigen::Rotation2Df(0).toRotationMatrix();
+ const Human h1 = {.pose = pose1,
+ .linearVelocity = Eigen::Vector2f(0, 0),
+ .detectionTime = armarx::DateTime::Now()};
+
+ Pose2D pose2 = Pose2D::Identity();
+ pose2.translation() = Eigen::Vector2f(2, 0);
+ pose2.linear() = Eigen::Rotation2Df(0).toRotationMatrix();
+ const Human h2 = {.pose = pose2,
+ .linearVelocity = Eigen::Vector2f(0, 0),
+ .detectionTime = armarx::DateTime::Now()};
+
+ Pose2D pose3 = Pose2D::Identity();
+ pose3.translation() = Eigen::Vector2f(0, 2);
+ pose3.linear() = Eigen::Rotation2Df(0).toRotationMatrix();
+ const Human h3 = {.pose = pose3,
+ .linearVelocity = Eigen::Vector2f(0, 0),
+ .detectionTime = armarx::DateTime::Now()};
+
+ Pose2D pose4 = Pose2D::Identity();
+ pose4.translation() = Eigen::Vector2f(0, 1);
+ pose4.linear() = Eigen::Rotation2Df(0).toRotationMatrix();
+ const Human h4 = {.pose = pose4,
+ .linearVelocity = Eigen::Vector2f(0, 0),
+ .detectionTime = armarx::DateTime::Now()};
+
+
+ std::vector<Human> humans = {h1, h2, h3, h4};
+
+ HumanGroup group = {.shape = {{}}, .humans = humans, .detectionTime = armarx::DateTime::Now()};
+
+ Polygon hull = generator.createShape(group);
+
+ // Check that the vertices of the polygon contain all 3 positions from the outer 3 people
+ BOOST_CHECK(std::find(hull.vertices.begin(), hull.vertices.end(), pose1.translation()) !=
+ hull.vertices.end());
+
+ BOOST_CHECK(std::find(hull.vertices.begin(), hull.vertices.end(), pose2.translation()) !=
+ hull.vertices.end());
+
+ BOOST_CHECK(std::find(hull.vertices.begin(), hull.vertices.end(), pose3.translation()) !=
+ hull.vertices.end());
+
+
+ // Check that the hull doesn't contain the inner person
+ BOOST_CHECK(std::find(hull.vertices.begin(), hull.vertices.end(), pose4.translation()) ==
+ hull.vertices.end());
+}
+
+BOOST_AUTO_TEST_CASE(testConvexHullGenerator2_line)
+{
+ ConvexHullGenerator generator = ConvexHullGenerator();
+
+ Pose2D pose1 = Pose2D::Identity();
+ pose1.translation() = Eigen::Vector2f(-2, 0);
+ pose1.linear() = Eigen::Rotation2Df(0).toRotationMatrix();
+ const Human h1 = {.pose = pose1,
+ .linearVelocity = Eigen::Vector2f(0, 0),
+ .detectionTime = armarx::DateTime::Now()};
+
+ Pose2D pose2 = Pose2D::Identity();
+ pose2.translation() = Eigen::Vector2f(2, 0);
+ pose2.linear() = Eigen::Rotation2Df(0).toRotationMatrix();
+ const Human h2 = {.pose = pose2,
+ .linearVelocity = Eigen::Vector2f(0, 0),
+ .detectionTime = armarx::DateTime::Now()};
+
+ std::vector<Human> humans = {h1, h2};
+
+ HumanGroup group = {.shape = {{}}, .humans = humans, .detectionTime = armarx::DateTime::Now()};
+
+ Polygon hull = generator.createShape(group);
+
+ // Check that the vertices of the polygon contains both positions
+ BOOST_CHECK(std::find(hull.vertices.begin(), hull.vertices.end(), pose1.translation()) !=
+ hull.vertices.end());
+
+ BOOST_CHECK(std::find(hull.vertices.begin(), hull.vertices.end(), pose2.translation()) !=
+ hull.vertices.end());
+}
+
+BOOST_AUTO_TEST_CASE(testHumanGrouper1_twoOfThree)
+{
+ /* ^ ^
+ * | |
+ * h1> <h2
+ *
+ * h3
+ * V
+ */
+
+ Pose2D pose1 = Pose2D::Identity();
+ pose1.translation() = Eigen::Vector2f(2, 1);
+ pose1.linear() = Eigen::Rotation2Df(0).toRotationMatrix();
+ const Human h1 = {.pose = pose1,
+ .linearVelocity = Eigen::Vector2f(0, 1),
+ .detectionTime = armarx::DateTime::Now()};
+
+ Pose2D pose2 = Pose2D::Identity();
+ pose2.translation() = Eigen::Vector2f(3, 0.9);
+ pose2.linear() = Eigen::Rotation2Df(M_PI).toRotationMatrix();
+ const Human h2 = {.pose = pose2,
+ .linearVelocity = Eigen::Vector2f(0, 1.1),
+ .detectionTime = armarx::DateTime::Now()};
+
+ Pose2D pose3 = Pose2D::Identity();
+ pose3.translation() = Eigen::Vector2f(2.5, 0);
+ pose3.linear() = Eigen::Rotation2Df(-M_PI_2).toRotationMatrix();
+ const Human h3 = {.pose = pose3,
+ .linearVelocity = Eigen::Vector2f(0, 0),
+ .detectionTime = armarx::DateTime::Now()};
+
+ HumanGrouper::GroupingSettings settings = {
+ .groupingThreshold = 2, .maxOrientationInfluence = 3, .movementInfluence = 2};
+
+ HumanGrouper grouper = HumanGrouper(settings);
+
+ std::vector<Human> humans = {h1, h2, h3};
+ grouper.updateHumans(humans);
+
+ std::vector<HumanGroup> groups = grouper.getCurrentGroups();
+
+ BOOST_CHECK_EQUAL(groups.size(), 2);
+ HumanGroup g1 = groups.at(0);
+ HumanGroup g2 = groups.at(1);
+
+ BOOST_CHECK((g1.humans.size() == 2) != (g2.humans.size() == 2));
+ HumanGroup& groupOfTwo = g1.humans.size() == 2 ? g1 : g2;
+
+
+ // Check that the group of two contains h1 and h2
+ BOOST_CHECK(std::find(groupOfTwo.humans.begin(), groupOfTwo.humans.end(), h1) !=
+ groupOfTwo.humans.end());
+ BOOST_CHECK(std::find(groupOfTwo.humans.begin(), groupOfTwo.humans.end(), h2) !=
+ groupOfTwo.humans.end());
+
+ // Check that a shape was set (the detailed convex hull generation is tested seperatly)
+ BOOST_CHECK(groupOfTwo.shape.vertices.size() > 0);
+}
diff --git a/source/armarx/navigation/human/test/human_tracker_test.cpp b/source/armarx/navigation/human/test/human_tracker_test.cpp
index 322021491589c466cccc1981fe946819c4caf812..0e45d1a8da21fb717e8177d3c6c0fe1ff869fa72 100644
--- a/source/armarx/navigation/human/test/human_tracker_test.cpp
+++ b/source/armarx/navigation/human/test/human_tracker_test.cpp
@@ -239,7 +239,7 @@ namespace armarx::navigation::human
BOOST_CHECK_EQUAL(tracker.getTrackedHumans().size(), 1);
- CamMm late = {.detectionTime = DateTime(Duration::MilliSeconds(1000))};
+ CamMm late = {.detectionTime = DateTime(Duration::MilliSeconds(1000)), .humanPoses = {}};
tracker.update(late);
BOOST_CHECK_EQUAL(tracker.getTrackedHumans().size(), 0);
diff --git a/source/armarx/navigation/human/types.cpp b/source/armarx/navigation/human/types.cpp
index 854b0d15569474955bde4550b77c6a168aa8238e..8bf473be7ddf059411f47b62d9373258cc688d6e 100644
--- a/source/armarx/navigation/human/types.cpp
+++ b/source/armarx/navigation/human/types.cpp
@@ -14,6 +14,15 @@ namespace armarx::navigation::human
return estimation;
}
+ bool
+ Human::operator==(const Human& other) const
+ {
+ return other.pose.matrix() == this->pose.matrix() &&
+ other.linearVelocity == this->linearVelocity &&
+ other.detectionTime == this->detectionTime;
+ }
+
+
aron::data::DictPtr
Human::toAron() const
{
@@ -34,6 +43,46 @@ namespace armarx::navigation::human
return bo;
}
+ aron::data::DictPtr
+ ExponentialPenaltyModel::toAron() const
+ {
+ arondto::ExponentialPenaltyModel dto;
+ human::toAron(dto, *this);
+ return dto.toAron();
+ }
+
+ ExponentialPenaltyModel
+ ExponentialPenaltyModel::FromAron(const aron::data::DictPtr& dict)
+ {
+ ARMARX_CHECK_NOT_NULL(dict);
+ arondto::ExponentialPenaltyModel dto;
+ dto.fromAron(dict);
+
+ ExponentialPenaltyModel bo;
+ fromAron(dto, bo);
+ return bo;
+ }
+
+ aron::data::DictPtr
+ ProxemicZone::toAron() const
+ {
+ arondto::ProxemicZone dto;
+ human::toAron(dto, *this);
+ return dto.toAron();
+ }
+
+ ProxemicZone
+ ProxemicZone::FromAron(const aron::data::DictPtr& dict)
+ {
+ ARMARX_CHECK_NOT_NULL(dict);
+ arondto::ProxemicZone dto;
+ dto.fromAron(dict);
+
+ ProxemicZone bo;
+ fromAron(dto, bo);
+ return bo;
+ }
+
aron::data::DictPtr
HumanGroup::toAron() const
diff --git a/source/armarx/navigation/human/types.h b/source/armarx/navigation/human/types.h
index 0c3e08043b9fcea45aa603a0ef01d0eeba87f63d..f519375e07da4466c50a3a2839a447987639fbc8 100644
--- a/source/armarx/navigation/human/types.h
+++ b/source/armarx/navigation/human/types.h
@@ -30,14 +30,47 @@
namespace armarx::navigation::human
{
+ struct LinearPenaltyModel
+ {
+ float minDistance; // [m]
+ float epsilon; // [m]
+ };
+
+ struct ExponentialPenaltyModel
+ {
+ float minDistance; // [m]
+ float epsilon; // [m]
+ float exponent;
+
+ aron::data::DictPtr toAron() const;
+ static ExponentialPenaltyModel FromAron(const aron::data::DictPtr& dict);
+ };
+
+ struct ProxemicZone
+ {
+ core::Pose2D pose;
+ shapes::Ellipse shape;
+ ExponentialPenaltyModel penalty;
+ float weight;
+ bool homotopicRelevance = true;
+
+ aron::data::DictPtr toAron() const;
+ static ProxemicZone FromAron(const aron::data::DictPtr& dict);
+ };
+
+ using ProxemicZones = std::vector<ProxemicZone>;
+
struct Human
{
core::Pose2D pose;
Eigen::Vector2f linearVelocity;
DateTime detectionTime;
+ ProxemicZones proxemicZones = {};
core::Pose2D estimateAt(const DateTime& time) const;
+ bool operator==(const Human& other) const;
+
aron::data::DictPtr toAron() const;
static Human FromAron(const aron::data::DictPtr& dict);
};
@@ -57,28 +90,4 @@ namespace armarx::navigation::human
using HumanGroups = std::vector<HumanGroup>;
- struct LinearPenaltyModel
- {
- float minDistance; // [m]
- float epsilon; // [m]
- };
-
- struct ExponentialPenaltyModel
- {
- float minDistance; // [m]
- float epsilon; // [m]
- float exponent;
- };
-
- struct ProxemicZone
- {
- core::Pose2D pose;
- shapes::Ellipse shape;
- ExponentialPenaltyModel penalty;
- float weight;
- bool homotopicRelevance = true;
- };
-
- using ProxemicZones = std::vector<ProxemicZone>;
-
} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/memory/CMakeLists.txt b/source/armarx/navigation/memory/CMakeLists.txt
index 267142f9ea3e658d9c9b1770476061f988453565..9059612ce21b6425a880412b2e526a519038ac2c 100644
--- a/source/armarx/navigation/memory/CMakeLists.txt
+++ b/source/armarx/navigation/memory/CMakeLists.txt
@@ -11,6 +11,8 @@ armarx_add_library(memory
client/costmap/Reader.cpp
client/human/Reader.cpp
client/human/Writer.cpp
+ client/human_group/Reader.cpp
+ client/human_group/Writer.cpp
# ./client/events/Reader.cpp
HEADERS
memory.h
@@ -24,6 +26,8 @@ armarx_add_library(memory
client/costmap/Reader.h
client/human/Reader.h
client/human/Writer.h
+ client/human_group/Reader.h
+ client/human_group/Writer.h
# ./client/events/Reader.h
DEPENDENCIES
ArmarXCoreInterfaces
diff --git a/source/armarx/navigation/memory/client/human/Reader.cpp b/source/armarx/navigation/memory/client/human/Reader.cpp
index f1ad2e2b96594dcf79c9b7ce5932de2fcccd320f..905eb69e53dff5ee0ea06ac0efa2bbb09e2cc980 100644
--- a/source/armarx/navigation/memory/client/human/Reader.cpp
+++ b/source/armarx/navigation/memory/client/human/Reader.cpp
@@ -41,23 +41,6 @@ namespace armarx::navigation::memory::client::human
return qb;
}
- armarx::armem::client::query::Builder
- Reader::buildHumanGroupsQuery(const Query& query) const
- {
- armarx::armem::client::query::Builder qb;
-
- // clang-format off
- qb
- .coreSegments().withName(properties().coreSegmentName)
- .providerSegments().withName(query.providerName)
- .entities().withName("groups")
- .snapshots().beforeOrAtTime(query.timestamp);
- // clang-format on
-
- return qb;
- }
-
-
std::string
Reader::propertyPrefix() const
{
@@ -107,95 +90,6 @@ namespace armarx::navigation::memory::client::human
return humans;
}
- navigation::human::HumanGroups
- asGroups(const armem::wm::ProviderSegment& providerSegment,
- const DateTime& timestamp,
- const Duration& maxAge)
- {
- navigation::human::HumanGroups humans;
-
- ARMARX_CHECK(not providerSegment.empty()) << "No entities";
- ARMARX_CHECK(providerSegment.size() == 1) << "There should be only one entity!";
-
- providerSegment.forEachEntity(
- [&humans, ×tamp, &maxAge](const armem::wm::Entity& entity)
- {
- const auto& entitySnapshot = entity.getLatestSnapshot();
- ARMARX_CHECK(not entitySnapshot.empty()) << "No entity snapshot instances";
-
- entitySnapshot.forEachInstance(
- [&](const armem::wm::EntityInstance& entityInstance)
- {
- const Duration dtToNow = timestamp - entityInstance.metadata().timeCreated;
-
- if (dtToNow < maxAge and dtToNow.isPositive())
- {
- const auto dto = navigation::human::arondto::HumanGroup::FromAron(
- entityInstance.data());
-
- navigation::human::HumanGroup human;
- fromAron(dto, human);
- humans.push_back(human);
- }
- });
- });
-
- return humans;
- }
-
- Reader::HumanGroupResult
- Reader::queryHumanGroups(const Query& query) const
- {
- const auto qb = buildHumansQuery(query);
-
- ARMARX_DEBUG << "[MappingDataReader] query ... ";
-
- const armem::client::QueryResult qResult = memoryReader().query(qb.buildQueryInput());
-
- ARMARX_DEBUG << "[MappingDataReader] result: " << qResult;
-
- if (not qResult.success)
- {
- ARMARX_WARNING << "Failed to query data from memory: " << qResult.errorMessage;
- return {.groups = {},
- .status = HumanGroupResult::Status::Error,
- .errorMessage = qResult.errorMessage};
- }
-
- const auto coreSegment = qResult.memory.getCoreSegment(properties().coreSegmentName);
-
- if (not coreSegment.hasProviderSegment(query.providerName))
- {
- ARMARX_DEBUG << "Provider segment `" << query.providerName << "` does not exist (yet).";
- return {.groups = {}, .status = HumanGroupResult::Status::NoData};
- }
-
- const armem::wm::ProviderSegment& providerSegment =
- coreSegment.getProviderSegment(query.providerName);
-
- if (providerSegment.empty())
- {
- ARMARX_DEBUG << "No entities.";
- return {.groups = {},
- .status = HumanGroupResult::Status::NoData,
- .errorMessage = "No entities"};
- }
-
- try
- {
- return HumanGroupResult{.groups =
- asGroups(providerSegment, query.timestamp, query.maxAge),
- .status = HumanGroupResult::Status::Success};
- }
- catch (...)
- {
- return HumanGroupResult{.groups = {},
- .status = HumanGroupResult::Status::Error,
- .errorMessage = GetHandledExceptionString()};
- }
- }
-
-
Reader::HumanResult
Reader::queryHumans(const Query& query) const
{
diff --git a/source/armarx/navigation/memory/client/human/Reader.h b/source/armarx/navigation/memory/client/human/Reader.h
index 91a17afaad4cff3b4a15ad7bc11b9be0b1cb1fd2..3a47556a072f3528cc6567d5dec1ce91ee3a8ba6 100644
--- a/source/armarx/navigation/memory/client/human/Reader.h
+++ b/source/armarx/navigation/memory/client/human/Reader.h
@@ -65,31 +65,10 @@ namespace armarx::navigation::memory::client::human
}
};
- struct HumanGroupResult
- {
- armarx::navigation::human::HumanGroups groups;
-
- enum Status
- {
- Success,
- NoData,
- Error
- } status;
-
- std::string errorMessage = "";
-
- operator bool() const noexcept
- {
- return status == Status::Success;
- }
- };
-
HumanResult queryHumans(const Query& query) const;
- HumanGroupResult queryHumanGroups(const Query& query) const;
protected:
::armarx::armem::client::query::Builder buildHumansQuery(const Query& query) const;
- ::armarx::armem::client::query::Builder buildHumanGroupsQuery(const Query& query) const;
std::string propertyPrefix() const override;
Properties defaultProperties() const override;
diff --git a/source/armarx/navigation/memory/client/human/Writer.cpp b/source/armarx/navigation/memory/client/human/Writer.cpp
index c38c2ac775e064890eecbf12d95c55ce017fb4a8..97767d14c5901e39f1f3cc8d309e324c0437d6e7 100644
--- a/source/armarx/navigation/memory/client/human/Writer.cpp
+++ b/source/armarx/navigation/memory/client/human/Writer.cpp
@@ -6,8 +6,8 @@
#include <armarx/navigation/algorithms/aron/Costmap.aron.generated.h>
#include <armarx/navigation/algorithms/aron_conversions.h>
#include <armarx/navigation/human/aron/Human.aron.generated.h>
-#include <armarx/navigation/memory/constants.h>
#include <armarx/navigation/human/aron_conversions.h>
+#include <armarx/navigation/memory/constants.h>
namespace armarx::navigation::memory::client::human
@@ -42,62 +42,12 @@ namespace armarx::navigation::memory::client::human
std::transform(humans.begin(),
humans.end(),
std::back_inserter(update.instancesData),
- [](const navigation::human::Human& human) -> armarx::aron::data::DictPtr {
- navigation::human::arondto::Human dto;
- toAron(dto, human);
-
- return dto.toAron();
- });
-
-
- update.timeCreated = timestamp;
-
- ARMARX_DEBUG << "Committing " << update << " at time " << timestamp;
- armem::EntityUpdateResult updateResult = memoryWriter().commit(update);
-
- ARMARX_DEBUG << updateResult;
-
- if (not updateResult.success)
- {
- ARMARX_ERROR << updateResult.errorMessage;
- }
-
- return updateResult.success;
- }
-
- bool
- Writer::store(const armarx::navigation::human::HumanGroups& groups,
- // const std::string& name,
- const std::string& providerName,
- const armem::Time& timestamp)
- {
- std::lock_guard g{memoryWriterMutex()};
-
- const auto result =
- memoryWriter().addSegment(memory::constants::HumanCoreSegmentName, providerName);
-
- if (not result.success)
- {
- ARMARX_ERROR << result.errorMessage;
-
- // TODO(fabian.reister): message
- return false;
- }
-
- const auto providerId = armem::MemoryID(result.segmentID);
- const auto entityID = providerId.withEntityName("groups").withTimestamp(timestamp);
-
- armem::EntityUpdate update;
- update.entityID = entityID;
-
- std::transform(groups.begin(),
- groups.end(),
- std::back_inserter(update.instancesData),
- [](const navigation::human::HumanGroup& group) -> armarx::aron::data::DictPtr {
- navigation::human::arondto::HumanGroup dto;
- toAron(dto, group);
+ [](const navigation::human::Human& human) -> armarx::aron::data::DictPtr
+ {
+ navigation::human::arondto::Human dto;
+ toAron(dto, human);
- return dto.toAron();
+ return dto.toAron();
});
diff --git a/source/armarx/navigation/memory/client/human/Writer.h b/source/armarx/navigation/memory/client/human/Writer.h
index b224149c4aeb3c4858530f19ed051700060fc179..9afb179ae78b87bc808b6f87cf1919625278957b 100644
--- a/source/armarx/navigation/memory/client/human/Writer.h
+++ b/source/armarx/navigation/memory/client/human/Writer.h
@@ -51,12 +51,7 @@ namespace armarx::navigation::memory::client::human
~Writer() override;
bool store(const armarx::navigation::human::Humans& humans,
- // const std::string& name,
- const std::string& providerName,
- const armem::Time& timestamp);
-
- bool store(const armarx::navigation::human::HumanGroups& groups,
- // const std::string& name,
+ // const std::string& name,
const std::string& providerName,
const armem::Time& timestamp);
diff --git a/source/armarx/navigation/memory/client/human_group/Reader.cpp b/source/armarx/navigation/memory/client/human_group/Reader.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0f6c8ad53f7ca17c1d7139384e8a805a06527f9a
--- /dev/null
+++ b/source/armarx/navigation/memory/client/human_group/Reader.cpp
@@ -0,0 +1,146 @@
+#include "Reader.h"
+
+#include <ArmarXCore/core/exceptions/LocalException.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
+#include <RobotAPI/interface/armem/server/ReadingMemoryInterface.h>
+#include <RobotAPI/libraries/armem/client/Query.h>
+#include <RobotAPI/libraries/armem/client/Reader.h>
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
+#include <RobotAPI/libraries/armem/client/query/selectors.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+#include <RobotAPI/libraries/armem/util/util.h>
+#include <RobotAPI/libraries/aron/core/Exception.h>
+#include <RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h>
+
+#include "armarx/navigation/human/types.h"
+#include <armarx/navigation/algorithms/Costmap.h>
+#include <armarx/navigation/algorithms/aron_conversions.h>
+#include <armarx/navigation/human/aron/Human.aron.generated.h>
+#include <armarx/navigation/human/aron_conversions.h>
+#include <armarx/navigation/memory/constants.h>
+
+namespace armarx::navigation::memory::client::human_group
+{
+ Reader::~Reader() = default;
+
+ armarx::armem::client::query::Builder
+ Reader::buildHumanGroupsQuery(const Query& query) const
+ {
+ armarx::armem::client::query::Builder qb;
+
+ // clang-format off
+ qb
+ .coreSegments().withName(properties().coreSegmentName)
+ .providerSegments().withName(query.providerName)
+ .entities().withName("groups")
+ .snapshots().beforeOrAtTime(query.timestamp);
+ // clang-format on
+
+ return qb;
+ }
+
+
+ std::string
+ Reader::propertyPrefix() const
+ {
+ return "mem.nav.human_group.";
+ }
+
+ armarx::armem::client::util::SimpleReaderBase::Properties
+ Reader::defaultProperties() const
+ {
+ return {.memoryName = memory::constants::NavigationMemoryName,
+ .coreSegmentName = memory::constants::HumanGroupCoreSegmentName};
+ }
+
+ navigation::human::HumanGroups
+ asGroups(const armem::wm::ProviderSegment& providerSegment,
+ const DateTime& timestamp,
+ const Duration& maxAge)
+ {
+ navigation::human::HumanGroups humanGroups;
+
+ ARMARX_CHECK(not providerSegment.empty()) << "No entities";
+ ARMARX_CHECK(providerSegment.size() == 1) << "There should be only one entity!";
+
+ providerSegment.forEachEntity(
+ [&humanGroups, ×tamp, &maxAge](const armem::wm::Entity& entity)
+ {
+ const auto& entitySnapshot = entity.getLatestSnapshot();
+ ARMARX_CHECK(not entitySnapshot.empty()) << "No entity snapshot instances";
+
+ entitySnapshot.forEachInstance(
+ [&](const armem::wm::EntityInstance& entityInstance)
+ {
+ const Duration dtToNow = timestamp - entityInstance.metadata().timeCreated;
+
+ if (dtToNow < maxAge and dtToNow.isPositive())
+ {
+ const auto dto = navigation::human::arondto::HumanGroup::FromAron(
+ entityInstance.data());
+
+ navigation::human::HumanGroup humanGroup;
+ fromAron(dto, humanGroup);
+ humanGroups.push_back(humanGroup);
+ }
+ });
+ });
+
+ return humanGroups;
+ }
+
+ Reader::HumanGroupResult
+ Reader::queryHumanGroups(const Query& query) const
+ {
+ const auto qb = buildHumanGroupsQuery(query);
+
+ ARMARX_DEBUG << "[MappingDataReader] query ... ";
+
+ const armem::client::QueryResult qResult = memoryReader().query(qb.buildQueryInput());
+
+ ARMARX_DEBUG << "[MappingDataReader] result: " << qResult;
+
+ if (not qResult.success)
+ {
+ ARMARX_WARNING << "Failed to query data from memory: " << qResult.errorMessage;
+ return {.groups = {},
+ .status = HumanGroupResult::Status::Error,
+ .errorMessage = qResult.errorMessage};
+ }
+
+ const auto coreSegment = qResult.memory.getCoreSegment(properties().coreSegmentName);
+
+ if (not coreSegment.hasProviderSegment(query.providerName))
+ {
+ ARMARX_DEBUG << "Provider segment `" << query.providerName << "` does not exist (yet).";
+ return {.groups = {}, .status = HumanGroupResult::Status::NoData};
+ }
+
+ const armem::wm::ProviderSegment& providerSegment =
+ coreSegment.getProviderSegment(query.providerName);
+
+ if (providerSegment.empty())
+ {
+ ARMARX_DEBUG << "No entities.";
+ return {.groups = {},
+ .status = HumanGroupResult::Status::NoData,
+ .errorMessage = "No entities"};
+ }
+
+ try
+ {
+ return HumanGroupResult{.groups =
+ asGroups(providerSegment, query.timestamp, query.maxAge),
+ .status = HumanGroupResult::Status::Success};
+ }
+ catch (...)
+ {
+ return HumanGroupResult{.groups = {},
+ .status = HumanGroupResult::Status::Error,
+ .errorMessage = GetHandledExceptionString()};
+ }
+ }
+
+} // namespace armarx::navigation::memory::client::human_group
diff --git a/source/armarx/navigation/memory/client/human_group/Reader.h b/source/armarx/navigation/memory/client/human_group/Reader.h
new file mode 100644
index 0000000000000000000000000000000000000000..537ac74d20f17ecf4253cef01444929b46a74348
--- /dev/null
+++ b/source/armarx/navigation/memory/client/human_group/Reader.h
@@ -0,0 +1,77 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <mutex>
+
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
+#include <RobotAPI/libraries/armem/client/util/SimpleReaderBase.h>
+#include <RobotAPI/libraries/armem/core/Time.h>
+
+#include "armarx/navigation/human/types.h"
+#include <armarx/navigation/algorithms/Costmap.h>
+
+namespace armarx::navigation::memory::client::human_group
+{
+
+ class Reader : virtual public armarx::armem::client::util::SimpleReaderBase
+ {
+ public:
+ using armarx::armem::client::util::SimpleReaderBase::SimpleReaderBase;
+ ~Reader() override;
+
+ struct Query
+ {
+ std::string providerName;
+ armem::Time timestamp;
+ Duration maxAge;
+ };
+
+ struct HumanGroupResult
+ {
+ armarx::navigation::human::HumanGroups groups;
+
+ enum Status
+ {
+ Success,
+ NoData,
+ Error
+ } status;
+
+ std::string errorMessage = "";
+
+ operator bool() const noexcept
+ {
+ return status == Status::Success;
+ }
+ };
+
+ HumanGroupResult queryHumanGroups(const Query& query) const;
+
+ protected:
+ ::armarx::armem::client::query::Builder buildHumanGroupsQuery(const Query& query) const;
+
+ std::string propertyPrefix() const override;
+ Properties defaultProperties() const override;
+ };
+
+} // namespace armarx::navigation::memory::client::human_group
diff --git a/source/armarx/navigation/memory/client/human_group/Writer.cpp b/source/armarx/navigation/memory/client/human_group/Writer.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2d38c45d4608672a5913f5a7cdc94329f2e34626
--- /dev/null
+++ b/source/armarx/navigation/memory/client/human_group/Writer.cpp
@@ -0,0 +1,83 @@
+#include "Writer.h"
+
+#include <iterator>
+
+#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/human/aron/Human.aron.generated.h>
+#include <armarx/navigation/human/aron_conversions.h>
+#include <armarx/navigation/memory/constants.h>
+
+
+namespace armarx::navigation::memory::client::human_group
+{
+ Writer::~Writer() = default;
+
+ bool
+ Writer::store(const armarx::navigation::human::HumanGroups& groups,
+ // const std::string& name,
+ const std::string& providerName,
+ const armem::Time& timestamp)
+ {
+ std::lock_guard g{memoryWriterMutex()};
+
+ const auto result =
+ memoryWriter().addSegment(memory::constants::HumanGroupCoreSegmentName, providerName);
+
+ if (not result.success)
+ {
+ ARMARX_ERROR << result.errorMessage;
+
+ // TODO(fabian.reister): message
+ return false;
+ }
+
+ const auto providerId = armem::MemoryID(result.segmentID);
+ const auto entityID = providerId.withEntityName("groups").withTimestamp(timestamp);
+
+ armem::EntityUpdate update;
+ update.entityID = entityID;
+
+ std::transform(groups.begin(),
+ groups.end(),
+ std::back_inserter(update.instancesData),
+ [](const navigation::human::HumanGroup& group) -> armarx::aron::data::DictPtr
+ {
+ navigation::human::arondto::HumanGroup dto;
+ toAron(dto, group);
+
+ return dto.toAron();
+ });
+
+
+ update.timeCreated = timestamp;
+
+ ARMARX_DEBUG << "Committing " << update << " at time " << timestamp;
+ armem::EntityUpdateResult updateResult = memoryWriter().commit(update);
+
+ ARMARX_DEBUG << updateResult;
+
+ if (not updateResult.success)
+ {
+ ARMARX_ERROR << updateResult.errorMessage;
+ }
+
+ return updateResult.success;
+ }
+
+ std::string
+ Writer::propertyPrefix() const
+ {
+ return "mem.nav.human_group.";
+ }
+
+ armarx::armem::client::util::SimpleWriterBase::SimpleWriterBase::Properties
+ Writer::defaultProperties() const
+ {
+ return SimpleWriterBase::Properties{.memoryName = memory::constants::NavigationMemoryName,
+ .coreSegmentName =
+ memory::constants::HumanGroupCoreSegmentName};
+ }
+
+} // namespace armarx::navigation::memory::client::human_group
diff --git a/source/armarx/navigation/memory/client/human_group/Writer.h b/source/armarx/navigation/memory/client/human_group/Writer.h
new file mode 100644
index 0000000000000000000000000000000000000000..c3de9d343a4c83c95ada602d146c339e361fa86b
--- /dev/null
+++ b/source/armarx/navigation/memory/client/human_group/Writer.h
@@ -0,0 +1,64 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <mutex>
+
+#include <RobotAPI/libraries/armem/client/util/SimpleWriterBase.h>
+#include <RobotAPI/libraries/armem_vision/types.h>
+
+#include <armarx/navigation/algorithms/Costmap.h>
+#include <armarx/navigation/human/types.h>
+
+namespace armarx::navigation::memory::client::human_group
+{
+
+ /**
+ * @defgroup Component-ExampleClient ExampleClient
+ * @ingroup RobotAPI-Components
+ * A description of the component ExampleClient.
+ *
+ * @class ExampleClient
+ * @ingroup Component-ExampleClient
+ * @brief Brief description of class ExampleClient.
+ *
+ * Detailed description of class ExampleClient.
+ */
+ class Writer : virtual public armarx::armem::client::util::SimpleWriterBase
+ {
+ public:
+ using armarx::armem::client::util::SimpleWriterBase::SimpleWriterBase;
+ ~Writer() override;
+
+ bool store(const armarx::navigation::human::HumanGroups& groups,
+ // const std::string& name,
+ const std::string& providerName,
+ const armem::Time& timestamp);
+
+ protected:
+ std::string propertyPrefix() const override;
+ Properties defaultProperties() const override;
+ };
+
+
+} // namespace armarx::navigation::memory::client::human_group
diff --git a/source/armarx/navigation/memory/constants.h b/source/armarx/navigation/memory/constants.h
index f0932218de3bba512f5bbabaafba864e2e1afc37..23c01a1c112d74e1345f7e8f58bfe96ec1af190d 100644
--- a/source/armarx/navigation/memory/constants.h
+++ b/source/armarx/navigation/memory/constants.h
@@ -33,6 +33,7 @@ namespace armarx::navigation::memory::constants
inline const std::string LocationCoreSegmentName = "Location";
inline const std::string CostmapCoreSegmentName = "Costmap";
inline const std::string HumanCoreSegmentName = "Human";
+ inline const std::string HumanGroupCoreSegmentName = "HumanGroup";
inline const std::string GlobalPlannerResultCoreSegment = "Results_GlobalPlanner";
inline const std::string LocalPlannerResultCoreSegment = "Results_LocalPlanner";