MovePlatformToLandmark.cpp

Go to the documentation of this file.

#include "MovePlatformToLandmark.h"
 
#include <RobotAPI/libraries/core/FramedPose.h>
 
#include "util/PlatformUtil.h"
 
namespace armarx::skills
{
 
    namespace
    {
        platform_control::arondto::MovePlatformToLandmarkAcceptedType
        GetDefaultParameterization()
        {
            platform_control::arondto::MovePlatformToLandmarkAcceptedType ret;
            ret.orientationalAccuracy = 0.1;
            ret.positionalAccuracy = 100;
            return ret;
        }
    } // namespace
 
    SkillDescription MovePlatformToLandmark::Description = skills::SkillDescription{
        .skillId = {.skillName = "MovePlatformToLandmark"},
        .description = "Move a platform unit to target landmark. Uses a navigation memory to resolve the landmark.",
        .rootProfileDefaults = GetDefaultParameterization().toAron(),
        .timeout = armarx::Duration::MilliSeconds(100) + MovePlatformToPose::Description.timeout,
        .parametersType = platform_control::arondto::MovePlatformToLandmarkAcceptedType::ToAronType(),
    };
 
    MovePlatformToLandmark::MovePlatformToLandmark(armem::client::MemoryNameSystem& mns,
                                                   armarx::viz::Client& arviz,
                                                   PlatformControlSkillContext& context) :
        PlatformControlSkill(mns, arviz, Description.skillId.toString(), context),
        SimpleSpecializedSkill<ArgType>(Description)
    {
    }
 
    Skill::MainResult
    MovePlatformToLandmark::main(const SpecializedMainInput& in)
    {
        auto navigationReader = armarx::armem::locations::client::Reader();
        navigationReader.connect(mns);
 
        // convert landmark
        auto conversion =
            convertLandmarkToPose(in.parameters.graph, in.parameters.landmark, navigationReader);
 
        // create params to reuse skill
        platform_control::arondto::MovePlatformToPoseAcceptedType args;
        args.robotName = in.parameters.robotName;
        args.orientationalAccuracy = in.parameters.orientationalAccuracy;
        args.positionalAccuracy = in.parameters.positionalAccuracy;
        args.pose = conversion;
 
        SkillProxy prx(manager, MovePlatformToPose::Description);
        return {prx.executeSkill(in.executorName, args.toAron()).status, nullptr};
    }
} // namespace armarx::skills