RobotStateWriter.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::RobotSensorMemory
 * @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 "RobotStateWriter.h"
 
// STL
#include <chrono>
 
// Simox
#include <SimoxUtility/math/convert/rpy_to_mat3f.h>
 
// ArmarX
#include "ArmarXCore/core/logging/Logging.h"
#include "ArmarXCore/core/time/Metronome.h"
#include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
 
#include "RobotAPI/libraries/armem_robot_state/client/common/constants.h"
#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/server/localization/Segment.h>
#include <RobotAPI/libraries/aron/core/data/variant/All.h>
#include <RobotAPI/libraries/core/FramedPose.h>
 
namespace armarx::armem::server::robot_state::proprioception
{
 
    void
    RobotStateWriter::connect(armarx::plugins::DebugObserverComponentPlugin& debugObserverPlugin)
    {
        // Thread-local copy of debug observer helper.
        this->debugObserver =
            DebugObserverHelper(Logging::tag.tagName, debugObserverPlugin.getDebugObserver(), true);
    }
 
    static float
    toDurationMs(std::chrono::high_resolution_clock::time_point start,
                 std::chrono::high_resolution_clock::time_point end)
    {
        auto duration = std::chrono::duration_cast<std::chrono::microseconds>(end - start);
        return duration.count() / 1000.f;
    }
 
    void
    RobotStateWriter::run(float pollFrequency,
                          Queue& dataBuffer,
                          MemoryToIceAdapter& memory,
                          localization::Segment& localizationSegment)
    {
        while (isRunning())
        {
            if (debugObserver)
            {
                // This locks the queue, but I did not find an interface to lock the queue,
                // get the size and wait_pull().
                size_t dataBufferSize = dataBuffer.size();
                debugObserver->setDebugObserverDatafield("RobotStateWriter | Queue Size",
                                                         static_cast<long>(dataBufferSize));
 
                if (static_cast<long>(dataBufferSize) > properties.queueSizeWarningThreshold)
                {
                    ARMARX_WARNING << deactivateSpam(300)
                                   << "RobotStateWriter queue size (" << dataBufferSize
                                   << ") exceeds threshold ("
                                   << properties.queueSizeWarningThreshold
                                   << "). The writer is not keeping up with incoming data.";
                }
            }
 
            RobotUnitData robotUnitData;
            if (const auto status = dataBuffer.wait_pull(robotUnitData);
                status == boost::queue_op_status::success)
            {
                auto start = std::chrono::high_resolution_clock::now();
                auto endBuildUpdate = start, endProprioception = start, endLocalization = start;
 
                Update update = buildUpdate(robotUnitData);
                endBuildUpdate = std::chrono::high_resolution_clock::now();
 
                // Commits lock the core segments.
 
                // Proprioception + Exteroception
                armem::CommitResult resultProprioception =
                    memory.commitLocking(update.proprioception);
 
                ARMARX_DEBUG << deactivateSpam(1) << VAROUT(update.exteroception.updates.size());
                armem::CommitResult resultExteroception =
                    memory.commitLocking(update.exteroception);
                endProprioception = std::chrono::high_resolution_clock::now();
 
                // Localization
                for (const armem::robot_state::localization::Transform& transform :
                     update.localization)
                {
                    localizationSegment.commitTransformLocking(transform);
                }
                endLocalization = std::chrono::high_resolution_clock::now();
 
                if (not resultProprioception.allSuccess())
                {
                    ARMARX_WARNING << "Could not commit data to proprioception segment in memory. "
                                      "Error message: "
                                   << resultProprioception.allErrorMessages();
                }
 
                if (not resultExteroception.allSuccess())
                {
                    ARMARX_WARNING << "Could not commit data to exteroception segment in memory. "
                                      "Error message: "
                                   << resultExteroception.allErrorMessages();
                }
 
                if (debugObserver)
                {
                    auto end = std::chrono::high_resolution_clock::now();
 
                    debugObserver->setDebugObserverDatafield("RobotStateWriter | t Commit (ms)",
                                                             toDurationMs(start, end));
                    debugObserver->setDebugObserverDatafield(
                        "RobotStateWriter | t Commit 1. Build Update (ms)",
                        toDurationMs(start, endBuildUpdate));
                    debugObserver->setDebugObserverDatafield(
                        "RobotStateWriter | t Commit 2. Proprioception (ms)",
                        toDurationMs(endBuildUpdate, endProprioception));
                    debugObserver->setDebugObserverDatafield(
                        "RobotStateWriter | t Commit 3. Localization (ms)",
                        toDurationMs(endProprioception, endLocalization));
                }
            }
            else
            {
                ARMARX_WARNING << "Queue pull was not successful. "
                               << static_cast<std::underlying_type<boost::queue_op_status>::type>(
                                      status);
            }
 
            if (debugObserver)
            {
                debugObserver->sendDebugObserverBatch();
            }
        }
    }
 
