25 #include <Eigen/Eigen>
27 #include <SimoxUtility/color/cmaps.h>
48 defs->defineOptionalProperty<std::string>(
49 "ArVizTopicName",
"ArVizTopic",
"Layer updates are sent over this topic.");
53 properties.manyLayers,
54 "layers.ManyElements",
55 "Show a layer with a lot of elements (used for testing, may impact performance).");
63 return "ArVizExample";
87 const bool join =
true;
101 Eigen::Vector3f pos = Eigen::Vector3f::Zero();
102 pos.x() = 100.0f * std::sin(timeInSeconds);
107 .
size(Eigen::Vector3f(100.0f, 100.0f, 100.0f));
109 bool toggleVisibility = (
static_cast<int>(timeInSeconds) % 2 == 0);
115 const float delta = 20. * std::sin(timeInSeconds);
117 .
position(Eigen::Vector3f{0, 100, 150})
119 .axisLengths(Eigen::Vector3f{70.f + delta, 70.f - delta, 30.f}));
122 Eigen::Vector3f pos = Eigen::Vector3f::Zero();
123 pos.y() = 100.0f * std::sin(timeInSeconds);
136 Eigen::Vector3f pos = Eigen::Vector3f::Zero();
137 pos.z() = 100.0f * std::sin(timeInSeconds);
145 Eigen::Vector3f pos = Eigen::Vector3f::Zero();
159 float modTime = std::fmod(timeInSeconds, 2.0 *
M_PI);
160 arrow.
length(200.0f + 100.0f * std::sin(modTime));
162 Eigen::AngleAxisf dirRot(modTime, Eigen::Vector3f::UnitZ());
163 Eigen::Vector3f direction = dirRot * Eigen::Vector3f::UnitX();
166 Eigen::Vector3f pos = Eigen::Vector3f::Zero();
179 Eigen::Vector3f offset = {-800, 0, 500};
180 std::vector<Eigen::Vector3f> pathPoints{
185 Eigen::Vector3f additionalPoint = {-200, 200, 200};
192 .color(simox::Color::magenta())
200 pathPoints.push_back(additionalPoint);
204 offset = offset - 300 * Eigen::Vector3f::UnitX();
206 for (
size_t i = 0; i < pathPoints.size() - 1; i += 2)
210 .fromTo(pathPoints.at(i) + offset, pathPoints.at(i + 1) + offset)
211 .color(simox::Color::lime())
220 offset = offset - 300 * Eigen::Vector3f::UnitX();
222 for (
size_t i = 0; i < pathPoints.size() - 1; ++i)
226 .fromTo(pathPoints.at(i) + offset, pathPoints.at(i + 1) + offset)
227 .
color(simox::Color::cyan())
234 offset = offset - 300 * Eigen::Vector3f::UnitX();
236 for (
size_t i = 0; i < pathPoints.size(); ++i)
241 if (i < pathPoints.size() - 1)
245 .fromTo(pathPoints.at(i) + offset, pathPoints.at(i + 1) + offset)
251 .position(pathPoints.at(i) + offset)
261 Eigen::Vector3f pos = Eigen::Vector3f::Zero();
263 for (
int i = 0; i < 10; ++i)
266 int randomIndex = std::rand();
270 pos.y() = i * 200.0f;
274 .file(
"armar6_rt",
"armar6_rt/robotmodel/Armar6-SH/Armar6-RightHand-v3.xml")
284 for (
int i = 0; i < 6; ++i)
286 Eigen::Vector3f pos = Eigen::Vector3f(-200.0, 200.0, 300);
289 Eigen::Vector3f normal = Eigen::Vector3f::Zero();
290 normal(i / 2) = (i % 2 == 0 ? 1.0 : -1.0);
299 float modTime = std::fmod(timeInSeconds, 2.0 *
M_PI);
305 Eigen::Vector3f pos = Eigen::Vector3f::Zero();
314 float t = 1.0f + std::sin(timeInSeconds);
315 float offset = 50.0f * t;
316 poly.
addPoint(Eigen::Vector3f{-200.0f - offset, -200.0f - offset, 0.0f});
317 poly.
addPoint(Eigen::Vector3f{-200.0f, +200.0f, 0.0f});
318 poly.
addPoint(Eigen::Vector3f{+200.0f + offset, +200.0f + offset, 0.0f});
319 poly.
addPoint(Eigen::Vector3f{+200.0f, -200.0f, 0.0f});
324 Eigen::Vector3f pos = Eigen::Vector3f::Zero();
332 float t = 1.0f + std::sin(timeInSeconds);
333 float offset = 20.0f * t;
334 poly.
addPoint(Eigen::Vector3f{-100.0f - offset, -100.0f - offset, 0.0f});
335 poly.
addPoint(Eigen::Vector3f{-100.0f, +100.0f, 0.0f});
336 poly.
addPoint(Eigen::Vector3f{+100.0f + offset, +100.0f + offset, 0.0f});
337 poly.
addPoint(Eigen::Vector3f{+100.0f, -100.0f, 0.0f});
342 armarx::Vector3f vertices[] = {
343 {-100.0f, -100.0f, 0.0f},
344 {-100.0f, +100.0f, 0.0f},
345 {+100.0f, +100.0f, 0.0f},
346 {+100.0f, +100.0f, 200.0f},
348 std::size_t verticesSize =
sizeof(vertices) /
sizeof(vertices[0]);
355 std::size_t colorsSize =
sizeof(colors) /
sizeof(colors[0]);
357 viz::data::Face faces[] = {
375 std::size_t facesSize =
sizeof(faces) /
sizeof(faces[0]);
377 Eigen::Vector3f pos = Eigen::Vector3f::Zero();
384 .
colors(colors, colorsSize)
385 .
faces(faces, facesSize);
390 Eigen::Vector3f pos = Eigen::Vector3f::Zero();
394 "armar6_rt",
"armar6_rt/robotmodel/Armar6-SH/Armar6-SH.xml");
406 float value = 0.5f * (1.0f + std::sin(timeInSeconds));
418 .
position(Eigen::Vector3f(2000.0f, 0.0f, 0.0f))
419 .
size(Eigen::Vector3f(200.0f, 200.0f, 200.0f))
429 .position(Eigen::Vector3f(2000.0f, 0.0f, 400.0f))
435 for (
int x = -200;
x <= 200; ++
x)
438 double phase = timeInSeconds +
x / 50.0;
439 double heightT =
std::max(0.0,
std::min(0.5 * (1.0 + std::sin(phase)), 1.0));
440 for (
int y = -200; y <= 200; ++y)
443 p.z = 100.0 * heightT;
445 p.color.g = 255.0 * heightT;
446 p.color.b = 255.0 * (1.0 - heightT);
447 pc.addPointUnchecked(p);
458 Eigen::Vector3f pos = Eigen::Vector3f::Zero();
459 pos.x() = 100.0f * std::sin(timeInSeconds);
464 .
file(
"PriorKnowledgeData",
465 "PriorKnowledgeData/objects/Maintenance/"
466 "bauhaus-screwbox-large/bauhaus-screwbox-large.xml");
472 Eigen::Vector3f pos = Eigen::Vector3f::Zero();
473 pos.x() = 300.0f + 100.0f * std::sin(timeInSeconds);
476 Eigen::AngleAxisf orientation(M_PI_2, Eigen::Vector3f::UnitX());
482 .
file(
"PriorKnowledgeData",
483 "PriorKnowledgeData/objects/Maintenance/cable-ties/cable-ties.wrl");
485 layer.
add(spraybottle);
486 layer.
add(
viz::Pose(
"spraybottle pose").pose(pos, orientation.toRotationMatrix()));
493 int time = int(timeInSeconds);
495 const Eigen::Vector3f at = {-400, 0, 100};
496 const Eigen::Vector3f dir = {-150, 0, 0};
498 layer.
add(
viz::Box(
"box_always").position(at).size(100).color(simox::Color::azure()));
500 .position(at + Eigen::Vector3f(0, 0, 100))
501 .orientation(Eigen::AngleAxisf(
float(
M_PI), Eigen::Vector3f::UnitZ()) *
502 Eigen::Quaternionf::FromTwoVectors(Eigen::Vector3f::UnitZ(),
503 -Eigen::Vector3f::UnitY()))
506 .color(simox::Color::black()));
512 .position(at + 1.0 * dir)
514 .color(simox::Color::purple()));
518 .position(at + 2.0 * dir)
520 .color(simox::Color::pink()));
524 .position(at + 3.0 * dir)
527 .color(simox::Color::turquoise()));
535 const Eigen::Vector3f at = {-800, 0, 500};
536 const float size = 5;
537 const float dist = 10;
539 const double period = 10;
540 const float angle =
float(2 *
M_PI * std::fmod(timeInSeconds, period) / period);
543 float cf = 1.f / num;
544 for (
int x = 0;
x < num; ++
x)
546 for (
int y = 0; y < num; ++y)
548 for (
int z = 0; z < num; ++z)
550 std::stringstream ss;
551 ss <<
"box_" <<
x <<
"_" << y <<
"_" << z;
553 .
position(at + dist * Eigen::Vector3f(
x, y, z))
569 const E::Vector3f at(-500, -500, 1500);
571 E::Quaternionf::FromTwoVectors(E::Vector3f::UnitZ(), -E::Vector3f::UnitY());
572 const E::Vector2f size(200, 20);
573 const E::Vector2i num(64, 2);
578 std::string
const& name = pair.first;
579 const simox::color::ColorMap& cmap = pair.second;
581 Eigen::Vector3f shift(0, 0, -1.5f *
index * size.y());
587 [&cmap](
size_t,
size_t,
const E::Vector3f& p)
588 {
return viz::Color(cmap((p.x() + 100.f) / 200.f)); });
595 .
position(at + shift + E::Vector3f(2 + size.x() / 2, -2, 2 - size.y() / 2))
598 .
color(simox::Color::black()));
609 .
position(Eigen::Vector3f(2000.0f, 0.0f, 2000.0f))
610 .
size(Eigen::Vector3f(200.0f, 200.0f, 200.0f))
616 .contextMenu({
"First Option",
"Second Option",
"Third Option"})
618 .translation(viz::AXES_XY)
619 .scaling(viz::AXES_XYZ));
624 .
position(Eigen::Vector3f(1000.0f, 0.0f, 2000.0f))
632 .contextMenu({
"Cyl Option 1",
"Cyl Option 2"})
634 .translation(viz::AXES_YZ)
673 stage.
add(permanentLayer);
675 bool manyElements = getProperty<bool>(
"layers.ManyElements");
680 stage.
add(manyElementsLayer);
685 stage.
add(colorMapsLayer);
689 stage.
add(interactionLayer);
697 while (!task->isStopped())
703 exampleLayer.
clear();
705 pathsAndLinesLayer.
clear();
709 objectsLayer.
clear();
711 disAppearingLayer.
clear();
716 robotHandsLayer.
clear();
724 stage.
add({testLayer,
739 if (interactions.
size() > 0)
748 <<
"] Chosen context menu: " <<
interaction.chosenContextMenuEntry();
752 std::string transformState;
755 transformState =
"Begin";
759 transformState =
"During";
763 transformState =
"End";
767 transformState =
"<Unknwon>";
770 <<
"] Transformation " << transformState <<
": \n"
772 <<
"\n scale: " <<
interaction.scale().transpose();
782 c.waitForCycleDuration();