25 #include <VirtualRobot/Nodes/RobotNode.h>
34 #include <VisionX/libraries/armem_human/aron/HumanPose.aron.generated.h>
44 const std::string RGBDPoseEstimationWithMemoryWriter::defaultName =
45 "RGBDPoseEstimationWithMemoryWriter";
48 RGBDPoseEstimationWithMemoryWriter::createPropertyDefinitions()
53 def->
required(properties.providerName,
"ImageProviderName");
55 def->optional(properties.enableMemory,
57 "Enables or disables writing to the human memory.");
58 def->optional(properties.humanMemory.memoryName,
59 "humanMemory.memory_name",
60 "Name of the human memory.");
61 def->optional(properties.humanMemory.coreSegmentName,
62 "humanMemory.core_segment_name",
63 "Name of the core segment in the human memory.");
65 RGBDOpenPoseEstimationComponentPluginUser::postCreatePropertyDefinitions(def);
70 RGBDPoseEstimationWithMemoryWriter::getDefaultName()
const
72 return RGBDPoseEstimationWithMemoryWriter::defaultName;
76 RGBDPoseEstimationWithMemoryWriter::GetDefaultName()
78 return RGBDPoseEstimationWithMemoryWriter::defaultName;
82 RGBDPoseEstimationWithMemoryWriter::reportEntities()
84 if (properties.enableMemory)
95 update.referencedTime = ts;
97 for (
auto& [_, humanPose] : openposeResult3D)
99 auto pose = humanPose.keypointMap;
101 armarx::human::arondto::HumanPose dto;
102 dto.poseModelId = op_settings.model_pose;
104 dto.humanTrackingId = std::nullopt;
106 for (
auto& [name, keypoint] : pose)
108 armarx::human::arondto::PoseKeypoint kp;
109 kp.confidence = keypoint.confidence;
112 posGlobal.header.agent = localRobot->getName();
114 posGlobal.position.x() = keypoint.globalX;
115 posGlobal.position.y() = keypoint.globalY;
116 posGlobal.position.z() = keypoint.globalZ;
119 toAron(kp.positionGlobal.emplace(), positionGlobal);
121 FramedPosition pos(Eigen::Vector3f(keypoint.x, keypoint.y, keypoint.z),
123 localRobot->getName());
125 localRobot->getRootNode()->getName(),
126 localRobot->getName());
127 toAron(kp.positionRobot.emplace(), posRobot);
129 dto.keypoints[keypoint.label] = kp;
132 update.instancesData.push_back(dto.toAron());
143 RGBDOpenPoseEstimationComponentPluginUser::reportEntities();
147 RGBDPoseEstimationWithMemoryWriter::onInitImageProcessor()
149 RGBDOpenPoseEstimationComponentPluginUser::preOnInitImageProcessor();
150 usingImageProvider(properties.providerName);
152 timeoutCounter2d = 0;
153 readErrorCounter2d = 0;
158 RGBDPoseEstimationWithMemoryWriter::onConnectImageProcessor()
160 RGBDOpenPoseEstimationComponentPluginUser::preOnConnectImageProcessor();
162 if (properties.enableMemory)
167 humanMemoryWriter = memoryNameSystem().useWriter(properties.humanMemory);
181 imageProviderInfo = getImageProvider(properties.providerName, imageDisplayType);
182 rgbImageFormat = imageProviderInfo.imageFormat;
184 numImages =
static_cast<unsigned int>(imageProviderInfo.numberImages);
191 imageBuffer =
new CByteImage*[2];
192 openposeResultImage =
new CByteImage*[1];
199 1, imageProviderInfo.imageFormat.dimension, imageProviderInfo.imageFormat.type);
203 RGBDOpenPoseEstimationComponentPluginUser::postOnConnectImageProcessor();
207 RGBDPoseEstimationWithMemoryWriter::onDisconnectImageProcessor()
209 RGBDOpenPoseEstimationComponentPluginUser::preOnDisconnectImageProcessor();
211 delete[] imageBuffer;
213 RGBDOpenPoseEstimationComponentPluginUser::postOnDisconnectImageProcessor();
217 RGBDPoseEstimationWithMemoryWriter::onExitImageProcessor()
222 RGBDPoseEstimationWithMemoryWriter::process()
227 if (result_image_ready)
229 std::lock_guard outputImage_lock(openposeResultImageMutex);
231 provideResultImages(openposeResultImage, imageMetaInfo);
235 if (!waitForImages(properties.providerName))
239 <<
" (#timeout " << timeoutCounter2d <<
", #read error "
240 << readErrorCounter2d <<
", #success " << sucessCounter2d <<
")";
244 std::lock_guard lock_images(imageBufferMutex);
245 if (
static_cast<unsigned int>(getImages(
246 properties.providerName, imageBuffer, imageMetaInfo)) != numImages)
248 ++readErrorCounter2d;
250 <<
" (#timeout " << timeoutCounter2d <<
", #read error "
251 << readErrorCounter2d <<
", #success " << sucessCounter2d <<
")";
259 std::lock_guard lock_rgb(rgbImageBufferMutex);
260 std::lock_guard lock_depth(depthImageBufferMutex);
261 ::ImageProcessor::CopyImage(imageBuffer[0], rgbImageBuffer);
262 ::ImageProcessor::CopyImage(imageBuffer[1], depthImageBuffer);
264 timestamp_of_update = imageMetaInfo->timeProvided;