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..28f93084c13d29eb2dd363dbaa03719efaa63e76 100644 --- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.cpp +++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.cpp @@ -14,6 +14,8 @@ namespace armarx::armem::articulated_object { + + armem::articulated_object::ArticulatedObject convert(const VirtualRobot::Robot& obj, const armem::Time& timestamp) { @@ -51,6 +53,21 @@ namespace armarx::armem::articulated_object .timestamp = timestamp}; } + armem::articulated_object::ArticulatedObject + convert2(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::storeArticulatedObject(const VirtualRobot::RobotPtr& articulatedObject, const armem::Time& timestamp, @@ -64,4 +81,20 @@ namespace armarx::armem::articulated_object return store(armemArticulatedObject, isStatic); } + + bool + ArticulatedObjectWriter::storeArticulatedObject2( + const VirtualRobot::RobotPtr& articulatedObject, + const armem::Time& timestamp, + const bool isStatic) + { + + ARMARX_CHECK_NOT_NULL(articulatedObject); + + armarx::armem::articulated_object::ArticulatedObject armemArticulatedObject = + convert2(*articulatedObject, Time::Now()); + + 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..4171529f5733c1294fe94b414b1fcd48492aca1c 100644 --- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.h +++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.h @@ -17,5 +17,12 @@ namespace armarx::armem::articulated_object bool storeArticulatedObject(const VirtualRobot::RobotPtr& articulatedObject, const armem::Time& timestamp, bool isStatic = false); + + bool storeArticulatedObject2(const VirtualRobot::RobotPtr& articulatedObject, + const armem::Time& timestamp, + bool isStatic = false); + + private: + }; } // namespace armarx::armem::articulated_object