diff --git a/include/mujincontrollerclient/binpickingtask.h b/include/mujincontrollerclient/binpickingtask.h index 231352e9..123ebb2d 100644 --- a/include/mujincontrollerclient/binpickingtask.h +++ b/include/mujincontrollerclient/binpickingtask.h @@ -234,6 +234,12 @@ class MUJINCLIENT_API BinPickingTaskResource : public TaskResource { RuntimeRegistrationInfo(); + struct DetectionPointCloudCorner3D + { + std::array cornerPoint; ///< The point in 3D where the corner is located. + std::array, 2> cornerAxes; ///< Two normalized vectors that describe the axes of the corner in 3D. + }; + struct ObjectInfo { ObjectInfo(); @@ -249,6 +255,8 @@ class MUJINCLIENT_API BinPickingTaskResource : public TaskResource double objectWeight; ///< If non-zero, use this weight fo registration. unit is kg. zero means unknown. uint64_t sensorTimeStampMS; ///< sensor timestamp of the item. If non-zero, then valid. uint64_t updateTimeStampMS; ///< timestamp this request was sent. If non-zero, then valid. + std::vector detectionPointCloudCorners3D; ///< a list of 3D corners in the pointcloud from which the detection was created. + } objectInfo; struct EndEffectorPoseInfo diff --git a/src/binpickingtask.cpp b/src/binpickingtask.cpp index 7c665b73..45c8b829 100644 --- a/src/binpickingtask.cpp +++ b/src/binpickingtask.cpp @@ -624,6 +624,18 @@ void BinPickingTaskResource::ResultGetBinpickingState::Parse(const rapidjson::Va LoadJsonValueByPath(v, "/runtimeRegistrationInfo/objectInfo/quaternion", runtimeRegistrationInfo.objectInfo.quaternion); LoadJsonValueByPath(v, "/runtimeRegistrationInfo/objectInfo/translationInEndEffector", runtimeRegistrationInfo.objectInfo.translationInEndEffector); LoadJsonValueByPath(v, "/runtimeRegistrationInfo/objectInfo/quaternionInEndEffector", runtimeRegistrationInfo.objectInfo.quaternionInEndEffector); + runtimeRegistrationInfo.objectInfo.detectionPointCloudCorners3D.clear(); + const rapidjson::Value *pChild = rapidjson::Pointer("/runtimeRegistrationInfo/objectInfo/detectionPointCloudCorners3D").Get(v); + if (!!pChild && !pChild->IsNull()) { + const rapidjson::Value& child = *pChild; + runtimeRegistrationInfo.objectInfo.detectionPointCloudCorners3D.resize(child.Size()); + for(int idpcc = 0; idpcc < (int)runtimeRegistrationInfo.objectInfo.detectionPointCloudCorners3D.size(); ++idpcc) { + const rapidjson::Value& result = child[idpcc]; + runtimeRegistrationInfo.objectInfo.detectionPointCloudCorners3D[idpcc].cornerPoint = GetJsonValueByKey >(result, "cornerPoint"); + runtimeRegistrationInfo.objectInfo.detectionPointCloudCorners3D[idpcc].cornerAxes = GetJsonValueByKey, 2> >(result, "cornerAxes"); + } + } + runtimeRegistrationInfo.objectInfo.unit = GetJsonValueByPath(v, "/runtimeRegistrationInfo/objectInfo/unit", "mm"); runtimeRegistrationInfo.objectInfo.pickLocationName = GetJsonValueByPath(v, "/runtimeRegistrationInfo/objectInfo/pickLocationName", ""); runtimeRegistrationInfo.objectInfo.registrationLocationName = GetJsonValueByPath(v, "/runtimeRegistrationInfo/objectInfo/registrationLocationName", "");