LocationNameResolver.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @author     Niklas Arlt ( niklas dot arlt at kit dot edu )
 * @date       2026
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "LocationNameResolver.h"
 
#include <map>
#include <sstream>
#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/util/StringHelpers.h>
 
#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
#include <RobotAPI/libraries/armem_objects/types.h>
 
#include <armarx/navigation/core/location.h>
 
#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
{
    namespace
    {
 
        // Try to match the requested name to an object. If there is a (perfect) match, we check
        // whether there is a location associated with the object (`inFrontOf`).
        std::optional<ResolvedLocation>
        resolveFromObject(const std::string& locationName,
                          const LocationNameResolverServices& services)
        {
            // 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(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 '"
                        << locationName << "'.";
 
            const auto locations = services.locationReader->locations();
 
            const auto getMatchingLocation =
                [&](const objpose::ObjectPose& pose) -> std::vector<core::Location>
            {
                const std::string inFrontOfName = pose.objectID.getClassID().str() + "/inFrontOf";
                const std::string inFrontOfNameWithInstance =
                    pose.objectID.getClassID().str() + "/inFrontOf:" + pose.objectID.instanceName();
 
                const auto matchingLocations =
                    core::util::findMatchingLocations(locations, inFrontOfName, std::nullopt);
                ARMARX_INFO << "Found " << matchingLocations.size() << " locations matching name "
                            << QUOTED(inFrontOfName) << ".";
 
                const auto matchingLocationsWithInstance = core::util::findMatchingLocations(
                    locations, inFrontOfNameWithInstance, std::nullopt);
                ARMARX_INFO << "Found " << matchingLocationsWithInstance.size()
                            << " locations matching name " << QUOTED(inFrontOfNameWithInstance)
                            << ".";
 
                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<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 the requested name to a named location in the location graph.
        std::optional<ResolvedLocation>
        resolveFromKnownLocations(const std::string& locationName,
                                  const LocationNameResolverServices& services,
                                  double minMatchRatio)
        {
            std::stringstream log;
            auto matchResult =
                services.locationReader->resolveLocationName(locationName, 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 '" << 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};
        }
 
    } // namespace
 
    std::optional<ResolvedLocation>
    resolveLocationName(const std::string& locationName,
                        const LocationNameResolverServices& services,
                        const double minMatchRatio,
                        const bool useObjectLocations)
    {
        ARMARX_CHECK_NOT_NULL(services.locationReader);
        ARMARX_CHECK_NOT_NULL(services.objectClassReader);
 
        if (useObjectLocations)
        {
            if (const auto resolved = resolveFromObject(locationName, services))
            {
                ARMARX_INFO << "Resolved location name '" << locationName
                            << "' to location associated with matching object.";
                return resolved;
            }
        }
 
        if (const auto resolved =
                resolveFromKnownLocations(locationName, services, minMatchRatio))
        {
            ARMARX_INFO << "Resolved location name '" << locationName << "' to known location: "
                        << QUOTED(resolved->provider + "::" + resolved->name) << ".";
            return resolved;
        }
 
        return std::nullopt;
    }
 
} // namespace armarx::navigation::skills