LookForObjectsWithKinematicUnit.cpp

Go to the documentation of this file.

#include "LookForObjectsWithKinematicUnit.h"
 
#include <thread>
#include <vector>
 
#include <Eigen/Core>
#include <Eigen/Geometry>
 
#include <VirtualRobot/Robot.h>
 
#include <ArmarXCore/core/time/Duration.h>
 
#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectInfo.h>
#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
#include <RobotAPI/libraries/aron/core/aron_conversions.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
 
#include <armarx/view_selection/skills/constants.h>
 
namespace armarx::view_selection::skills
{
 
    std::map<std::string, float> neck_joint_angles = {{"Neck_1_Yaw", 0.0}, {"Neck_2_Pitch", 0.3}};
    VirtualRobot::RobotPtr localRobot;
 
    LookForObjectsWithKinematicUnit::LookForObjectsWithKinematicUnit(const Services& srv) :
        Base(DefaultSkillDescription())
    {
        srv_.emplace(srv);
    }
 
    ::armarx::skills::Skill::InitResult
    LookForObjectsWithKinematicUnit::init(const Base::SpecializedInitInput& in)
    {
        return ::armarx::skills::Skill::InitResult{
            .status = ::armarx::skills::TerminatedSkillStatus::Succeeded};
    }
 
    void
    LookForObjectsWithKinematicUnit::setNeckJointAngles(float goal_yaw,
                                                        float goal_pitch,
                                                        int waypoints_num)
    {
        float yaw_step_size = (goal_yaw - neck_joint_angles["Neck_1_Yaw"]) / waypoints_num;
        float pitch_step_size = (goal_pitch - neck_joint_angles["Neck_2_Pitch"]) / waypoints_num;
 
        for (int i = 1; i < waypoints_num; i++)
        {
            neck_joint_angles["Neck_1_Yaw"] = neck_joint_angles["Neck_1_Yaw"] + yaw_step_size;
            neck_joint_angles["Neck_2_Pitch"] = neck_joint_angles["Neck_2_Pitch"] + pitch_step_size;
            srv_->kinematicUnit->setJointAngles(
                {{"Neck_1_Yaw", neck_joint_angles["Neck_1_Yaw"]},
                 {"Neck_2_Pitch", neck_joint_angles["Neck_2_Pitch"]}});
            std::this_thread::sleep_for(std::chrono::milliseconds(300));
        }
        std::this_thread::sleep_for(std::chrono::milliseconds(300));
    }
 
    void
    LookForObjectsWithKinematicUnit::speak(const std::string& text)
    {
        ARMARX_INFO << "Saying '" << text.substr(0, 50) << "...'";
        srv_->textToSpeech->reportText(text);
    }
 
