Skip to content
Snippets Groups Projects
Commit 1fd9cb56 authored by Tobias Gröger's avatar Tobias Gröger
Browse files

Fix proxemic zone visualization, draw them in different colors

Use the proper pose to draw the proxemic zone ellipse.
Use different colors for the intimate and personal zones.
parent a59775fe
No related branches found
No related tags found
2 merge requests!51Fix visualization, fix human obstacles,!28Draft: Dev -> Main
......@@ -58,6 +58,7 @@ namespace armarx::navigation::local_planning
{
auto proxemicZones = proxemics.createProxemicZones(human);
int i = 0;
for (const auto& proxemicZone : proxemicZones)
{
auto pose = conv::toRos(proxemicZone.pose);
......@@ -85,13 +86,14 @@ namespace armarx::navigation::local_planning
if (visLayer != nullptr)
{
const Eigen::Vector3f axisLength(proxemicZone.shape.a, proxemicZone.shape.b, 0);
const core::Pose pose3d = conv::to3D(human.pose);
const core::Pose pose3d = conv::to3D(proxemicZone.pose);
visLayer->add(viz::Ellipsoid("proxemicZone_" + std::to_string(visualizationIndex++))
.pose(pose3d)
.axisLengths(axisLength)
.color(simox::Color::blue()));
.color(PROXEMIC_ZONE_COLOR[i % PROXEMIC_ZONE_COLOR.size()]));
}
i++;
}
}
......
......@@ -50,6 +50,9 @@ namespace armarx::navigation::local_planning
teb_local_planner::ObstContainer& container;
human::ProxemicZoneCreator proxemics;
int visualizationIndex;
const std::array<simox::Color, 2> PROXEMIC_ZONE_COLOR = {simox::Color::red(),
simox::Color::blue()};
};
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment