Skip to content
Snippets Groups Projects
Commit ab40a345 authored by Timo Weberruß's avatar Timo Weberruß
Browse files

Start implementing group visualization

parent 213b53ae
No related branches found
No related tags found
2 merge requests!109Social layers,!55Draft: Implement human grouping
This commit is part of merge request !109. Comments created here will be created in the context of that merge request.
Hide whitespace changes
Inline Side-by-side
source/armarx/navigation/components/navigation_memory/Component.cpp
+ 10
− 8
Edit View file @ ab40a345
......@@ -41,16 +41,16 @@
#include "Visu.h"
#include <armarx/navigation/algorithms/Costmap.h>
#include <armarx/navigation/memory/constants.h>
#include <armarx/navigation/algorithms/aron/Costmap.aron.generated.h>
#include <armarx/navigation/core/Graph.h>
#include <armarx/navigation/core/aron/Graph.aron.generated.h>
#include <armarx/navigation/core/aron/Location.aron.generated.h>
#include <armarx/navigation/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>
namespace armarx::navigation::components::navigation_memory
......@@ -440,11 +440,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;
......@@ -484,4 +486,4 @@ namespace armarx::navigation::components::navigation_memory
}
} // namespace armarx::navigation::components::navigation_memory
} // namespace armarx::navigation::components::navigation_memory
source/armarx/navigation/components/navigation_memory/Visu.cpp
+ 79
− 6
Edit View file @ ab40a345
......@@ -21,6 +21,7 @@
*/
#include "Visu.h"
#include <Eigen/src/Geometry/Translation.h>
#include <SimoxUtility/color/Color.h>
......@@ -52,12 +53,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>())
{
}
......@@ -180,7 +183,7 @@ namespace armarx::navigation::memory
visualize(const human::Humans& humans, viz::Layer& layer)
{
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)
......@@ -198,10 +201,38 @@ namespace armarx::navigation::memory
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.scale(1.7); // 1.7m
mmm.overrideColor(viz::Color::orange(255, 100));
layer.add(mmm);
}
}
void
visualize(const human::HumanGroups& humanGroups, viz::Layer& layer)
{
const Eigen::Translation3f human_T_mmm(Eigen::Vector3f{0, 0, 1000});
for (const auto& humanGroup : humanGroups)
{
viz::Polygon polygon();
polygon.addPoints(humanGroup.shape.vertices);
// viz::Cylinder cylinder(std::to_string(layer.size()));
// cylinder.fromTo(conv::to3D(human.pose.translation()),
// conv::to3D(human.pose.translation()) + Eigen::Vector3f{0, 0, 10});
// cylinder.color(simox::Color::orange());
// cylinder.radius(300);
// layer.add(cylinder);
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, 100));
layer.add(mmm);
}
}
......@@ -266,7 +297,8 @@ namespace armarx::navigation::memory
[&](const wm::Entity& entity)
{
entity.getLatestSnapshot().forEachInstance(
[&namedProviderHumans](const armarx::armem::wm::EntityInstance& instance)
[&namedProviderHumans](
const armarx::armem::wm::EntityInstance& instance)
{
const auto dto =
navigation::human::arondto::Human::FromAron(instance.data());
......@@ -274,8 +306,8 @@ namespace armarx::navigation::memory
navigation::human::Human human;
fromAron(dto, human);
namedProviderHumans[instance.id().providerSegmentName]
.emplace_back(std::move(human));
namedProviderHumans[instance.id().providerSegmentName].emplace_back(
std::move(human));
});
});
});
......@@ -287,5 +319,46 @@ namespace armarx::navigation::memory
}
}
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)
{
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("humans_" + providerName));
visualize(humanGroups, layer);
}
}
} // namespace armarx::navigation::memory
source/armarx/navigation/components/navigation_memory/Visu.h
+ 4
− 1
Edit View file @ ab40a345
......@@ -48,7 +48,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 +57,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);
void drawHumanGroups(std::vector<viz::Layer>& layers, bool enabled);
viz::ScopedClient arviz;
......@@ -64,6 +66,7 @@ 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;
};
......
source/armarx/navigation/memory/constants.h
+ 1
− 0
Edit View file @ ab40a345
......@@ -33,5 +33,6 @@ 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";
} // namespace armarx::navigation::memory::constants
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment