From 203afee52a186a25bf4a06df3030342a620fdb93 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Thu, 10 Oct 2024 17:08:56 +0200
Subject: [PATCH] bugfix: grasp sets might have similar names for different
robots: using robot name prefix in local map
---
.../server/KnownGraspProviderSegment.cpp | 20 +++++++++++++++++--
1 file changed, 18 insertions(+), 2 deletions(-)
diff --git a/source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.cpp b/source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.cpp
index a3a55ac0f..233732570 100644
--- a/source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.cpp
+++ b/source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.cpp
@@ -3,6 +3,8 @@
#include <VirtualRobot/XML/ObjectIO.h>
#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
+#include <ArmarXCore/core/exceptions/LocalException.h>
+#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/rapidxml/wrapper/RapidXmlReader.h>
namespace armarx::armem::grasping::segment
@@ -71,12 +73,16 @@ namespace armarx::armem::grasping::segment
retGraspSet.grasps.push_back(retGrasp);
}
- ret.graspSets[retGraspSet.name] = retGraspSet;
+
+ ARMARX_CHECK(ret.graspSets.count(retGraspSet.robot + "/" + retGraspSet.name) == 0)
+ << "The grasp set `" << retGraspSet.robot + "/" + retGraspSet.name << "` was defined twice!";
+
+ ret.graspSets[retGraspSet.robot + "/" + retGraspSet.name] = retGraspSet;
}
return ret;
} catch (...) {
- ARMARX_WARNING << graspFilePath << " is not a manipulation object!";
+ ARMARX_WARNING << graspFilePath << " is not a manipulation object!" << GetHandledExceptionString();
return std::nullopt;
}
}
@@ -109,6 +115,16 @@ namespace armarx::armem::grasping::segment
{
knownGraspCandidate->toAron()
};
+
+ ARMARX_VERBOSE << VAROUT(knownGraspCandidate->graspSets.size());
+ for(const auto& gs : knownGraspCandidate->graspSets)
+ {
+ ARMARX_VERBOSE << VAROUT(gs.second.grasps.size());
+ for(const auto& grasp : gs.second.grasps)
+ {
+ ARMARX_VERBOSE << VAROUT(grasp.name);
+ }
+ }
}
}
ARMARX_INFO << "Loaded " << commit.updates.size() << " grasp candidates from object classes from '" << objectFinder.getPackageName() << "'.";
--
GitLab