diff --git a/source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.cpp b/source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.cpp index a3a55ac0fafa59c5b0997dc08b4ac2065b616a15..233732570d5c43ef5e1ff9925b942dc702a34edc 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() << "'.";