RobotStateMemory.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 "RobotStateMemory.h"
 
#include <SimoxUtility/algorithm/contains.h>
#include <SimoxUtility/algorithm/get_map_keys_values.h>
#include <SimoxUtility/algorithm/string.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/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
 
#include <RobotAPI/interface/core/PoseBase.h>
#include <RobotAPI/libraries/armem/server/ltm/Memory.h>
#include <RobotAPI/libraries/armem_robot_state/memory_ids.h>
#include <RobotAPI/libraries/armem_robot_state/server/common/Visu.h>
#include <RobotAPI/libraries/armem_robot_state/server/proprioception/aron_conversions.h>
#include <RobotAPI/libraries/core/Pose.h>
 
namespace armarx::armem::server::robot_state
{
 
    RobotStateMemory::RobotStateMemory() :
        descriptionSegment(iceAdapter()),
        proprioceptionSegment(iceAdapter()),
        exteroceptionSegment(iceAdapter()),
        localizationSegment(iceAdapter()),
        commonVisu(descriptionSegment, proprioceptionSegment, localizationSegment)
    {
        addPlugin(debugObserver);
        ARMARX_CHECK_NOT_NULL(debugObserver);
 
        addPlugin(robotUnit.plugin);
        ARMARX_CHECK_NOT_NULL(robotUnit.plugin);
        robotUnit.plugin->setRobotUnitAsOptionalDependency(true);
        robotUnit.dataBuffer = std::make_unique<RobotUnit::Queue>();
    }
 
    RobotStateMemory::~RobotStateMemory() = default;
 
    armarx::PropertyDefinitionsPtr
    RobotStateMemory::createPropertyDefinitions()
    {
        armarx::PropertyDefinitionsPtr const defs =
            new ComponentPropertyDefinitions(getConfigIdentifier());
 
        const std::string robotUnitPrefix = RobotUnit::sensorValuePrefix;
 
 
        defs->optional(robotUnit.waitForRobotUnit,
                       "WaitForRobotUnit",
                       "Add the robot unit as dependency to the component. This memory requires a "
                       "running RobotUnit, therefore we should add it as explicit dependency.");
 
        defs->optional(robotUnit.reader.properties.sensorPrefix,
                       robotUnitPrefix + "SensorValuePrefix",
                       "Prefix of all sensor values.");
        defs->optional(robotUnit.pollFrequency,
                       robotUnitPrefix + "UpdateFrequency",
                       "The frequency to store values in Hz. All other values get discarded. "
                       "Minimum is 1, max is " +
                           std::to_string(RobotUnit::ROBOT_UNIT_MAXIMUM_FREQUENCY) + ".")
            .setMin(1)
            .setMax(RobotUnit::ROBOT_UNIT_MAXIMUM_FREQUENCY);
 
        defs->optional(robotUnit.writer.properties.queueSizeWarningThreshold,
                       robotUnitPrefix + "QueueSizeWarningThreshold",
                       "If the RobotStateWriter's data queue grows larger than this value, "
                       "a warning is logged (rate-limited).");
 
 
        const std::string prefix = "mem.";
 
        setMemoryName(armem::robot_state::memoryID.memoryName);
 
        descriptionSegment.defineProperties(defs, prefix + "desc.");
        exteroceptionSegment.defineProperties(defs, prefix + "ext.");
        proprioceptionSegment.defineProperties(defs, prefix + "prop.");
        localizationSegment.defineProperties(defs, prefix + "loc.");
        commonVisu.defineProperties(defs, prefix + "visu.");
 
        return defs;
    }
 
    std::string
    RobotStateMemory::getDefaultName() const
    {
        return "RobotStateMemory";
    }
 
