WhatCanYouSeeNow.cpp

Go to the documentation of this file.

#include "WhatCanYouSeeNow.h"
 
#include <RobotSkillTemplates/libraries/skill_joint_control/MoveJointsToPosition.h>
 
#include "util/VisualSearchUtil.h"
 
namespace armarx::skills
{
 
    SkillDescription WhatCanYouSeeNow::Description = skills::SkillDescription{
        .skillId = {.skillName = "WhatCanYouSeeNow"},
        .description = "Move the head to three known configurations and search for objects",
        .timeout = armarx::Duration::MilliSeconds(2000),
        .parametersType = visual_search::arondto::WhatCanYouSeeNowAcceptedType::ToAronType(),
    };
 
    WhatCanYouSeeNow::WhatCanYouSeeNow(armem::client::MemoryNameSystem& mns,
                                       armarx::viz::Client& arviz,
                                       VisualSearchSkillContext& context) :
        VisualSearchSkill(mns, arviz, Description.skillId.toString(), context),
        SimpleSpecializedSkill<ArgType>(Description)
    {
    }
 
    Skill::MainResult
    WhatCanYouSeeNow::main(const SpecializedMainInput& in)
    {
        armem::robot_state::VirtualRobotReader robotReader;
        armem::obj::instance::Writer objectWriter;
        armem::obj::instance::Reader objectReader;
 
        robotReader.connect(mns);
        objectWriter.connect(mns);
        objectReader.connect(mns);
 
        // //////////////////////////////
        // get robot
        // //////////////////////////////
        auto robot =
            robotReader.getSynchronizedRobot(in.parameters.robotName,
                                             armem::Time::Now(),
                                             VirtualRobot::RobotIO::RobotDescription::eStructure);
        if (!robot)
        {
            ARMARX_ERROR << "Lost robot.";
            return {TerminatedSkillStatus::Failed, nullptr};
        }
 
        // Look down
        {
            skills::joint_control::arondto::MoveJointsToPositionAcceptedType params;
            params.robotName = in.parameters.robotName;
            params.jointTargetTolerance = 0.03; // Todo
            params.jointMaxSpeed = 0.75;
            params.accelerationTime = 500;
            params.setVelocitiesToZeroAtEnd = true;
            params.targetJointMap["Neck_1_Pitch"] = 0.6;
 
            SkillProxy prx(manager, SkillID{
                .providerId = ProviderID{.providerName = context.jointControlSkillProvider},
                .skillName = MoveJointsToPosition::Description.skillId.skillName});
 
            if (prx.executeSkill(in.executorName, params.toAron())
                    .status != TerminatedSkillStatus::Succeeded)
            {
                return {TerminatedSkillStatus::Failed, nullptr};
            }
        }
 
        // //////////////////////////////
        // get object pose
        // //////////////////////////////
        objectReader.requestLocalization(
            in.parameters.objectEntityId, "", armarx::core::time::Duration::MilliSeconds(500));
        std::this_thread::sleep_for(std::chrono::milliseconds(500));
 
        // Look up
        {
            skills::joint_control::arondto::MoveJointsToPositionAcceptedType params;
            params.robotName = in.parameters.robotName;
            params.jointTargetTolerance = 0.03; // Todo
            params.jointMaxSpeed = 0.75;
            params.accelerationTime = 500;
            params.setVelocitiesToZeroAtEnd = true;
            params.targetJointMap["Neck_1_Pitch"] = 0.0;
 
            SkillProxy prx(manager, SkillID{
                .providerId = ProviderID{.providerName = context.jointControlSkillProvider},
                .skillName = MoveJointsToPosition::Description.skillId.skillName});
 
            if (prx.executeSkill(in.executorName, params.toAron())
                    .status != TerminatedSkillStatus::Succeeded)
            {
                return {TerminatedSkillStatus::Failed, nullptr};
            }
        }
 
        return {TerminatedSkillStatus::Succeeded, nullptr};
    }
} // namespace armarx::skills