Refactor, add minimal match ratio parameter

source/armarx/navigation/skills/NavigateToNamedLocation.cpp

@@ -16,7 +16,6 @@ namespace armarx::navigation::skills
     NavigateToNamedLocation::GetSkillDescription()
     {
         ParamType defaultParameters;
-        defaultParameters.locationName = "table";
 
         defaultParameters.navigateToLocation.fromAron(
             NavigateToLocation::DefaultSkillDescription().rootProfileDefaults);
@@ -55,15 +54,16 @@ namespace armarx::navigation::skills
     {
         ARMARX_TRACE;
 
-        std::string graspLocationName = in.parameters.locationName;
         // Find location
 
-        armarx::armem::MemoryID const* locationID;
+        // Capture this for logging.
+        auto resolveLocationName = [this](armarx::armem::client::Reader& locationReader,
+                                          const std::string& locationName,
+                                          double minMatchRatio) -> armarx::armem::MemoryID
         {
             // Get locations from memory.
             armarx::armem::client::QueryResult result =
-                services.locationReader.getLatestSnapshotsIn(
-                    armarx::navigation::location::coreSegmentID);
+                locationReader.getLatestSnapshotsIn(armarx::navigation::location::coreSegmentID);
             if (not result.success)
             {
                 std::stringstream msg;
@@ -86,12 +86,34 @@ namespace armarx::navigation::skills
                     locationIdsToWords[instance.id()].push_back(instance.id().entityName);
                 });
 
-            std::stringstream log;
-            locationID = simox::alg::fuzzy_multi_match(graspLocationName, locationIdsToWords, log);
+            if (locationIdsToWords.empty())
+            {
+                std::stringstream msg;
+                msg << "Found no locations in core segment "
+                    << armarx::navigation::location::coreSegmentID << " the name '" << locationName
+                    << "' could be matched against.";
+                throw armarx::skills::error::SkillFailedException(__PRETTY_FUNCTION__, msg.str());
+            }
 
+            std::stringstream log;
+            auto match = simox::alg::fuzzy_multi_match(locationName, locationIdsToWords, log);
             ARMARX_INFO << log.str();
-        }
-        ARMARX_CHECK_NOT_NULL(locationID);
+            ARMARX_CHECK(match.has_value());
+
+            if (match->ratio < minMatchRatio)
+            {
+                std::stringstream msg;
+                msg << "Match ratio " << match->ratio << " of best match '" << *match->word
+                    << "' (location ID " << match->option
+                    << ") is below the requested minimum match ratio (" << minMatchRatio << ").";
+                throw armarx::skills::error::SkillFailedException(__PRETTY_FUNCTION__, msg.str());
+            }
+
+            return *match->option;
+        };
+
+        const armarx::armem::MemoryID locationID = resolveLocationName(
+            services.locationReader, in.parameters.locationName, in.parameters.minMatchRatio);
 
         // Navigate to location
         {
@@ -101,8 +123,7 @@ namespace armarx::navigation::skills
                 manager, properties.subSkillIDs.navigateToLocation);
             Parameters subSkillParams = in.parameters.navigateToLocation;
 
-            subSkillParams.location =
-                locationID->providerSegmentName + "/" + locationID->entityName;
+            subSkillParams.location = locationID.providerSegmentName + "/" + locationID.entityName;
 
             auto update = callSubskill(subSkill, subSkillParams.toAron());
 
@@ -112,8 +133,8 @@ namespace armarx::navigation::skills
             }
         }
 
+        // TODO: Return resolved location ID.
         return MakeSucceededResult();
     }
 
-
 } // namespace armarx::navigation::skills

source/armarx/navigation/skills/aron/NavigateToNamedLocation.xml

@@ -8,7 +8,11 @@
         <Object name='armarx::navigation::skills::arondto::NavigateToNamedLocationParams'>
 
             <ObjectChild key='locationName'>
-                <string />
+                <string default="table" />
+            </ObjectChild>
+
+            <ObjectChild key='minMatchRatio'>
+                <float64 default="0.5" />
             </ObjectChild>
 
             <ObjectChild key='navigateToLocation'>