diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.cpp index fdb948721af1d837b83285170a6c943d6024eb4b..9e03b50c03ad434319bfbddf41e680c46f6b6f7d 100644 --- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.cpp +++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.cpp @@ -14,11 +14,14 @@ namespace armarx::armem::articulated_object { - armem::articulated_object::ArticulatedObject + + // Slow version with filesystem access. It runs CMake for each project / environment variable. + static armem::articulated_object::ArticulatedObject convert(const VirtualRobot::Robot& obj, const armem::Time& timestamp) { ARMARX_DEBUG << "Filename is " << obj.getFilename(); + // this is very inefficient. It runs CMake for each project const std::vector<std::string> packages = armarx::CMakePackageFinder::FindAllArmarXSourcePackages(); const std::string package = armarx::ArmarXDataPath::getProject(packages, obj.getFilename()); @@ -51,6 +54,37 @@ namespace armarx::armem::articulated_object .timestamp = timestamp}; } + // fast version without filesystem (CMake) access + static armem::articulated_object::ArticulatedObject + convertWithoutPackagePath(const VirtualRobot::Robot& obj, const armem::Time& timestamp) + { + ARMARX_DEBUG << "Filename is " << obj.getFilename(); + + return armem::articulated_object::ArticulatedObject{ + .description = {.name = obj.getType(), .xml = {}, .visualization = {}, .info = {}}, + .instance = obj.getName(), + .config = {.timestamp = timestamp, + .globalPose = Eigen::Isometry3f(obj.getRootNode()->getGlobalPose()), + .jointMap = obj.getJointValues(), + .proprioception = std::nullopt}, + .timestamp = timestamp}; + } + + bool + ArticulatedObjectWriter::storeArticulatedObjectWithObjectClass( + const VirtualRobot::RobotPtr& articulatedObject, + const armem::Time& timestamp, + const bool isStatic) + { + + ARMARX_CHECK_NOT_NULL(articulatedObject); + + armarx::armem::articulated_object::ArticulatedObject armemArticulatedObject = + convert(*articulatedObject, timestamp); + + return store(armemArticulatedObject, isStatic); + } + bool ArticulatedObjectWriter::storeArticulatedObject(const VirtualRobot::RobotPtr& articulatedObject, const armem::Time& timestamp, @@ -60,8 +94,9 @@ namespace armarx::armem::articulated_object ARMARX_CHECK_NOT_NULL(articulatedObject); armarx::armem::articulated_object::ArticulatedObject armemArticulatedObject = - convert(*articulatedObject, Time::Now()); + convertWithoutPackagePath(*articulatedObject, timestamp); - return store(armemArticulatedObject, isStatic); + return storeInstance(armemArticulatedObject, isStatic); } + } // namespace armarx::armem::articulated_object diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.h b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.h index edc401b6e0d55487f9446c1b5d0129cd623ea5c1..4d6fca627cc7a6c234c7f75b4688d0b56a145708 100644 --- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.h +++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.h @@ -14,8 +14,22 @@ namespace armarx::armem::articulated_object public: using Writer::Writer; + /** + * @brief Stores the articulated object in the memory. + * + */ bool storeArticulatedObject(const VirtualRobot::RobotPtr& articulatedObject, const armem::Time& timestamp, bool isStatic = false); + + /** + * @brief Stores the articulated object. If the object class in unknown, + * it will also be stored in the corresponding class core segment. + */ + bool storeArticulatedObjectWithObjectClass(const VirtualRobot::RobotPtr& articulatedObject, + const armem::Time& timestamp, + bool isStatic = false); + + private: }; } // namespace armarx::armem::articulated_object