    ::armarx::skills::Skill::MainResult
    LookForObjectsWithKinematicUnit::main(const Base::SpecializedMainInput& in)
    {
 
 
        srv_->kinematicUnit->switchControlMode(
            {{"Neck_1_Yaw", armarx::ControlMode::ePositionControl},
             {"Neck_2_Pitch", armarx::ControlMode::ePositionControl},
             {"TorsoJoint", armarx::ControlMode::ePositionControl}});
        srv_->kinematicUnit->setJointAngles({{"TorsoJoint", -109.76679992675781}});
        std::this_thread::sleep_for(std::chrono::milliseconds(300));
 
        ARMARX_INFO << "start the running";
        // doing the neck movement routine
        // move neck to start position
        setNeckJointAngles(0.0, 0.2, 10);
        ARMARX_INFO << "finished first movement'";
        //  move the neck to the max right
        setNeckJointAngles(-0.593412, 0.66654124, 10);
        std::this_thread::sleep_for(std::chrono::milliseconds(450));
        //  move the neck to the max left
        setNeckJointAngles(0.0, 0.66654124, 10);
        std::this_thread::sleep_for(std::chrono::milliseconds(450));
        setNeckJointAngles(0.593412, 0.66654124, 10);
        std::this_thread::sleep_for(std::chrono::milliseconds(450));
        // move the neck back to the normal
        setNeckJointAngles(0.0, 0.2, 10);
        ARMARX_INFO << "no problem with the neck";
 
        // getting the
        objpose::ObjectPoseSeq objectPoses = srv_->objectPoseClient.fetchObjectPoses();
        // these objects are always in the memory so, we need to not consider them for what can u see now scenario
        armarx::ObjectID objectId;
        std::vector<std::string> predefined_poses = {"r003-door",
                                                     "r003-pillar",
                                                     "clean-box",
                                                     "dirty-box",
                                                     "beam",
                                                     "hund-table-120x80",
                                                     "hund-table-180x60",
                                                     "hund-table-180x80",
                                                     "hund-table-200x80",
                                                     "mobile-dishwasher",
                                                     "mobile-fridge",
                                                     "mobile-kitchen-counter",
                                                     "guard-support-full-with-light",
                                                     "ladder-closed",
                                                     "mounting-adhesive",
                                                     "spraybottle",
                                                     "workbench",
                                                     "box"};
 
        std::vector<std::string> objects_names;
 
        armarx::ObjectFinder objFind;
 
        for (auto o : objectPoses)
        {
 
            const std::string className = o.objectID.className();
 
            if (std::find(predefined_poses.begin(), predefined_poses.end(), className) ==
                    predefined_poses.end() &&
                std::find(objects_names.begin(), objects_names.end(), className) ==
                    objects_names.end())
            {
                std::string objectName;
                std::optional<armarx::ObjectInfo> info = objFind.findObject(o.objectID);
                if (info)
                {
                    std::optional<std::vector<std::string>> names = info->loadSpokenNames();
                    if (names.has_value() and not names->empty())
                    {
                        objectName = names->front();
                    }
                }
 
                if (objectName.empty())
                {
                    objectName = className;
                    objectName.erase(std::remove_if(std::begin(objectName),
                                                    std::end(objectName),
                                                    [](auto ch) { return std::isdigit(ch); }),
                                     objectName.end());
                }
 
                objects_names.push_back(objectName);
            }
        }
 
        ARMARX_INFO << "no problem with the poses";
        for (int i = 0; i < static_cast<int>(objects_names.size()); i++)
        {
            if (i == 0)
            {
                if (static_cast<int>(objects_names.size()) == 1)
                {
                    speak("I can see the following object");
                    std::this_thread::sleep_for(std::chrono::milliseconds(200));
                }
                else
                {
 
                    speak("I can see the following objects");
                    std::this_thread::sleep_for(std::chrono::milliseconds(200));
                }
                // speak("I can see the following objects");
                // std::this_thread::sleep_for(std::chrono::milliseconds(200));
            }
            else if (i == (static_cast<int>(objects_names.size()) - 1))
            {
                speak("and");
                std::this_thread::sleep_for(std::chrono::milliseconds(200));
            }
            speak(objects_names[i]);
            std::this_thread::sleep_for(std::chrono::milliseconds(200));
        }
 
 
        return ::armarx::skills::Skill::MainResult{
            .status = ::armarx::skills::TerminatedSkillStatus::Succeeded};
    }
 
    void
    LookForObjectsWithKinematicUnit::onStopRequested()
    {
    }
 
    armarx::skills::SkillDescription
    LookForObjectsWithKinematicUnit::DefaultSkillDescription()
    {
        ParamType defaultParameters;
        // TODO(): set parameter defaults
 
        return armarx::skills::SkillDescription{
            .skillId = {.skillName = constants::skill_names::LookForObjectsWithKinematicUnit},
            .description = "",
            .rootProfileDefaults = defaultParameters.toAron(),
            .timeout = armarx::core::time::Duration(),
            .parametersType = Params::ToAronType(),
        };
    }
 
 
} // namespace armarx::view_selection::skills