ArticulatedObjectVisu.cpp

Go to the documentation of this file.

#include "ArticulatedObjectVisu.h"
 
#include <algorithm>
 
#include <SimoxUtility/math/pose.h>
 
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/CycleUtil.h>
#include <ArmarXCore/core/time/TimeUtil.h>
 
#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
 
#include "Segment.h"
 
namespace armarx::armem::server::obj::instance
{
 
    void ArticulatedObjectVisu::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 ArticulatedObjectVisu::visualizeProvider(
        const std::string& providerName,
        const armarx::armem::articulated_object::ArticulatedObjects& objects) const
    {
        viz::Layer layer = arviz.layer(providerName);
 
        visualizeObjects(layer, objects);
 
        return layer;
    }
 
    void ArticulatedObjectVisu::visualizeObjects(
        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(xmlPath.path, xmlPath.path)
                         .joints(obj.config.jointMap)
                         .pose(obj.config.globalPose);
            // clang-format on
 
            robot.useFullModel();
 
            layer.add(robot);
        };
 
        std::for_each(objects.begin(), objects.end(), visualizeObject);
    }
 
    void ArticulatedObjectVisu::init()
    {
        updateTask = new PeriodicTask<ArticulatedObjectVisu>(this, &ArticulatedObjectVisu::visualizeRun, 1000 / p.frequencyHz);
 
        ARMARX_INFO << "ArticulatedObjectVisu: init";
        updateTask->start();
    }
 
    void ArticulatedObjectVisu::visualizeRun()
    {
        // std::scoped_lock lock(visuMutex);
        ARMARX_INFO << "Update task";
 
        // if (not p.enabled)
        // {
        //     return;
        // }
 
        // 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()) },
        //     });
        // }
    }
 
    // 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();
    // }
 
} // namespace armarx::armem::server::obj::instance