    bool
    RobotStateWriter::isRunning()
    {
        return task and not task->isStopped();
    }
 
    RobotStateWriter::Update
    RobotStateWriter::buildUpdate(const RobotUnitData& data)
    {
        // Send batch to memory
        Update update;
 
        if (data.proprioception)
        {
            armem::EntityUpdate& up = update.proprioception.add();
            up.entityID = properties.robotUnitProviderID.withEntityName(
                properties.robotUnitProviderID.providerSegmentName);
            up.entityID.coreSegmentName =
                ::armarx::armem::robot_state::constants::proprioceptionCoreSegment;
            up.referencedTime = data.timestamp;
            up.arrivedTime = data.timestampArrived;
            up.instancesData = {data.proprioception};
        }
 
        // Exteroception
        if (data.exteroception)
        {
            armem::EntityUpdate& up = update.exteroception.add();
            up.entityID = properties.robotUnitProviderID.withEntityName(
                properties.robotUnitProviderID.providerSegmentName);
            up.entityID.coreSegmentName =
                ::armarx::armem::robot_state::constants::exteroceptionCoreSegment;
            up.referencedTime = data.timestamp;
            up.instancesData = {data.exteroception};
        }
 
        // Extract odometry data.
        ARMARX_CHECK_NOT_NULL(data.proprioception);
        const std::string platformKey = "platform";
        if (data.proprioception->hasElement(platformKey))
        {
            ARMARX_DEBUG << "Found odometry data.";
            auto platformData =
                aron::data::Dict::DynamicCastAndCheck(data.proprioception->getElement(platformKey));
            update.localization.emplace_back(getTransform(platformData, data.timestamp));
        }
        else
        {
            ARMARX_INFO << "No odometry data! "
                        << "(No element '" << platformKey << "' in proprioception data.)"
                        << "\nIf you are using a mobile platform this should not have happened."
                        << "\nThis error is only logged once."
                        << "\nThese keys exist: " << data.proprioception->getAllKeys();
            noOdometryDataLogged = true;
        }
 
        return update;
    }
 
    armem::robot_state::localization::Transform
    RobotStateWriter::getTransform(const aron::data::DictPtr& platformData,
                                   const Time& timestamp) const
    {
        prop::arondto::Platform platform;
        platform.fromAron(platformData);
 
        const Eigen::Vector3f& relPose = platform.relativePosition;
 
        Eigen::Isometry3f odometryPose = Eigen::Isometry3f::Identity();
        odometryPose.translation() << relPose.x(), relPose.y(), 0; // TODO set height
        odometryPose.linear() = simox::math::rpy_to_mat3f(0.f, 0.f, relPose.z());
 
        armem::robot_state::localization::Transform transform;
        transform.header.parentFrame = armarx::OdometryFrame;
        transform.header.frame = armarx::armem::robot_state::constants::robotRootNodeName;
        transform.header.agent = properties.robotUnitProviderID.providerSegmentName;
        transform.header.timestamp = timestamp;
        transform.transform = odometryPose;
 
        return transform;
    }
 
} // namespace armarx::armem::server::robot_state::proprioception