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

Visualize proxemics zone in nav mem visu

parent 07361bfa
No related branches found
No related tags found
1 merge request!109Social layers
Hide whitespace changes
Inline Side-by-side
source/armarx/navigation/components/navigation_memory/Visu.cpp
+ 89
− 77
View file @ 3ca26629
...@@ -15,6 +15,7 @@ ...@@ -15,6 +15,7 @@
* *
* @package Navigation::ArmarXObjects::NavigationMemory * @package Navigation::ArmarXObjects::NavigationMemory
* @author Rainer Kartmann ( rainer dot kartmann at kit dot edu ) * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
* @author Timo Weberruß ( timo dot weberruss at student dot kit dot edu )
* @date 2021 * @date 2021
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
* GNU General Public License * GNU General Public License
...@@ -139,113 +140,124 @@ namespace armarx::navigation::memory ...@@ -139,113 +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 const auto asColor = [&cmap, &vmax](const float distance) -> viz::data::Color
visualize(const algorithms::Costmap& costmap, viz::Layer& layer, const std::string& name)
{ {
const auto cmap = simox::color::cmaps::viridis(); const auto color = cmap.at(distance, 0.F, vmax);
const float vmax = costmap.getGrid().array().maxCoeff(); return {color.a, color.r, color.g, color.b};
};
const auto asColor = [&cmap, &vmax](const float distance) -> viz::data::Color const std::int64_t cols = costmap.getGrid().cols();
{ const std::int64_t rows = costmap.getGrid().rows();
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(); auto mesh = viz::Mesh(name);
const std::int64_t rows = costmap.getGrid().rows();
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; for (int r = 0; r < rows; r++)
std::vector<std::vector<viz::data::Color>> colors; {
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); verticesRow.at(c) = conv::to3D(costmap.toPositionGlobal({r, c}));
auto& colorsRow = colors.emplace_back(cols); colorsRow.at(c) = asColor(costmap.getGrid()(r, c));
for (int c = 0; c < cols; 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 void
visualize(const human::Humans& humans, viz::Layer& layer, const bool visuTransparent) 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"; ARMARX_INFO << "Visualizing " << humans.size() << " humans";
for (const auto& human : humans) for (const auto& human : humans)
{ {
// viz::Cylinder cylinder(std::to_string(layer.size())); // viz::Cylinder cylinder(std::to_string(layer.size()));
// cylinder.fromTo(conv::to3D(human.pose.translation()), // cylinder.fromTo(conv::to3D(human.pose.translation()),
// conv::to3D(human.pose.translation()) + Eigen::Vector3f{0, 0, 10}); // conv::to3D(human.pose.translation()) + Eigen::Vector3f{0, 0, 10});
// cylinder.color(simox::Color::orange()); // cylinder.color(simox::Color::orange());
// cylinder.radius(300); // cylinder.radius(300);
// layer.add(cylinder); // layer.add(cylinder);
viz::Robot mmm(std::to_string(layer.size())); viz::Robot mmm(std::to_string(layer.size()));
mmm.file("RobotAPI", "RobotAPI/robots/MMM/mmm.xml"); mmm.file("RobotAPI", "RobotAPI/robots/MMM/mmm.xml");
mmm.pose(conv::to3D(human.pose) * human_T_mmm); 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, visuTransparent ? 100 : 255)); mmm.overrideColor(viz::Color::orange(255, visuTransparent ? 100 : 255));
layer.add(mmm); layer.add(mmm);
core::Pose pose3d = conv::to3D(human.pose); core::Pose pose3d = conv::to3D(human.pose);
pose3d.translation() += Eigen::Vector3f{0, 0, 1000}; 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);
{
auto arrow = viz::Arrow(std::to_string(layer.size())) auto arrow = viz::Arrow(std::to_string(layer.size()))
.pose(pose3d) .fromTo(pose3d.translation(),
.length(200) pose3d.translation() + conv::to3D(human.linearVelocity))
.color(simox::Color::red()); .color(simox::Color::blue());
arrow.width(10);
layer.add(arrow); layer.add(arrow);
}
{ auto proxemicZones = human.proxemicZones;
auto arrow =
viz::Arrow(std::to_string(layer.size())) int i = 0;
.fromTo(pose3d.translation(), for (const auto& proxemicZone : proxemicZones)
pose3d.translation() + conv::to3D(human.linearVelocity)) {
.color(simox::Color::blue()); const Eigen::Vector3f axisLength(
arrow.width(10); proxemicZone.shape.a, proxemicZone.shape.b, 10.f - i);
layer.add(arrow); const core::Pose pose3d = conv::to3D(proxemicZone.pose);
}
layer.add(viz::Ellipsoid("proxemicZone_" + std::to_string(visualizationIndex))
.pose(pose3d)
.axisLengths(axisLength)
.color(PROXEMIC_ZONE_COLOR[i % PROXEMIC_ZONE_COLOR.size()]));
i++;
} }
} }
}
void void
visualize(const human::HumanGroups& humanGroups, viz::Layer& layer) Visu::visualize(const human::HumanGroups& humanGroups, viz::Layer& layer)
{
for (const auto& humanGroup : humanGroups)
{ {
for (const auto& humanGroup : humanGroups) viz::Polygon polygon(std::to_string(layer.size()));
{
viz::Polygon polygon(std::to_string(layer.size()));
const std::vector<Eigen::Vector2f> verts2D = humanGroup.shape.vertices; const std::vector<Eigen::Vector2f> verts2D = humanGroup.shape.vertices;
std::vector<Eigen::Vector3f> verts3D; std::vector<Eigen::Vector3f> verts3D;
for (const auto& v2d : verts2D) for (const auto& v2d : verts2D)
{ {
verts3D.emplace_back(Eigen::Vector3f(v2d.x(), v2d.y(), 50)); 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);
} }
polygon.points(verts3D);
polygon.color(simox::Color::kit_yellow());
polygon.lineWidth(10);
layer.add(polygon);
} }
}
} // namespace
void void
Visu::drawCostmaps(std::vector<viz::Layer>& layers, bool enabled) Visu::drawCostmaps(std::vector<viz::Layer>& layers, bool enabled)
......
source/armarx/navigation/components/navigation_memory/Visu.h
+ 12
− 0
View file @ 3ca26629
...@@ -31,6 +31,7 @@ ...@@ -31,6 +31,7 @@
#include <RobotAPI/libraries/armem/core/forward_declarations.h> #include <RobotAPI/libraries/armem/core/forward_declarations.h>
#include "armarx/navigation/algorithms/Costmap.h" #include "armarx/navigation/algorithms/Costmap.h"
#include "armarx/navigation/human/types.h"
namespace armarx::navigation::graph namespace armarx::navigation::graph
...@@ -69,6 +70,17 @@ namespace armarx::navigation::memory ...@@ -69,6 +70,17 @@ namespace armarx::navigation::memory
const armem::server::wm::CoreSegment& humanGroupSegment; const armem::server::wm::CoreSegment& humanGroupSegment;
std::unique_ptr<navigation::graph::GraphVisu> visu; 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 visualize(const human::HumanGroups& humanGroups, viz::Layer& layer);
void
visualize(const algorithms::Costmap& costmap, viz::Layer& layer, const std::string& name);
}; };
......
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