RotateTowardsLocation.cpp

Go to the documentation of this file.

#include "RotateTowardsLocation.h"
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/Duration.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
 
#include <RobotAPI/libraries/core/Pose.h>
#include <RobotAPI/libraries/skills/core/Skill.h>
#include <RobotAPI/libraries/skills/core/SkillDescription.h>
#include <RobotAPI/libraries/skills/core/SkillStatusUpdate.h>
 
#include <armarx/navigation/skills/aron/RotateTowards.aron.generated.h>
#include <armarx/navigation/skills/skill_ids.h>
 
namespace armarx::navigation::skills
{
 
    armarx::skills::SkillDescription
    RotateTowardsLocation::GetSkillDescription()
    {
        ParamType defaultParameters;
 
        defaultParameters.navigatingSkillParams = NavigatingSkillHelper::DefaultSkillDescription();
        defaultParameters.navigatingSkillParams.globalPlanningAlgorithm =
            arondto::GlobalPlanningAlgorithm::Point2Point;
 
        return armarx::skills::SkillDescription{
            .skillId = skill_ids::RotateTowardsLocation,
            .description = "Rotate the robot to face a specified location.",
            .rootProfileDefaults = defaultParameters.toAron(),
            .timeout = armarx::core::time::Duration::Hours(1),
            .parametersType = Params::ToAronType(),
        };
    }
 
    RotateTowardsLocation::Properties::SubSkillIds::SubSkillIds() :
        rotateTowards{armarx::navigation::skills::skill_ids::RotateTowards}
    {
    }
 
    RotateTowardsLocation::RotateTowardsLocation(const Properties& properties,
                                                 const Services& services) :
        Base(GetSkillDescription()), services_(services), properties_(properties)
    {
    }
 
    ::armarx::skills::Skill::MainResult
    RotateTowardsLocation::main(const Base::SpecializedMainInput& in)
    {
        ARMARX_TRACE;
 
        const auto locations = services_.locationReader.resolveLocation(
            in.parameters.location, in.parameters.locationProvider);
 
        ARMARX_CHECK_NOT_EMPTY(locations) << "Location " << QUOTED(in.parameters.location) << " not available!"; 
        if (locations.size() > 1)
        {
            ARMARX_WARNING << "Multiple locations matched the requested location name '"
                           << in.parameters.location << "'. Using the first one.";
        }
        const auto positionIce = locations.front().framedPose.position;
        const auto position = armarx::fromIce(positionIce);
 
        throwIfSkillShouldTerminate();
 
        // Rotate towards position
        {
            using ParamsT = arondto::RotateTowardsParams;
 
            auto update = callSubskill<ParamsT>(properties_.subSkillIDs.rotateTowards,
                                                [&](ParamsT& parameters) noexcept
                                                {
                                                    parameters.navigatingSkillParams =
                                                        in.parameters.navigatingSkillParams;
                                                    parameters.targetPositionGlobal = position;
                                                });
            if (armarx::skills::skillExecutionFailed(update))
            {
                return MakeFailedResult();
            }
        }
 
        throwIfSkillShouldTerminate();
 
        return MakeSucceededResult();
    }
 
} // namespace armarx::navigation::skills