NavigateToNamedLocation.cpp

Go to the documentation of this file.

#include "NavigateToNamedLocation.h"
 
#include <sstream>
 
#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/armem/core/MemoryID.h>
#include <RobotAPI/libraries/skills/core/Skill.h>
#include <RobotAPI/libraries/skills/core/SkillDescription.h>
#include <RobotAPI/libraries/skills/core/error/Exception.h>
 
#include <armarx/navigation/skills/NavigateToLocation.h>
#include <armarx/navigation/skills/aron/NavigateToLocation.aron.generated.h>
#include <armarx/navigation/skills/skill_ids.h>
 
namespace armarx::navigation::skills
{
 
    armarx::skills::SkillDescription
    NavigateToNamedLocation::GetSkillDescription()
    {
        ParamType defaultParameters;
 
        defaultParameters.navigateToLocation.fromAron(
            NavigateToLocation::DefaultSkillDescription().rootProfileDefaults);
        defaultParameters.navigateToLocation.location = "<Resolved according to 'locationName'>";
 
        std::stringstream description;
        description << "Resolve a named location and navigate to it."
                    << "\n\n`locationName`: The location's human-readable name."
                    << "\n\n`minMatchRatio`: Minimum match ratio required to accept a match "
                       "between a location's name and `locationName`."
                    << "\n\nThe parameters `navigateToLocation` are passed to the sub skill "
                       "`NavigateToLocation`, except for `navigateToLocation.location`, which is "
                       "set to the location that `locationName` is resolved to.";
 
        return armarx::skills::SkillDescription{
            .skillId = skill_ids::NavigateToNamedLocation,
            .description = description.str(),
            .rootProfileDefaults = defaultParameters.toAron(),
            .timeout = armarx::core::time::Duration::Hours(1),
            .parametersType = Params::ToAronType(),
        };
    }
 
    NavigateToNamedLocation::Properties::SubSkillIds::SubSkillIds() :
        navigateToLocation{armarx::navigation::skills::skill_ids::NavigateToLocation}
    {
    }
 
    NavigateToNamedLocation::NavigateToNamedLocation(const Properties& properties,
                                                     const Services& services) :
        Base(GetSkillDescription()), services(services), properties(properties)
    {
    }
 
    ::armarx::skills::Skill::MainResult
    NavigateToNamedLocation::main(const Base::SpecializedMainInput& in)
    {
        ARMARX_TRACE;
 
        // Resolve location name
        armarx::armem::MemoryID locationId;
        {
            std::stringstream log;
            auto matchResult = services.locationReader.resolveLocationName(
                in.parameters.locationName, in.parameters.minMatchRatio, &log);
 
            ARMARX_INFO << log.str();
            ARMARX_CHECK(matchResult.resolved.has_value() xor matchResult.errorMessage.has_value());
 
            if (matchResult.errorMessage.has_value())
            {
                std::stringstream msg;
                msg << "Failed to resolve location name '" << in.parameters.locationName
                    << "'. Reason: " << matchResult.errorMessage.value();
                throw armarx::skills::error::SkillFailedException(__PRETTY_FUNCTION__, msg.str());
            }
 
            ARMARX_CHECK(matchResult.resolved.has_value());
            locationId = matchResult.resolved->locationId;
        }
 
        ARMARX_IMPORTANT << "Navigating to location '" << locationId << "'.";
 
        throwIfSkillShouldTerminate();
 
        // Navigate to location
        {
            arondto::NavigateToLocationParams parameters = in.parameters.navigateToLocation;
            parameters.location = locationId.providerSegmentName + "/" + locationId.entityName;
 
            auto update = callSubskill(properties.subSkillIDs.navigateToLocation, parameters);
            if (not(update.has_value() and update->hasBeenSucceeded()))
            {
                return MakeFailedResult();
            }
        }
 
        throwIfSkillShouldTerminate();
 
        return MakeSucceededResult();
    }
 
} // namespace armarx::navigation::skills