SegmentAdapter.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::armem_objects::SegmentAdapter
 * @author     Fabian Reister ( fabian dot reister at kit dot edu )
 * @date       2024
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "SegmentAdapter.h"
 
#include <iterator>
#include <memory>
#include <optional>
 
#include <SimoxUtility/algorithm/get_map_keys_values.h>
#include <VirtualRobot/Robot.h>
 
#include "ArmarXCore/core/time/Clock.h"
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/ice_conversions/ice_conversions_templates.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/CycleUtil.h>
#include <ArmarXCore/core/time/DateTime.h>
#include <ArmarXCore/core/time/Duration.h>
#include <ArmarXCore/core/time/Frequency.h>
#include <ArmarXCore/core/time/Metronome.h>
#include <ArmarXCore/core/time/ice_conversions.h>
#include <ArmarXCore/observers/variant/Variant.h>
 
#include "RobotAPI/libraries/core/FramedPose.h"
#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
#include <RobotAPI/libraries/ArmarXObjects/predictions.h>
#include <RobotAPI/libraries/armem_objects/aron/FamiliarObjectInstance.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
#include <RobotAPI/libraries/aron/core/aron_conversions.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
 
namespace armarx::armem::server::obj::familiar_object_instance
{
 
    SegmentAdapter::SegmentAdapter(MemoryToIceAdapter& iceMemory) : segment(iceMemory)
    {
    }
 
    std::string
    SegmentAdapter::getName() const
    {
        return Logging::tag.tagName;
    }
 
    void
    SegmentAdapter::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
    {
        segment.defineProperties(defs, prefix);
        visu.defineProperties(defs, prefix + "visu.");
    }
 
    void
    SegmentAdapter::init()
    {
        segment.setTag(getName());
        segment.decay.setTag(getName());
        // segment.setPredictionEngines(predictionEngines);
        visu.setTag(getName());
 
        segment.init();
    }
 
    void
    SegmentAdapter::connect(
        std::experimental::observer_ptr<robot_state::VirtualRobotReader> virtualRobotReader,
        viz::Client arviz,
        DebugObserverInterfacePrx debugObserver)
    {
        this->debugObserver = debugObserver;
        this->arviz = arviz;
 
        ARMARX_CHECK_NOT_NULL(virtualRobotReader);
        segment.robots.reader = virtualRobotReader;
 
        visu.arviz = arviz;
 
        if (!visu.updateTask)
        {
            ARMARX_IMPORTANT << "Enabled visu";
            visu.updateTask = new SimpleRunningTask<>([this]() { this->visualizeRun(); });
            visu.updateTask->start();
        }
 
        segment.connect(arviz);
    }
 
    void
    SegmentAdapter::reportFamiliarObjectPoses(
        const std::string& providerName,
        const ::armarx::objpose::ProvidedFamiliarObjectPoseSeq& data,
        const Ice::Current&)
    {
 
        if (data.empty())
        {
            return;
        }
 
        // Note, that in general, timestamps in `data` can differ. Therefore, we maintain the sync timestamp
        // and check whether we have to update the robot. Otherwise, we can avoid these Ice calls.
        std::optional<armarx::DateTime> syncTimestamp;
 
 
        const auto convertAndFillFamiliarObjectInstances =
            [this, providerName, &syncTimestamp](const ::armarx::aron::data::dto::DictPtr& data)
            -> armarx::armem::arondto::FamiliarObjectInstance
        {
            auto famObjInstance = armarx::armem::arondto::FamiliarObjectInstance::FromAron(data);
 
 
            armarx::FramedPose framedPose(famObjInstance.poseSensFrame.pose,
                                          famObjInstance.poseSensFrame.header.frame,
                                          famObjInstance.poseSensFrame.header.agent);
 
            ARMARX_CHECK_NOT_EMPTY(famObjInstance.poseSensFrame.header.agent);
            ARMARX_CHECK_NOT_EMPTY(famObjInstance.poseSensFrame.header.frame);
 
            VirtualRobot::RobotPtr robot =
                segment.robots.get(famObjInstance.poseSensFrame.header.agent, providerName);
            ARMARX_CHECK_NOT_NULL(robot) << famObjInstance.poseSensFrame.header.agent;
 
 
            if (not syncTimestamp.has_value() or syncTimestamp.value() != famObjInstance.timestamp)
            {
                ARMARX_CHECK(
                    segment.robots.reader->synchronizeRobot(*robot, famObjInstance.timestamp));
                syncTimestamp = famObjInstance.timestamp;
            }
 
 
            // fill global pose info if not set yet
            if (not famObjInstance.poseGlobal.has_value())
            {
                famObjInstance.poseGlobal.emplace();
                famObjInstance.poseGlobal->pose = framedPose.toGlobalEigen(robot);
                famObjInstance.poseGlobal->header.agent = famObjInstance.poseSensFrame.header.agent;
                famObjInstance.poseGlobal->header.frame = armarx::GlobalFrame;
            }
            else
            {
                ARMARX_CHECK_EQUAL(famObjInstance.poseGlobal->header.frame, armarx::GlobalFrame);
            }
 
 
            return famObjInstance;
        };
 
        std::vector<armarx::armem::arondto::FamiliarObjectInstance> familiarObjectInstances;
        familiarObjectInstances.reserve(data.size());
 
        std::transform(std::begin(data),
                       std::end(data),
                       std::back_inserter(familiarObjectInstances),
                       convertAndFillFamiliarObjectInstances);
 
        segment.commit(familiarObjectInstances, providerName);
    }
 
    void
    SegmentAdapter::handleProviderUpdate(const std::string& providerName)
    {
        // Initialized to 0 on first access.
        if (segment.providers.count(providerName) == 0)
        {
            segment.providers[providerName] = objpose::ProviderInfo();
        }
    }
 
    void
    SegmentAdapter::visualizeRun()
    {
        if (not visu.enabled)
        {
            ARMARX_INFO << "Visu disabled";
            return;
        }
 
        armarx::Metronome metronome(armarx::Frequency::HertzDouble(visu.frequencyHz));
 
        while (visu.updateTask && !visu.updateTask->isStopped())
        {
            {
                std::scoped_lock lock(visuMutex);
 
                const std::map<std::string,
                               std::vector<armarx::armem::arondto::FamiliarObjectInstance>>
                    familiarObjectsByProvider = segment.doLocked(
                        [this]()
                        { return segment.getFamiliarObjectsByProvider(armarx::Clock::Now()); });
 
                ARMARX_VERBOSE << "Visualizing " << familiarObjectsByProvider.size()
                               << " providers.";
                visu.visualizeFamiliarObjectsByProvider(familiarObjectsByProvider);
            }
 
            metronome.waitForNextTick();
        }
    }
 
 
} // namespace armarx::armem::server::obj::familiar_object_instance