Skip to content
Snippets Groups Projects

armem/dev => master

Merged armem/dev => master
armem/dev into master
Merged Fabian Reister requested to merge armem/dev into master
Viewing commit 3b04ab9e
Prev Next
Show latest version
2 files
+ 0
− 271
Compare changes
  • Side-by-side
  • Inline
Files
2
source/RobotAPI/libraries/armem_robot_state/server/localization/Visu.cpp deleted 100644 → 0
+ 0
− 170
#include "Visu.h"
#include "ArmarXCore/core/logging/Logging.h"
#include "ArmarXCore/core/time/CycleUtil.h"
#include <algorithm>
#include <SimoxUtility/math/pose.h>
#include <ArmarXCore/core/time/TimeUtil.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
#include "Segment.h"
namespace armarx::armem::server::obj::articulated_object_instance
{
void Visu::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
{
defs->optional(p.enabled, prefix + "enabled",
"Enable or disable visualization of objects.");
defs->optional(p.frequencyHz, prefix + "frequenzyHz",
"Frequency of visualization.");
}
viz::Layer Visu::visualizeProvider(
const std::string& providerName,
const armarx::armem::articulated_object::ArticulatedObjects& objects) const
{
viz::Layer layer = arviz.layer(providerName);
visualizeRobots(layer, objects);
return layer;
}
void Visu::visualizeR(viz::Layer& layer, const armarx::armem::articulated_object::ArticulatedObjects& objects) const
{
const auto visualizeObject = [&](const armarx::armem::articulated_object::ArticulatedObject & obj)
{
const auto xmlPath = obj.description.xml.serialize();
// clang-format off
auto robot = viz::Robot(obj.description.name)
// .file(xmlPath.package, xmlPath.path)
.file("ArmarXObjects", "./data/ArmarXObjects/Environment/mobile-kitchen/dishwasher-only/dishwasher.xml")
.joints(obj.config.jointMap)
.pose(obj.config.globalPose);
robot.useFullModel();
// clang-format on
layer.add(robot);
};
std::for_each(objects.begin(), objects.end(), visualizeObject);
}
void Visu::init()
{
updateTask = new SimpleRunningTask<>([this]()
{
this->visualizeRun();
});
updateTask->start();
}
// void Visu::RemoteGui::setup(const Visu& visu)
// {
// using namespace armarx::RemoteGui::Client;
// enabled.setValue(visu.enabled);
// inGlobalFrame.setValue(visu.inGlobalFrame);
// alpha.setRange(0, 1.0);
// alpha.setValue(visu.alpha);
// alphaByConfidence.setValue(visu.alphaByConfidence);
// oobbs.setValue(visu.oobbs);
// objectFrames.setValue(visu.objectFrames);
// {
// float max = 10000;
// objectFramesScale.setRange(0, max);
// objectFramesScale.setDecimals(2);
// objectFramesScale.setSteps(int(10 * max));
// objectFramesScale.setValue(visu.objectFramesScale);
// }
// GridLayout grid;
// int row = 0;
// grid.add(Label("Enabled"), {row, 0}).add(enabled, {row, 1});
// row++;
// grid.add(Label("Global Frame"), {row, 0}).add(inGlobalFrame, {row, 1});
// row++;
// grid.add(Label("Alpha"), {row, 0}).add(alpha, {row, 1}, {1, 3});
// row++;
// grid.add(Label("Alpha by Confidence"), {row, 0}).add(alphaByConfidence, {row, 1});
// row++;
// grid.add(Label("OOBB"), {row, 0}).add(oobbs, {row, 1});
// row++;
// grid.add(Label("Object Frames"), {row, 0}).add(objectFrames, {row, 1});
// grid.add(Label("Scale:"), {row, 2}).add(objectFramesScale, {row, 3});
// row++;
// group.setLabel("Visualization");
// group.addChild(grid);
// }
// void Visu::RemoteGui::update(Visu& visu)
// {
// visu.enabled = enabled.getValue();
// visu.inGlobalFrame = inGlobalFrame.getValue();
// visu.alpha = alpha.getValue();
// visu.alphaByConfidence = alphaByConfidence.getValue();
// visu.oobbs = oobbs.getValue();
// visu.objectFrames = objectFrames.getValue();
// visu.objectFramesScale = objectFramesScale.getValue();
// }
void Visu::visualizeRun()
{
CycleUtil cycle(static_cast<int>(1000 / p.frequencyHz));
while (updateTask && not updateTask->isStopped())
{
{
// std::scoped_lock lock(visuMutex);
ARMARX_IMPORTANT << "Update task";
if (p.enabled)
{
// TIMING_START(Visu);
const auto articulatedObjects = segment.getArticulatedObjects();
ARMARX_INFO << "Found " << articulatedObjects.size() << " articulated objects";
viz::Layer layer = arviz.layer("ArticulatedObjectInstances");
ARMARX_INFO << "visualizing objects";
visualizeObjects(layer, articulatedObjects);
ARMARX_INFO << "Committing objects";
arviz.commit({layer});
ARMARX_INFO << "Done committing";
// TIMING_END_STREAM(Visu, ARMARX_VERBOSE);
// if (debugObserver)
// {
// debugObserver->setDebugChannel(getName(),
// {
// { "t Visualize [ms]", new Variant(Visu.toMilliSecondsDouble()) },
// });
// }
}
}
cycle.waitForCycleDuration();
}
}
} // namespace armarx::armem::server::obj::articulated_object_instance