ObjectPoseProviderExample.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::ObjectPoseProviderExample
 * @author     Rainer Kartmann ( rainer dot kartmann at kit dot edu )
 * @date       2020
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "ObjectPoseProviderExample.h"
 
#include <SimoxUtility/algorithm/string/string_tools.h>
#include <SimoxUtility/math/pose/pose.h>
 
#include <ArmarXCore/core/time/CycleUtil.h>
 
#include <RobotAPI/libraries/core/Pose.h>
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
#include <RobotAPI/libraries/ArmarXObjects/ProvidedObjectPose.h>
 
 
namespace armarx
{
 
    armarx::PropertyDefinitionsPtr ObjectPoseProviderExample::createPropertyDefinitions()
    {
        armarx::PropertyDefinitionsPtr defs = new ComponentPropertyDefinitions(getConfigIdentifier());
 
        defs->topic(debugObserver);
 
        defs->optional(initialObjectIDs, "Objects", "Object IDs of objects to be tracked.")
        .map(simox::alg::join(initialObjectIDs, ", "), initialObjectIDs);
 
        defs->optional(singleShot, "SingleShot", "If true, publishes only one snapshot.");
 
        return defs;
    }
 
    std::string ObjectPoseProviderExample::getDefaultName() const
    {
        return "ObjectPoseProviderExample";
    }
 
    void ObjectPoseProviderExample::onInitComponent()
    {
        for (const auto& initial : initialObjectIDs)
        {
            // -1 -> infinitely
            requestedObjects.requestObjects({ObjectID(initial)}, -1);
        }
 
        {
            providerInfo.objectType = objpose::ObjectType::KnownObject;
            std::vector<ObjectInfo> objects = objectFinder.findAllObjectsOfDataset("KIT");
            for (const auto& obj : objects)
            {
                providerInfo.supportedObjects.push_back(armarx::toIce(obj.id()));
            }
        }
    }
 
    void ObjectPoseProviderExample::onConnectComponent()
    {
        poseEstimationTask = new SimpleRunningTask<>([this]()
        {
            this->poseEstimationTaskRun();
        });
        poseEstimationTask->start();
    }
 
    void ObjectPoseProviderExample::onDisconnectComponent()
    {
    }
 
    void ObjectPoseProviderExample::onExitComponent()
    {
    }
 
    objpose::ProviderInfo ObjectPoseProviderExample::getProviderInfo(const Ice::Current&)
    {
        return providerInfo;
    }
 
    objpose::provider::RequestObjectsOutput ObjectPoseProviderExample::requestObjects(
        const objpose::provider::RequestObjectsInput& input, const Ice::Current&)
    {
        ARMARX_INFO << "Requested object IDs for " << input.relativeTimeoutMS << " ms: "
                    << input.objectIDs;
        {
            std::scoped_lock lock(requestedObjectsMutex);
            requestedObjects.requestObjects(input.objectIDs, input.relativeTimeoutMS);
        }
 
        objpose::provider::RequestObjectsOutput output;
        // All requests are successful.
        for (const auto& id : input.objectIDs)
        {
            output.results[id].success = true;
        }
        return output;
    }
 
    void ObjectPoseProviderExample::poseEstimationTaskRun()
    {
        CycleUtil cycle(50);
        IceUtil::Time start = TimeUtil::GetTime();
 
        std::map<ObjectID, ObjectInfo> objectInfos;
 
        while (poseEstimationTask and not poseEstimationTask->isStopped())
        {
            IceUtil::Time now = TimeUtil::GetTime();
            float t = float((now - start).toSecondsDouble());
 
            objpose::RequestedObjects::Update update;
            {
                std::scoped_lock lock(requestedObjectsMutex);
                update = requestedObjects.updateRequestedObjects(now);
            }
 
            if (update.added.size() > 0 || update.removed.size() > 0)
            {
                ARMARX_INFO << "Added: " << update.added
                            << "Removed: " << update.removed;
            }
 
            int i = 0;
            armarx::objpose::ProvidedObjectPoseSeq poses;
            for (const ObjectID& id : update.current)
            {
                if (objectInfos.count(id) == 0)
                {
                    if (std::optional<ObjectInfo> info = objectFinder.findObject(id))
                    {
                        objectInfos.emplace(id, *info);
                    }
                    else
                    {
                        ARMARX_WARNING << "Could not find object class '" << id << "'.";
                    }
                }
                const ObjectInfo& info = objectInfos.at(id);
 
                armarx::objpose::ProvidedObjectPose& pose = poses.emplace_back();
                pose.providerName = getName();
                pose.objectType = objpose::ObjectType::KnownObject;
 
                pose.objectID = info.id();
 
                Eigen::Vector3f pos = 200 * Eigen::Vector3f(std::sin(t - i), std::cos(t - i), 1 + i);
                Eigen::AngleAxisf ori((t - i) / M_PI_2, Eigen::Vector3f::UnitZ());
                pose.objectPose = simox::math::pose(pos, ori);
                // pose.objectPoseFrame = armarx::GlobalFrame;
                pose.objectPoseFrame = "DepthCamera";
 
                pose.objectPoseGaussian = objpose::PoseManifoldGaussian();
                pose.objectPoseGaussian->mean = pose.objectPose;
                pose.objectPoseGaussian->covariance = Eigen::Matrix6f::Identity();
                // Translational (co)variance.
                const float posVar = 10 + 10 * std::sin(t - i);
                pose.objectPoseGaussian->covariance.diagonal()(0) = 1.0 * posVar;
                pose.objectPoseGaussian->covariance.diagonal()(1) = 3.0 * posVar;
                pose.objectPoseGaussian->covariance.diagonal()(2) = 2.0 * posVar;
                if (i % 2 == 1)
                {
                    Eigen::Matrix3f rot = Eigen::AngleAxisf(0.25 * (t - i), Eigen::Vector3f::UnitZ()).toRotationMatrix();
                    pose.objectPoseGaussian->positionCovariance()
                            = rot * pose.objectPoseGaussian->positionCovariance() * rot.transpose();
                }
 
                // Rotational (co)variance.
                const float oriVar = (M_PI_4) + (M_PI_4) * std::sin(t - i);
                pose.objectPoseGaussian->covariance.diagonal()(3) = 1.0 * oriVar;
                pose.objectPoseGaussian->covariance.diagonal()(4) = 4.0 * oriVar;
                pose.objectPoseGaussian->covariance.diagonal()(5) = 2.0 * oriVar;
                if (i % 2 == 1)
                {
                    Eigen::Matrix3f rot = Eigen::AngleAxisf(0.25 * (t - i), Eigen::Vector3f::UnitZ()).toRotationMatrix();
                    pose.objectPoseGaussian->orientationCovariance()
                            = rot * pose.objectPoseGaussian->orientationCovariance() * rot.transpose();
                }
 
                pose.confidence = 0.75 + 0.25 * std::sin(t - i);
                pose.timestamp = DateTime::Now();
 
                i++;
            }
 
            ARMARX_VERBOSE << "Reporting " << poses.size() << " object poses";
            objectPoseTopic->reportObjectPoses(getName(), objpose::toIce(poses));
 
            if (singleShot)
            {
                break;
            }
 
            cycle.waitForCycleDuration();
        }
    }
}