RGBDPoseEstimationWithMemoryWriter.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of ArmarX.
3  *
4  * ArmarX is free software; you can redistribute it and/or modify
5  * it under the terms of the GNU General Public License version 2 as
6  * published by the Free Software Foundation.
7  *
8  * ArmarX is distributed in the hope that it will be useful, but
9  * WITHOUT ANY WARRANTY; without even the implied warranty of
10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11  * GNU General Public License for more details.
12  *
13  * You should have received a copy of the GNU General Public License
14  * along with this program. If not, see <http://www.gnu.org/licenses/>.
15  *
16  * @package VisionX::RGBDOpenPoseEstimation
17  * @author Fabian Peller <fabian.peller@kit.edu>
18  * @date 2020
19  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
20  * GNU General Public License
21  */
22 
24 
25 #include <VirtualRobot/Nodes/RobotNode.h>
26 
28 
33 
34 #include <VisionX/libraries/armem_human/aron/HumanPose.aron.generated.h>
37 
38 
39 using namespace visionx;
40 
41 namespace armarx
42 {
43 
44  const std::string RGBDPoseEstimationWithMemoryWriter::defaultName =
45  "RGBDPoseEstimationWithMemoryWriter";
46 
48  RGBDPoseEstimationWithMemoryWriter::createPropertyDefinitions()
49  {
51  new armarx::ComponentPropertyDefinitions{getConfigIdentifier()}};
52 
53  def->required(properties.providerName, "ImageProviderName");
54 
55  def->optional(properties.enableMemory,
56  "humanMemory.enable",
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.");
64 
65  RGBDOpenPoseEstimationComponentPluginUser::postCreatePropertyDefinitions(def);
66  return def;
67  }
68 
69  std::string
70  RGBDPoseEstimationWithMemoryWriter::getDefaultName() const
71  {
72  return RGBDPoseEstimationWithMemoryWriter::defaultName;
73  }
74 
75  std::string
76  RGBDPoseEstimationWithMemoryWriter::GetDefaultName()
77  {
78  return RGBDPoseEstimationWithMemoryWriter::defaultName;
79  }
80 
81  void
82  RGBDPoseEstimationWithMemoryWriter::reportEntities()
83  {
84  if (properties.enableMemory)
85  {
86  armem::Time ts = armem::Time(armem::Duration::MicroSeconds(timestamp_of_update));
87 
89  update.entityID = armem::MemoryID()
90  .withMemoryName(properties.humanMemory.memoryName)
91  .withCoreSegmentName(properties.humanMemory.coreSegmentName)
92  .withProviderSegmentName(getName())
93  .withEntityName("3DDetections")
94  .withTimestamp(ts);
95  update.referencedTime = ts;
96 
97  for (auto& [_, humanPose] : openposeResult3D)
98  {
99  auto pose = humanPose.keypointMap;
100 
101  armarx::human::arondto::HumanPose dto;
102  dto.poseModelId = op_settings.model_pose;
103  // OpenPose does not allow for tracking.
104  dto.humanTrackingId = std::nullopt;
105 
106  for (auto& [name, keypoint] : pose)
107  {
108  armarx::human::arondto::PoseKeypoint kp;
109  kp.confidence = keypoint.confidence;
110 
112  posGlobal.header.agent = localRobot->getName();
113  posGlobal.header.frame = armarx::GlobalFrame;
114  posGlobal.position.x() = keypoint.globalX;
115  posGlobal.position.y() = keypoint.globalY;
116  posGlobal.position.z() = keypoint.globalZ;
117  FramedPosition positionGlobal(
118  posGlobal.position, armarx::GlobalFrame, posGlobal.header.agent);
119  toAron(kp.positionGlobal.emplace(), positionGlobal);
120 
121  FramedPosition pos(Eigen::Vector3f(keypoint.x, keypoint.y, keypoint.z),
122  cameraNodeName,
123  localRobot->getName());
124  armarx::FramedPosition posRobot(pos.toRootFrame(localRobot)->toEigen(),
125  localRobot->getRootNode()->getName(),
126  localRobot->getName());
127  toAron(kp.positionRobot.emplace(), posRobot);
128 
129  dto.keypoints[keypoint.label] = kp;
130  }
131 
132  update.instancesData.push_back(dto.toAron());
133  }
134 
135  const armem::EntityUpdateResult updateResult = humanMemoryWriter.commit(update);
136 
137  if (not updateResult.success)
138  {
139  ARMARX_ERROR << updateResult.errorMessage;
140  }
141  }
142 
143  RGBDOpenPoseEstimationComponentPluginUser::reportEntities();
144  }
145 
146  void
147  RGBDPoseEstimationWithMemoryWriter::onInitImageProcessor()
148  {
149  RGBDOpenPoseEstimationComponentPluginUser::preOnInitImageProcessor();
150  usingImageProvider(properties.providerName);
151 
152  timeoutCounter2d = 0;
153  readErrorCounter2d = 0;
154  sucessCounter2d = 0;
155  }
156 
157  void
158  RGBDPoseEstimationWithMemoryWriter::onConnectImageProcessor()
159  {
160  RGBDOpenPoseEstimationComponentPluginUser::preOnConnectImageProcessor();
161 
162  if (properties.enableMemory)
163  {
164  ARMARX_IMPORTANT << "Waiting for memory ...";
165  try
166  {
167  humanMemoryWriter = memoryNameSystem().useWriter(properties.humanMemory);
168  }
170  {
171  ARMARX_ERROR << e.what();
172  return;
173  }
174  }
175  else
176  {
177  ARMARX_IMPORTANT << "Writing to memroy disabled via properties.";
178  }
179 
180  visionx::ImageType imageDisplayType = visionx::tools::typeNameToImageType("rgb");
181  imageProviderInfo = getImageProvider(properties.providerName, imageDisplayType);
182  rgbImageFormat = imageProviderInfo.imageFormat;
183 
184  numImages = static_cast<unsigned int>(imageProviderInfo.numberImages);
185  if (numImages != 2)
186  {
187  ARMARX_FATAL << "invalid number of images. aborting";
188  return;
189  }
190 
191  imageBuffer = new CByteImage*[2];
192  openposeResultImage = new CByteImage*[1];
193  imageBuffer[0] = visionx::tools::createByteImage(imageProviderInfo);
194  imageBuffer[1] = visionx::tools::createByteImage(imageProviderInfo);
195  rgbImageBuffer = visionx::tools::createByteImage(imageProviderInfo);
196  depthImageBuffer = visionx::tools::createByteImage(imageProviderInfo);
197 
198  enableResultImages(
199  1, imageProviderInfo.imageFormat.dimension, imageProviderInfo.imageFormat.type);
200 
201  ARMARX_INFO << getName() << " connect done";
202 
203  RGBDOpenPoseEstimationComponentPluginUser::postOnConnectImageProcessor();
204  }
205 
206  void
207  RGBDPoseEstimationWithMemoryWriter::onDisconnectImageProcessor()
208  {
209  RGBDOpenPoseEstimationComponentPluginUser::preOnDisconnectImageProcessor();
210 
211  delete[] imageBuffer;
212 
213  RGBDOpenPoseEstimationComponentPluginUser::postOnDisconnectImageProcessor();
214  }
215 
216  void
217  RGBDPoseEstimationWithMemoryWriter::onExitImageProcessor()
218  {
219  }
220 
221  void
222  RGBDPoseEstimationWithMemoryWriter::process()
223  {
224  if (running2D)
225  {
226  // check for result images
227  if (result_image_ready)
228  {
229  std::lock_guard outputImage_lock(openposeResultImageMutex);
230  ARMARX_DEBUG << deactivateSpam() << "publish result openpose image";
231  provideResultImages(openposeResultImage, imageMetaInfo);
232  }
233 
234  // check for new images
235  if (!waitForImages(properties.providerName))
236  {
237  ++timeoutCounter2d;
238  ARMARX_WARNING << "Timeout or error in wait for images"
239  << " (#timeout " << timeoutCounter2d << ", #read error "
240  << readErrorCounter2d << ", #success " << sucessCounter2d << ")";
241  }
242  else
243  {
244  std::lock_guard lock_images(imageBufferMutex);
245  if (static_cast<unsigned int>(getImages(
246  properties.providerName, imageBuffer, imageMetaInfo)) != numImages)
247  {
248  ++readErrorCounter2d;
249  ARMARX_WARNING << "Unable to transfer or read images"
250  << " (#timeout " << timeoutCounter2d << ", #read error "
251  << readErrorCounter2d << ", #success " << sucessCounter2d << ")";
252  return;
253  }
254  else
255  {
256  ARMARX_DEBUG << "Received an Image.";
257  ++sucessCounter2d;
258 
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);
263 
264  timestamp_of_update = imageMetaInfo->timeProvided;
265  update_ready = true;
266  }
267  }
268  }
269  else
270  {
271  ARMARX_DEBUG << deactivateSpam() << "Not running. Wait until start signal comes";
272  usleep(10000);
273  }
274  }
275 } // namespace armarx
ARMARX_IMPORTANT
#define ARMARX_IMPORTANT
Definition: Logging.h:190
visionx
ArmarX headers.
Definition: OpenPoseStressTest.h:38
armarx::armem::MemoryID::withMemoryName
MemoryID withMemoryName(const std::string &name) const
Definition: MemoryID.cpp:401
framed.h
armarx::armem::EntityUpdateResult
Result of an EntityUpdate.
Definition: Commit.h:69
armarx::PropertyDefinitionContainer::required
decltype(auto) required(PropertyType &setter, const std::string &name, const std::string &description="", PropertyDefinitionBase::PropertyConstness constness=PropertyDefinitionBase::eConstant)
Definition: PropertyDefinitionContainer.h:73
armarx::GlobalFrame
const std::string GlobalFrame
Definition: FramedPose.h:65
visionx::tools::createByteImage
CByteImage * createByteImage(const ImageFormatInfo &imageFormat, const ImageType imageType)
Creates a ByteImage for the destination type specified in the given imageProviderInfo.
Clock.h
ARMARX_FATAL
#define ARMARX_FATAL
Definition: Logging.h:199
deactivateSpam
SpamFilterDataPtr deactivateSpam(SpamFilterDataPtr const &spamFilter, float deactivationDurationSec, const std::string &identifier, bool deactivate)
Definition: Logging.cpp:75
armarx::armem::MemoryID::withProviderSegmentName
MemoryID withProviderSegmentName(const std::string &name) const
Definition: MemoryID.cpp:417
FramedPose.h
armarx::armem::MemoryID::withCoreSegmentName
MemoryID withCoreSegmentName(const std::string &name) const
Definition: MemoryID.cpp:409
ARMARX_DEBUG
#define ARMARX_DEBUG
Definition: Logging.h:184
armarx::FramedPosition
The FramedPosition class.
Definition: FramedPose.h:157
error.h
armarx::armem::EntityUpdate
An update of an entity for a specific point in time.
Definition: Commit.h:25
ARMARX_ERROR
#define ARMARX_ERROR
Definition: Logging.h:196
armarx::armem::Time
armarx::core::time::DateTime Time
Definition: forward_declarations.h:13
armarx::armem::server::ltm::util::mongodb::detail::update
bool update(mongocxx::collection &coll, const nlohmann::json &query, const nlohmann::json &update)
Definition: mongodb.cpp:68
armarx::FramedPosition::toRootFrame
FramedPositionPtr toRootFrame(const SharedRobotInterfacePrx &referenceRobot) const
Definition: FramedPose.cpp:902
armarx::core::time::DateTime
Represents a point in time.
Definition: DateTime.h:24
armarx::armem::MemoryID::withEntityName
MemoryID withEntityName(const std::string &name) const
Definition: MemoryID.cpp:425
armarx::ComponentPropertyDefinitions
Default component property definition container.
Definition: Component.h:69
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:181
IceUtil::Handle< class PropertyDefinitionContainer >
visionx::tools::typeNameToImageType
ImageType typeNameToImageType(const std::string &imageTypeName)
Converts an image type name as string into an ImageType integer.
Definition: TypeMapping.cpp:42
ImageUtil.h
armarx::armem::MemoryID::withTimestamp
MemoryID withTimestamp(Time time) const
Definition: MemoryID.cpp:433
armarx::armem::EntityUpdateResult::errorMessage
std::string errorMessage
Definition: Commit.h:76
armarx::armem::error::CouldNotResolveMemoryServer
Indicates that a query to the Memory Name System failed.
Definition: mns.h:24
TypeMapping.h
MemoryNameSystem.h
armarx::toAron
void toAron(arondto::PackagePath &dto, const PackageFileLocation &bo)
armarx::VariantType::FramedPosition
const VariantTypeId FramedPosition
Definition: FramedPose.h:38
ARMARX_WARNING
#define ARMARX_WARNING
Definition: Logging.h:193
armarx::core::time::Duration::MicroSeconds
static Duration MicroSeconds(std::int64_t microSeconds)
Constructs a duration in microseconds.
Definition: Duration.cpp:24
armarx::armem::EntityUpdateResult::success
bool success
Definition: Commit.h:71
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:27
RGBDPoseEstimationWithMemoryWriter.h
armarx::human::MemoryID
const armem::MemoryID MemoryID
Definition: memory_ids.cpp:28