    void
    RobotStateMemory::onInitComponent()
    {
        descriptionSegment.init();
        proprioceptionSegment.init();
        exteroceptionSegment.init();
        localizationSegment.init();
        commonVisu.init();
 
        robotUnit.pollFrequency =
            std::clamp(robotUnit.pollFrequency, 0.F, RobotUnit::ROBOT_UNIT_MAXIMUM_FREQUENCY);
 
        std::vector<std::string> includePaths;
        std::vector<std::string> packages = armarx::Application::GetProjectDependencies();
        packages.push_back(Application::GetProjectName());
 
        for (const std::string& projectName : packages)
        {
            if (projectName.empty())
            {
                continue;
            }
 
            const CMakePackageFinder project(projectName);
            const std::string pathsString = project.getIncludePaths();
            std::vector<std::string> projectIncludePaths = simox::alg::split(pathsString, ";,");
            includePaths.insert(
                includePaths.end(), projectIncludePaths.begin(), projectIncludePaths.end());
        }
 
        if (robotUnit.waitForRobotUnit)
        {
            usingProxy(robotUnit.plugin->getRobotUnitName());
        }
    }
 
    void
    RobotStateMemory::onConnectComponent()
    {
        {
            ARMARX_CHECK(not robotUnit.reader.task or not robotUnit.reader.task->isRunning());
            ARMARX_CHECK(not robotUnit.writer.task or not robotUnit.writer.task->isRunning());
            robotUnit.dataBuffer = std::make_unique<RobotUnit::Queue>();
        }
        ARMARX_CHECK_NOT_NULL(debugObserver->getDebugObserver());
 
        // Pass the debug observer to the long-term memory for timing measurements
        longtermMemory().setDebugObserver(debugObserver->getDebugObserver());
        ARMARX_INFO << "Debug observer set for LTM timing measurements";
 
        // 1. General setup
        localizationSegment.onConnect();
        commonVisu.connect(getArvizClient(), debugObserver->getDebugObserver());
 
        // 2. Check for RobotUnit. If RobotUnit is enabled. Otherwise, we do not wait for a streaming service and do not set up the segments
        if (robotUnit.plugin->hasRobotUnitName())
        {
            robotUnit.plugin->waitUntilRobotUnitIsRunning();
            const RobotUnitInterfacePrx robotUnitPrx = robotUnit.plugin->getRobotUnit();
 
            robotUnit.reader.setTag(getName());
            robotUnit.writer.setTag(getName());
 
            descriptionSegment.onConnect(robotUnitPrx);
            proprioceptionSegment.onConnect(robotUnitPrx);
 
            robotUnit.reader.connect(
                *robotUnit.plugin,
                *debugObserver,
                proprioceptionSegment.getRobotUnitProviderID().providerSegmentName);
            robotUnit.writer.connect(*debugObserver);
            robotUnit.writer.properties.robotUnitProviderID =
                proprioceptionSegment.getRobotUnitProviderID();
 
            robotUnit.reader.task = new SimpleRunningTask<>(
                [this]() { robotUnit.reader.run(robotUnit.pollFrequency, *robotUnit.dataBuffer); },
                "Robot Unit Reader");
            robotUnit.writer.task = new SimpleRunningTask<>(
                [this]()
                {
                    robotUnit.writer.run(robotUnit.pollFrequency,
                                         *robotUnit.dataBuffer,
                                         iceAdapter(),
                                         localizationSegment);
                },
                "Robot State Writer");
 
            startRobotUnitStream();
        }
 
        createRemoteGuiTab();
        RemoteGui_startRunningTask();
    }
 
    void
    RobotStateMemory::onDisconnectComponent()
    {
        stopRobotUnitStream();
    }
 
    void
    RobotStateMemory::onExitComponent()
    {
        stopRobotUnitStream();
    }
 
    armarx::PoseBasePtr
    RobotStateMemory::getGlobalRobotPose(Ice::Long timestampUs,
                                         const std::string& robotName,
                                         const ::Ice::Current& /*unused*/)
    {
        const armem::Time timestamp{armem::Duration::MicroSeconds(timestampUs)};
        ARMARX_DEBUG << "Getting robot pose of robot " << robotName << " at timestamp " << timestamp
                     << ".";
 
        auto poseMap = localizationSegment.getRobotGlobalPoses(timestamp);
 
        const bool robotNameFound = simox::alg::contains_key(poseMap, robotName);
        ARMARX_CHECK(robotNameFound)
            << "Robot with name " << robotName << " does not exist at or before timestamp "
            << timestamp << ".\n"
            << "Available robots are: " << simox::alg::get_keys(poseMap);
 
        return {new Pose(poseMap[robotName].matrix())};
    }
 
