objpose.cpp
Go to the documentation of this file.
1 #include "objpose.h"
2 
4 
5 
7 
10 #include <RobotAPI/libraries/ArmarXObjects/aron/ObjectType.aron.generated.h>
11 #include <RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.aron.generated.h>
12 #include <RobotAPI/libraries/ArmarXObjects/aron/PoseManifoldGaussian.aron.generated.h>
13 
14 
15 void armarx::objpose::fromAron(const arondto::ObjectAttachmentInfo& dto, ObjectAttachmentInfo& bo)
16 {
17  aron::fromAron(dto.frameName, bo.frameName);
18  aron::fromAron(dto.agentName, bo.agentName);
19  aron::fromAron(dto.poseInFrame, bo.poseInFrame);
20 
21 }
22 void armarx::objpose::toAron(arondto::ObjectAttachmentInfo& dto, const ObjectAttachmentInfo& bo)
23 {
24  aron::toAron(dto.frameName, bo.frameName);
25  aron::toAron(dto.agentName, bo.agentName);
26  aron::toAron(dto.poseInFrame, bo.poseInFrame);
27 
28 }
29 
30 
31 void armarx::objpose::fromAron(const arondto::PoseManifoldGaussian& dto, PoseManifoldGaussian& bo)
32 {
33  aron::fromAron(dto.mean, bo.mean);
34  aron::fromAron(dto.covariance, bo.covariance);
35 
36 }
37 void armarx::objpose::toAron(arondto::PoseManifoldGaussian& dto, const PoseManifoldGaussian& bo)
38 {
39  aron::toAron(dto.mean, bo.mean);
40  aron::toAron(dto.covariance, bo.covariance);
41 
42 }
43 
44 
45 void armarx::objpose::fromAron(const arondto::ObjectType& dto, ObjectType& bo)
46 {
47  switch (dto.value)
48  {
49  case arondto::ObjectType::AnyObject:
50  bo = ObjectType::AnyObject;
51  return;
52  case arondto::ObjectType::KnownObject:
53  bo = ObjectType::KnownObject;
54  return;
55  case arondto::ObjectType::UnknownObject:
56  bo = ObjectType::UnknownObject;
57  return;
58  }
59  ARMARX_UNEXPECTED_ENUM_VALUE(arondto::ObjectType, dto.value);
60 }
61 void armarx::objpose::toAron(arondto::ObjectType& dto, const ObjectType& bo)
62 {
63  switch (bo)
64  {
65  case ObjectType::AnyObject:
66  dto.value = arondto::ObjectType::AnyObject;
67  return;
68  case ObjectType::KnownObject:
69  dto.value = arondto::ObjectType::KnownObject;
70  return;
71  case ObjectType::UnknownObject:
72  dto.value = arondto::ObjectType::UnknownObject;
73  return;
74  }
75  ARMARX_UNEXPECTED_ENUM_VALUE(ObjectTypeEnum, bo);
76 }
77 
78 
79 void armarx::objpose::fromAron(const arondto::ObjectPose& dto, ObjectPose& bo)
80 {
81  aron::fromAron(dto.providerName, bo.providerName);
82  fromAron(dto.objectType, bo.objectType);
83  aron::fromAron(dto.isStatic, bo.isStatic);
84  fromAron(dto.objectID, bo.objectID);
85 
86  aron::fromAron(dto.objectPoseRobot, bo.objectPoseRobot);
87  aron::fromAron(dto.objectPoseRobotGaussian, bo.objectPoseRobotGaussian);
88  aron::fromAron(dto.objectPoseGlobal, bo.objectPoseGlobal);
89  aron::fromAron(dto.objectPoseGlobalGaussian, bo.objectPoseGlobalGaussian);
90  aron::fromAron(dto.objectPoseOriginal, bo.objectPoseOriginal);
91  aron::fromAron(dto.objectPoseOriginalFrame, bo.objectPoseOriginalFrame);
92  aron::fromAron(dto.objectPoseOriginalGaussian, bo.objectPoseOriginalGaussian);
93  aron::fromAron(dto.objectJointValues, bo.objectJointValues);
94 
95  aron::fromAron(dto.robotConfig, bo.robotConfig);
96  aron::fromAron(dto.robotPose, bo.robotPose);
97  aron::fromAron(dto.robotName, bo.robotName);
98 
99  if (dto.attachmentValid)
100  {
101  bo.attachment = ObjectAttachmentInfo();
102  fromAron(dto.attachment, *bo.attachment);
103  }
104  else
105  {
106  bo.attachment = std::nullopt;
107  }
108  aron::fromAron(dto.confidence, bo.confidence);
109  aron::fromAron(dto.timestamp, bo.timestamp);
110 
111 
112  if (dto.localOOBBValid)
113  {
114  bo.localOOBB = simox::OrientedBoxf();
115  fromAron(dto.localOOBB, *bo.localOOBB);
116  }
117  else
118  {
119  bo.localOOBB = std::nullopt;
120  }
121 }
122 
123 
124 void armarx::objpose::toAron(arondto::ObjectPose& dto, const ObjectPose& bo)
125 {
126  aron::toAron(dto.providerName, bo.providerName);
127  toAron(dto.objectType, bo.objectType);
128  aron::toAron(dto.isStatic, bo.isStatic);
129  toAron(dto.objectID, bo.objectID);
130 
131  aron::toAron(dto.objectPoseRobot, bo.objectPoseRobot);
132  aron::toAron(dto.objectPoseRobotGaussian, bo.objectPoseRobotGaussian);
133  aron::toAron(dto.objectPoseGlobal, bo.objectPoseGlobal);
134  aron::toAron(dto.objectPoseGlobalGaussian, bo.objectPoseGlobalGaussian);
135  aron::toAron(dto.objectPoseOriginal, bo.objectPoseOriginal);
136  aron::toAron(dto.objectPoseOriginalFrame, bo.objectPoseOriginalFrame);
137  aron::toAron(dto.objectPoseOriginalGaussian, bo.objectPoseOriginalGaussian);
138 
139  aron::toAron(dto.objectJointValues, bo.objectJointValues);
140 
141  aron::toAron(dto.robotConfig, bo.robotConfig);
142  aron::toAron(dto.robotPose, bo.robotPose);
143  aron::toAron(dto.robotName, bo.robotName);
144 
145  if (bo.attachment)
146  {
147  dto.attachmentValid = true;
148  toAron(dto.attachment, *bo.attachment);
149  }
150  else
151  {
152  dto.attachmentValid = false;
153  toAron(dto.attachment, ObjectAttachmentInfo());
154  }
155 
156  aron::toAron(dto.confidence, bo.confidence);
157  aron::toAron(dto.timestamp, bo.timestamp);
158 
159  if (bo.localOOBB)
160  {
161  dto.localOOBBValid = true;
162  toAron(dto.localOOBB, *bo.localOOBB);
163  }
164  else
165  {
166  dto.localOOBBValid = false;
167  toAron(dto.localOOBB, simox::OrientedBoxf());
168  }
169 }
170 
ARMARX_UNEXPECTED_ENUM_VALUE
#define ARMARX_UNEXPECTED_ENUM_VALUE(EnumType, value)
Throw an UnexpectedEnumValueException.
Definition: UnexpectedEnumValueException.h:61
objpose.h
armarx::objpose::PoseManifoldGaussian
A "gaussian" distribution in pose space (i.e.
Definition: PoseManifoldGaussian.h:25
UnexpectedEnumValueException.h
armarx::objpose::toAron
void toAron(arondto::ObjectAttachmentInfo &dto, const ObjectAttachmentInfo &bo)
Definition: objpose.cpp:22
ObjectPose.h
aron_conversions.h
armarx::objpose::fromAron
void fromAron(const arondto::ObjectAttachmentInfo &dto, ObjectAttachmentInfo &bo)
Definition: objpose.cpp:15
armarx::fromAron
void fromAron(const arondto::PackagePath &dto, PackageFileLocation &bo)
armarx::toAron
void toAron(arondto::PackagePath &dto, const PackageFileLocation &bo)
armarx::objpose::ObjectAttachmentInfo
Definition: ObjectPose.h:25
aron_conversions.h
armarx::objpose::ObjectPose
An object pose as stored by the ObjectPoseStorage.
Definition: ObjectPose.h:36
armarx::aron::bo
const std::optional< BoT > & bo
Definition: aron_conversions.h:168