NavigateToNamedLocation.cpp

Go to the documentation of this file.

#include "NavigateToNamedLocation.h"
 
#include <optional>
#include <string>
 
#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/ArmarXObjects/forward_declarations.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/SkillStatusUpdate.h>
#include <RobotAPI/libraries/skills/core/error/Exception.h>
 
#include <armarx/navigation/location/constants.h>
#include <armarx/navigation/memory/constants.h>
#include <armarx/navigation/skills/LocationNameResolver.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.useObjectLocations = true;
 
        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_CHECK_NOT_NULL(services.locationReader);
        ARMARX_CHECK_NOT_NULL(services.objectClassReader);
    }
 
    ::armarx::skills::Skill::MainResult
    NavigateToNamedLocation::main(const Base::SpecializedMainInput& in)
    {
        ARMARX_TRACE;
 
        const std::optional<ResolvedLocation> resolvedLocation =
            resolveLocationName(in.parameters.locationName,
                                LocationNameResolverServices{
                                    .locationReader = services.locationReader,
                                    .objectPoseClient = services.objectPoseClient,
                                    .objectClassReader = services.objectClassReader,
                                },
                                in.parameters.minMatchRatio,
                                in.parameters.useObjectLocations);
 
        if (not resolvedLocation.has_value())
        {
            ARMARX_ERROR << "Could not resolve location name '" << in.parameters.locationName
                         << "' to any location.";
            return MakeFailedResult();
        }
 
        ARMARX_IMPORTANT << "Navigating to location "
                         << QUOTED(resolvedLocation->provider + "::" + resolvedLocation->name)
                         << ".";
 
        throwIfSkillShouldTerminate();
 
        // Navigate to location
        {
            using ParamsT = arondto::NavigateToLocationParams;
 
            // pedantic check
            ARMARX_CHECK(resolvedLocation.has_value());
 
            auto update = callSubskill<ParamsT>(properties.subSkillIDs.navigateToLocation,
                                                [&resolvedLocation, &in](ParamsT& parameters) noexcept
                                                {
                                                    // override the parameters completely
                                                    parameters = in.parameters.navigateToLocation;
 
                                                    parameters.location = resolvedLocation->name;
                                                    parameters.locationProvider =
                                                        resolvedLocation->provider;
                                                });
            if (armarx::skills::skillExecutionFailed(update))
            {
                return MakeFailedResult();
            }
        }
 
        throwIfSkillShouldTerminate();
 
        return MakeSucceededResult();
    }
 
} // namespace armarx::navigation::skills