NavigateToNamedLocation.cpp

Go to the documentation of this file.

#include "NavigateToNamedLocation.h"
 
#include <map>
#include <optional>
#include <set>
#include <sstream>
#include <string>
#include <vector>
 
#include <SimoxUtility/algorithm/string/string_tools.h>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/Duration.h>
#include <ArmarXCore/core/util/StringHelpers.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
 
#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
#include <RobotAPI/libraries/ArmarXObjects/forward_declarations.h>
#include <RobotAPI/libraries/armem/core/MemoryID.h>
#include <RobotAPI/libraries/armem_objects/types.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/core/location.h>
#include <armarx/navigation/location/constants.h>
#include <armarx/navigation/memory/constants.h>
#include <armarx/navigation/skills/NavigateToLocation.h>
#include <armarx/navigation/skills/aron/NavigateToLocation.aron.generated.h>
#include <armarx/navigation/skills/skill_ids.h>
 
#include <range/v3/action.hpp>
#include <range/v3/algorithm/contains.hpp>
#include <range/v3/range/conversion.hpp>
#include <range/v3/view/concat.hpp>
#include <range/v3/view/filter.hpp>
#include <range/v3/view/map.hpp>
#include <range/v3/view/transform.hpp>
 
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;
 
        struct ResolvedLocation
        {
            std::string name;
            std::string provider;
        };
 
        // Try to match requested location name to an object. If there is a (perfect) match, we check whether there is a
        // location associated with the object (`inFrontOf`).
        const auto resolvedLocationFromObjectFn = [&]() -> std::optional<ResolvedLocation>
        {
            // query object poses from memory
            const auto objectPoseStorage = services.objectPoseClient.getObjectPoseStorage();
            const auto objectPosesIce = objectPoseStorage->getObjectPoses();
 
            const std::map<armarx::ObjectID, armem::clazz::ObjectClass> objectClasses =
                services.objectClassReader->getAllObjectClasses();
 
            objpose::ObjectPoseSeq objectPoses;
            objpose::fromIce(objectPosesIce, objectPoses);
 
            // search for matching objects (helper lambda)
            const auto isMatchingObject = [&](const objpose::ObjectPose& pose) -> bool
            {
                // make sure that the classes exist.
              if(not ranges::contains(ranges::views::keys(objectClasses),
                                              pose.objectID.getClassID(),
                                              [](const armarx::ObjectID& oid)
                                              { return oid.getClassID(); }))
                {
                  ARMARX_VERBOSE << "Requested to match object but there is no match";
                  return false;
                }
 
                const auto& objectClass = objectClasses.at(pose.objectID.getClassID());
                const auto& recognizedNames = objectClass.names.recognized;
 
                // perform case-insensitive comparison
                return ranges::contains(recognizedNames,
                                        simox::alg::to_lower(in.parameters.locationName),
                                        simox::alg::to_lower);
            };
 
            // find all matching objects
            const std::vector<objpose::ObjectPose> matchingObjects =
                objectPoses | ranges::views::filter(isMatchingObject) | ranges::to<std::vector>();
 
            ARMARX_INFO << "Found " << matchingObjects.size() << " objects matching location name '"
                        << in.parameters.locationName << "'.";
 
            const auto locations = services.locationReader->locations();
 
            const auto getMatchingLocation = [&](const objpose::ObjectPose& pose)
                -> std::vector<armarx::navigation::core::Location>
            {
                const std::string locationName = pose.objectID.getClassID().str() + "/inFrontOf";
                const std::string locationNameWithInstance =
                    pose.objectID.getClassID().str() + "/inFrontOf:" + pose.objectID.instanceName();
 
                const auto matchingLocations =
                    core::util::findMatchingLocations(locations, locationName, std::nullopt);
                ARMARX_INFO << "Found " << matchingLocations.size() << " locations matching name "
                            << QUOTED(locationName) << ".";
 
                const auto matchingLocationsWithInstance =
                    core::util::findMatchingLocations(locations,
                                                      locationNameWithInstance,
                                                      std::nullopt);
                ARMARX_INFO << "Found " << matchingLocationsWithInstance.size()
                            << " locations matching name " << QUOTED(locationNameWithInstance)
                            << ".";
 
                const auto appendLocationInstanceName =
                    [&pose](core::Location loc) -> core::Location
                {
                    loc.name += ":" + pose.objectID.instanceName();
                    return loc;
                };
 
                // For those matching locations on class level, append the instance name
                const auto matchingLocationsWithAppendedInstance =
                    matchingLocations | ranges::views::transform(appendLocationInstanceName) |
                    ranges::to<std::vector>();
 
                return ranges::views::concat(matchingLocationsWithAppendedInstance,
                                             matchingLocationsWithInstance) |
                       ranges::to<std::vector>();
            };
 
            // obtain all locations associated with matching objects
            std::vector<armarx::navigation::core::Location> associatedLocations;
            for (const auto& matchingObject : matchingObjects)
            {
                const auto locationsForObject = getMatchingLocation(matchingObject);
                for (const core::Location& loc : locationsForObject)
                {
                    ARMARX_INFO << "Found associated location " << QUOTED(loc.name)
                                << " for matching object " << QUOTED(matchingObject.objectID.str())
                                << ".";
                    associatedLocations.push_back(loc);
                }
            }
 
            ARMARX_INFO << "Found total of " << associatedLocations.size()
                        << " associated locations for matching objects.";
 
            for (const auto& loc : associatedLocations)
            {
                ARMARX_INFO << "- " << loc.name;
            }
 
            if (associatedLocations.empty())
            {
                return std::nullopt;
            }
 
            if (associatedLocations.size() > 1)
            {
                ARMARX_WARNING << "Multiple associated locations found for matching objects. Using "
                                  "the first one.";
            }
 
            const auto& firstLocation = *associatedLocations.begin();
 
            return ResolvedLocation{.name = firstLocation.name, .provider = firstLocation.provider};
        };
 
 
        // Try to match requested location name to a named location in the location graph.
        const auto resolvedLocationFromKnownLocationsFn = [&]() -> std::optional<ResolvedLocation>
        {
            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())
            {
                ARMARX_INFO << "Failed to resolve location name '" << in.parameters.locationName
                            << "'. Reason: " << matchResult.errorMessage.value();
                return std::nullopt;
            }
 
            ARMARX_CHECK(matchResult.resolved.has_value());
            return ResolvedLocation{.name = matchResult.resolved->locationId.entityName,
                                    .provider =
                                        matchResult.resolved->locationId.providerSegmentName};
        };
 
        // Decide which resolution to use
        std::optional<ResolvedLocation> resolvedLocation;
 
        if (in.parameters.useObjectLocations)
        {
            const auto resolvedLocationFromObject = resolvedLocationFromObjectFn();
            if (resolvedLocationFromObject.has_value())
            {
                resolvedLocation = resolvedLocationFromObject;
                ARMARX_INFO << "Resolved location name '" << in.parameters.locationName
                            << "' to location associated with matching object.";
            }
        }
 
        if (not resolvedLocation.has_value())
        {
            const auto resolvedLocationFromKnownLocations = resolvedLocationFromKnownLocationsFn();
 
            if (resolvedLocationFromKnownLocations.has_value())
            {
                resolvedLocation = resolvedLocationFromKnownLocations;
                ARMARX_INFO << "Resolved location name '" << in.parameters.locationName
                            << "' to known location: "
                            << QUOTED(resolvedLocation->provider + "::" + resolvedLocation->name)
                            << ".";
            }
        }
 
        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