    /*************************************************************/
    // RobotUnit Streaming functions
    /*************************************************************/
 
 
    void
    RobotStateMemory::startRobotUnitStream()
    {
        ARMARX_INFO << "Starting RobotUnit stream";
        ARMARX_CHECK(robotUnit.plugin->robotUnitIsRunning());
 
        if (robotUnit.reader.task->isRunning() || robotUnit.writer.task->isRunning())
        {
            if (robotUnit.reader.task->isRunning() && robotUnit.writer.task->isRunning())
            {
                return;
            }
            ARMARX_WARNING << "Found inconsistency in running tasks. Restarting all!";
            stopRobotUnitStream();
        }
        {
            std::stringstream ss;
            ss << "Getting sensor values for:" << std::endl;
            for (const auto& [name, dataEntry] : robotUnit.reader.description.entries)
            {
                const std::string type =
                    RobotUnitDataStreaming::DataEntryNames.to_name(dataEntry.type);
                ss << "\t" << name << " (type: '" << type << "')" << std::endl;
            }
            ARMARX_INFO << ss.str();
        }
 
        robotUnit.reader.task->start();
        robotUnit.writer.task->start();
    }
 
    void
    RobotStateMemory::stopRobotUnitStream()
    {
        ARMARX_INFO << "Stopping RobotUnit stream";
        robotUnit.dataBuffer->close();
        robotUnit.reader.task->stop();
        ARMARX_VERBOSE << "RobotUnit reader stopped";
        robotUnit.writer.task->stop();
        ARMARX_VERBOSE << "RobotUnit writer stopped";
    }
 
    void
    RobotStateMemory::createRemoteGuiTab()
    {
        using namespace armarx::RemoteGui::Client;
 
        VBoxLayout root;
 
        tab.clearRobotStateWriterQueueButton.setLabel("Clear RobotStateWriter Queue");
        root.addChild(tab.clearRobotStateWriterQueueButton);
 
        RemoteGui_createTab(getName(), root, &tab);
    }
 
    void
    RobotStateMemory::RemoteGui_update()
    {
        try
        {
            if (tab.clearRobotStateWriterQueueButton.wasClicked())
            {
                clearRobotStateWriterQueue();
            }
        }
        catch (const std::exception& e)
        {
            ARMARX_WARNING << "RemoteGui_update: exception while handling button click: "
                           << e.what();
        }
        catch (...)
        {
            ARMARX_WARNING << "RemoteGui_update: unknown exception while handling button click.";
        }
    }
 
    void
    RobotStateMemory::clearRobotStateWriterQueue()
    {
        std::size_t dropped = 0;
        try
        {
            if (not robotUnit.dataBuffer)
            {
                ARMARX_WARNING << "Cannot clear RobotStateWriter queue: data buffer is null.";
                return;
            }
 
            proprioception::RobotUnitData discarded;
            while (true)
            {
                boost::queue_op_status status = boost::queue_op_status::empty;
                try
                {
                    status = robotUnit.dataBuffer->try_pull(discarded);
                }
                catch (const std::exception& e)
                {
                    ARMARX_WARNING << "Exception during queue try_pull after dropping " << dropped
                                   << " entries: " << e.what() << ". Stopping drain.";
                    break;
                }
                catch (...)
                {
                    ARMARX_WARNING << "Unknown exception during queue try_pull after dropping "
                                   << dropped << " entries. Stopping drain.";
                    break;
                }
 
                if (status != boost::queue_op_status::success)
                {
                    break;
                }
                ++dropped;
            }
 
            ARMARX_IMPORTANT << "Cleared RobotStateWriter queue: dropped " << dropped
                             << " entries.";
        }
        catch (const std::exception& e)
        {
            ARMARX_WARNING << "clearRobotStateWriterQueue failed after dropping " << dropped
                           << " entries: " << e.what();
        }
        catch (...)
        {
            ARMARX_WARNING << "clearRobotStateWriterQueue failed with unknown exception after "
                              "dropping "
                           << dropped << " entries.";
        }
    }
 
} // namespace armarx::armem::server::robot_state