RobotHeadMovement.cpp

Go to the documentation of this file.

#include "RobotHeadMovement.h"
 
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
#include <ArmarXCore/core/exceptions/local/UnexpectedEnumValueException.h>
#include <ArmarXCore/core/time/TimeUtil.h>
 
namespace armarx::armem::server::obj::instance
{
 
    void
    RobotHeadMovement::defineProperties(armarx::PropertyDefinitionsPtr defs,
                                        const std::string& prefix)
    {
        defs->optional(
            checkHeadVelocity,
            prefix + "checkHeadVelocity",
            "If true, check whether the head is moving and discard updates in the meantime.");
        defs->optional(maxJointVelocity,
                       prefix + "maxJointVelocity",
                       "If a head joint's velocity is higher, the head is considered moving.");
        defs->optional(discardIntervalAfterMoveMS,
                       prefix + "discardIntervalAfterMoveMS",
                       "For how long new updates are ignored after moving the head.");
    }
 
    void
    RobotHeadMovement::fetchDatafields()
    {
        if (kinematicUnitObserver)
        {
            for (const std::string& jointName : jointNames)
            {
                std::string error = "";
                try
                {
                    DatafieldRefBasePtr datafield = kinematicUnitObserver->getDatafieldRefByName(
                        jointVelocitiesChannelName, jointName);
                    DatafieldRefPtr casted = DatafieldRefPtr::dynamicCast(datafield);
                    if (casted)
                    {
                        jointVelocitiesDatafields.push_back(casted);
                    }
                }
                catch (const InvalidDatafieldException& e)
                {
                    error = e.what();
                }
                catch (const InvalidChannelException& e)
                {
                    error = e.what();
                }
                if (error.size() > 0)
                {
                    ARMARX_WARNING << "Could not get datafield for joint '" << jointName
                                   << "' in channel '" << jointVelocitiesChannelName << "': \n "
                                   << error;
                }
            }
        }
        else
        {
            ARMARX_INFO << "Cannot fetch joint velocity datafields because there is no kinematic "
                           "unit observer.";
        }
    }
 
    bool
    RobotHeadMovement::isMoving() const
    {
        for (DatafieldRefPtr df : jointVelocitiesDatafields)
        {
            if (df)
            {
                float jointVelocity = df->getFloat();
                // ARMARX_IMPORTANT << "Is " << df->datafieldName << " " << VAROUT(std::abs(jointVelocity)) << " > " << VAROUT(maxJointVelocity) << "?";
                if (std::abs(jointVelocity) > maxJointVelocity)
                {
                    return true;
                }
            }
        }
        return false;
    }
 
    void
    RobotHeadMovement::movementStarts(long discardIntervalMs)
    {
        return movementStarts(Duration::MilliSeconds(discardIntervalMs));
    }
 
    void
    RobotHeadMovement::movementStarts(const Duration& discardInterval)
    {
        discardUpdatesUntil = DateTime::Now() + discardInterval;
    }
 
    void
    RobotHeadMovement::movementStops(long discardIntervalMs)
    {
        return movementStops(Duration::MilliSeconds(discardIntervalMs));
    }
 
    void
    RobotHeadMovement::movementStops(const Duration& discardInterval)
    {
        if (discardInterval.toMilliSeconds() < 0)
        {
            //  Stop discarding.
            discardUpdatesUntil = DateTime::Invalid();
        }
        else
        {
            // Basically the same as starting.
            discardUpdatesUntil = DateTime::Now() + discardInterval;
        }
    }
 
    objpose::SignalHeadMovementOutput
    RobotHeadMovement::signalHeadMovement(const objpose::SignalHeadMovementInput& input)
    {
        objpose::SignalHeadMovementOutput output;
        switch (input.action)
        {
            case objpose::HeadMovementAction::HeadMovementAction_Starting:
                this->movementStarts(input.discardUpdatesIntervalMilliSeconds < 0
                                         ? this->discardIntervalAfterMoveMS
                                         : input.discardUpdatesIntervalMilliSeconds);
                break;
            case objpose::HeadMovementAction::HeadMovementAction_Stopping:
                this->movementStops(input.discardUpdatesIntervalMilliSeconds);
                break;
            default:
                ARMARX_UNEXPECTED_ENUM_VALUE(objpose::HeadMovementAction, input.action);
                break;
        }
        output.discardUpdatesUntilMilliSeconds =
            this->discardUpdatesUntil.toMilliSecondsSinceEpoch();
        return output;
    }
 
    RobotHeadMovement::Discard
    RobotHeadMovement::getDiscard()
    {
        Discard discard;
        if (checkHeadVelocity)
        {
            if (isMoving())
            {
                movementStarts(discardIntervalAfterMoveMS);
                // ARMARX_IMPORTANT << "Ignoring pose update because robot head is moving! until " << discardUpdatesUntil;
                discard.all = true;
            }
            else if (DateTime::Now() < discardUpdatesUntil)
            {
                discard.all = true;
                // ARMARX_IMPORTANT << "Ignoring pose update because robot head has moved until: " << discardUpdatesUntil;
            }
            else
            {
                discard.updatesUntil = discardUpdatesUntil;
            }
        }
        return discard;
    }
 
    void
    RobotHeadMovement::RemoteGui::setup(const RobotHeadMovement& rhm)
    {
        using namespace armarx::RemoteGui::Client;
 
        checkHeadVelocity.setValue(rhm.checkHeadVelocity);
        {
            float max = 10.0;
            maxJointVelocity.setRange(0, max);
            maxJointVelocity.setDecimals(3);
            maxJointVelocity.setSteps(int(100 * max)); // = 0.01 steps
            maxJointVelocity.setValue(rhm.maxJointVelocity);
        }
        {
            int max = 10 * 1000;
            discardIntervalAfterMoveMS.setRange(0, max);
            discardIntervalAfterMoveMS.setValue(rhm.discardIntervalAfterMoveMS);
        }
 
        GridLayout grid;
        int row = 0;
        grid.add(Label("Check Head Motion"), {row, 0}).add(checkHeadVelocity, {row, 1});
        row++;
        grid.add(Label("Max Joint Velocity"), {row, 0}).add(maxJointVelocity, {row, 1});
        row++;
        grid.add(Label("Discard Interval after Move [ms]"), {row, 0})
            .add(discardIntervalAfterMoveMS, {row, 1});
        row++;
 
        group.setLabel("Robot Head Movement");
        group.addChild(grid);
    }
 
    void
    RobotHeadMovement::RemoteGui::update(RobotHeadMovement& rhm)
    {
        rhm.checkHeadVelocity = checkHeadVelocity.getValue();
        rhm.maxJointVelocity = maxJointVelocity.getValue();
        rhm.discardIntervalAfterMoveMS = discardIntervalAfterMoveMS.getValue();
    }
 
} // namespace armarx::armem::server::obj::instance