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 ...@@ -58,6 +58,7 @@ namespace armarx::navigation::local_planning
{ {
auto proxemicZones = proxemics.createProxemicZones(human); auto proxemicZones = proxemics.createProxemicZones(human);
int i = 0;
for (const auto& proxemicZone : proxemicZones) for (const auto& proxemicZone : proxemicZones)
{ {
auto pose = conv::toRos(proxemicZone.pose); auto pose = conv::toRos(proxemicZone.pose);
...@@ -85,13 +86,14 @@ namespace armarx::navigation::local_planning ...@@ -85,13 +86,14 @@ namespace armarx::navigation::local_planning
if (visLayer != nullptr) if (visLayer != nullptr)
{ {
const Eigen::Vector3f axisLength(proxemicZone.shape.a, proxemicZone.shape.b, 0); 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++)) visLayer->add(viz::Ellipsoid("proxemicZone_" + std::to_string(visualizationIndex++))
.pose(pose3d) .pose(pose3d)
.axisLengths(axisLength) .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 ...@@ -50,6 +50,9 @@ namespace armarx::navigation::local_planning
teb_local_planner::ObstContainer& container; teb_local_planner::ObstContainer& container;
human::ProxemicZoneCreator proxemics; human::ProxemicZoneCreator proxemics;
int visualizationIndex; 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