ObjectPoseClientExample.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @package    RobotAPI::ArmarXObjects::ObjectPoseClientExample
 * @author     Rainer Kartmann ( rainer dot kartmann at kit dot edu )
 * @date       2021
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "ObjectPoseClientExample.h"
 
#include <SimoxUtility/math/pose.h>
 
#include <ArmarXCore/core/ice_conversions/ice_conversions_templates.h>
#include <ArmarXCore/core/time/CycleUtil.h>
#include <ArmarXCore/core/time/ice_conversions.h>
 
#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
 
 
namespace armarx
{
 
    armarx::PropertyDefinitionsPtr ObjectPoseClientExample::createPropertyDefinitions()
    {
        armarx::PropertyDefinitionsPtr defs = new ComponentPropertyDefinitions(getConfigIdentifier());
 
        defs->optional(p.predictionsPerObject, "predictions.NumberPerObject",
                       "How many predictions with increasing time offsets to make per object.");
 
        defs->optional(p.millisecondPredictionIncrement, "predictions.TimeIncrement",
                       "The size of the prediction time offset increment in milliseconds.");
 
        return defs;
    }
 
    std::string ObjectPoseClientExample::getDefaultName() const
    {
        return "ObjectPoseClientExample";
    }
 
    void ObjectPoseClientExample::onInitComponent()
    {
    }
 
    void ObjectPoseClientExample::onConnectComponent()
    {
        setDebugObserverBatchModeEnabled(true);
 
        objectProcessingTask = new SimpleRunningTask<>([this]()
        {
            this->objectProcessingTaskRun();
        });
        objectProcessingTask->start();
    }
 
    void ObjectPoseClientExample::onDisconnectComponent()
    {
    }
 
    void ObjectPoseClientExample::onExitComponent()
    {
    }
 
 
    void ObjectPoseClientExample::objectProcessingTaskRun()
    {
        CycleUtil cycle(50);
 
        while (objectProcessingTask && !objectProcessingTask->isStopped())
        {
            // This client can be copied to other classes to give them access to the object pose storage.
            objpose::ObjectPoseClient client = getClient();
            const objpose::ObjectPoseSeq objectPoses = client.fetchObjectPoses();
 
            ARMARX_VERBOSE << "Received poses of " << objectPoses.size() << " objects.";
 
            {
                setDebugObserverDatafield("NumObjectPoses", objectPoses.size());
                sendDebugObserverBatch();
            }
 
            viz::StagedCommit stage = arviz.stage();
            {
                // Visualize the objects.
                viz::Layer layer = arviz.layer("Objects");
                for (const objpose::ObjectPose& objectPose : objectPoses)
                {
                    layer.add(viz::Object(objectPose.objectID.str())
                              .pose(objectPose.objectPoseGlobal)
                              .fileByObjectFinder(objectPose.objectID)
                              .alpha(objectPose.confidence));
                }
                stage.add(layer);
            }
            stage.add(visualizePredictions(client, objectPoses));
            arviz.commit(stage);
 
            cycle.waitForCycleDuration();
        }
    }
 
 
    viz::Layer
    ObjectPoseClientExample::visualizePredictions(const objpose::ObjectPoseClient& client,
                                                  const objpose::ObjectPoseSeq& objectPoses)
    {
        viz::Layer layer = arviz.layer("PredictionArray");
 
        objpose::ObjectPosePredictionRequestSeq requests;
        for (const objpose::ObjectPose& objectPose : objectPoses)
        {
            for (int i = 0; i < p.predictionsPerObject; ++i)
            {
                objpose::ObjectPosePredictionRequest request;
                toIce(request.objectID, objectPose.objectID);
                request.settings.predictionEngineID = "Linear Position Regression";
                toIce(request.timestamp,
                      DateTime::Now() +
                          Duration::MilliSeconds((i + 1) * p.millisecondPredictionIncrement));
                toIce(request.timeWindow, Duration::Seconds(10));
                requests.push_back(request);
            }
        }
 
        objpose::ObjectPosePredictionResultSeq results;
        try
        {
            results = client.objectPoseStorage->predictObjectPoses(requests);
        }
        catch (const Ice::LocalException& e)
        {
            ARMARX_INFO << "Failed to get predictions for object poses: " << e.what();
        }
 
        for (size_t i = 0; i < results.size(); ++i)
        {
            const objpose::ObjectPosePredictionResult& result = results.at(i);
            if (result.success)
            {
                auto predictedPose = armarx::fromIce<objpose::ObjectPose>(result.prediction);
                Eigen::Vector3f predictedPosition =
                    simox::math::position(predictedPose.objectPoseGlobal);
                int alpha =
                    (p.predictionsPerObject - (static_cast<int>(i) % p.predictionsPerObject)) *
                    255 / p.predictionsPerObject; // NOLINT
                layer.add(
                    viz::Arrow(predictedPose.objectID.str() + " Linear Prediction " +
                               std::to_string(i % p.predictionsPerObject))
                        .fromTo(simox::math::position(
                                    objectPoses.at(i / p.predictionsPerObject).objectPoseGlobal),
                                predictedPosition)
                        .color(viz::Color::green(255, alpha))); // NOLINT
            }
            else
            {
                ARMARX_INFO << "Prediction for object '" << result.prediction.objectID
                            << "' failed: " << result.errorMessage;
            }
        }
 
        return layer;
    }
} // namespace armarx