SimulatorToArviz.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 ArmarXSimulation::ArmarXObjects::SimulatorToArviz
17  * @author Peter Reiner ( ufekv at student dot kit dot edu )
18  * @date 2020
19  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
20  * GNU General Public License
21  */
22 
23 #include "SimulatorToArviz.h"
24 #include <VirtualRobot/RuntimeEnvironment.h>
25 
26 
27 namespace armarx
28 {
29 
30 
32  {
34 
35  defs->optional(properties.oldPriorKnowledge.use, "p.oldPriorKnowledge.use");
36  defs->optional(properties.oldPriorKnowledge.name, "p.oldPriorKnowledge.name");
37 
38  return defs;
39  }
40 
41 
43  {
44  }
45 
46 
48  {
49  return "SimulatorToArviz";
50  }
51 
52 
54  {
55  usingTopic("SimulatorListenerInterface");
56  usingTopic("SimulatorVisuUpdates");
57 
58  if (properties.oldPriorKnowledge.use)
59  {
60  usingProxy(properties.oldPriorKnowledge.name);
61  }
62  }
63 
64 
66  {
67  if (properties.oldPriorKnowledge.use)
68  {
69  getProxy(priorKnowledge, properties.oldPriorKnowledge.name);
70  objectClassSegment.initFromProxy(priorKnowledge, {});
71  }
72  }
73 
74 
76  {
77  }
78 
79 
81  {
82  }
83 
84 
85  void SimulatorToArviz::reportSceneUpdated(const SceneVisuData& scene, const ::Ice::Current&)
86  {
87  armarx::viz::Layer floorLayer = arviz.layer("FloorLayer");
88  armarx::viz::Layer robotLayer = arviz.layer("RobotLayer");
89  armarx::viz::Layer objectLayer = arviz.layer("ObjectLayer");
90  handleRobotLayer(scene.robots, robotLayer);
91  handleObjectLayer(scene.objects, objectLayer);
92 
93  if (scene.floor)
94  {
95  ARMARX_DEBUG << "has floor: " << scene.floor << " with filename: " << scene.floorTextureFile;
96  std::string filename = "images/Floor20x20.xml";
97  VirtualRobot::RuntimeEnvironment::getDataFileAbsolute(filename);
98  floorLayer.add(armarx::viz::Object("Floor").file("", filename));
99  }
100 
101  arviz.commit({floorLayer, robotLayer, objectLayer});
102  }
103 
104 
105  void SimulatorToArviz::handleRobotLayer(const RobotVisuList& robotList, armarx::viz::Layer& robotLayer)
106  {
107  for (const auto& e : robotList)
108  {
109  Eigen::Vector3f position(e.pose->position->x,
110  e.pose->position->y,
111  e.pose->position->z);
112  Eigen::Quaternionf orientation(e.pose->orientation->qw,
113  e.pose->orientation->qx,
114  e.pose->orientation->qy,
115  e.pose->orientation->qz);
116  ARMARX_DEBUG << "Drawing robot " << e.name << " using file: " << e.robotFile << " and joint values " << e.jointValues;
117  armarx::viz::Robot robot = armarx::viz::Robot(e.name)
118  .file(e.project, e.robotFile)
119  .useFullModel()
120  .joints(e.jointValues)
121  .useOriginalColor()
122  .scale(e.scaling)
123  .pose(position, orientation);
124 
125  robotLayer.add(robot);
126  }
127  }
128 
129 
130  void SimulatorToArviz::handleObjectLayer(const ObjectVisuList& objectList, armarx::viz::Layer& objectLayer)
131  {
132  for (const ObjectVisuData& data : objectList)
133  {
134  if (data.objectPrimitiveData)
135  {
136  handleObjectPrimitiveData(data, objectLayer);
137  }
138  else if (not data.filename.empty())
139  {
140  handleObjectFromFile(data, objectLayer);
141  }
142  else if (properties.oldPriorKnowledge.use)
143  {
144  handleObjectFromObjectClass(data, objectLayer);
145  }
146  }
147  }
148 
149 
150  void SimulatorToArviz::handleObjectPrimitiveData(const ObjectVisuData& _object, armarx::viz::Layer& objectLayer)
151  {
152  const PoseBasePtr& objectPose = _object.objectPoses.at(_object.name);
153  Eigen::Vector3f position(objectPose->position->x, objectPose->position->y, objectPose->position->z);
154  Eigen::Quaternionf orientation(objectPose->orientation->qw, objectPose->orientation->qx, objectPose->orientation->qy, objectPose->orientation->qz);
155 
156  switch (_object.objectPrimitiveData->type)
157  {
158  case ObjectType::Box:
159  {
160  BoxVisuPrimitivePtr boxObject = BoxVisuPrimitivePtr::dynamicCast(_object.objectPrimitiveData);
161  //armarx::viz::Box box = armarx::viz::Box(_object.name)
162  objectLayer.add(armarx::viz::Box(_object.name)
163  .size(Eigen::Vector3f{boxObject->width, boxObject->height, boxObject->depth})
164  .color(boxObject->color.r, boxObject->color.g, boxObject->color.b)
165  .position(position)
166  .orientation(orientation));
167  }
168  break;
169  case ObjectType::Sphere:
170  {
171  SphereVisuPrimitivePtr sphereObject = SphereVisuPrimitivePtr::dynamicCast(_object.objectPrimitiveData);
172  //armarx::viz::Sphere sphere = armarx::viz::Sphere(_object.name)
173  objectLayer.add(armarx::viz::Sphere(_object.name)
174  .radius(sphereObject->radius)
175  .color(sphereObject->color.r, sphereObject->color.g, sphereObject->color.b)
176  .position(position)
177  .orientation(orientation));
178  }
179  break;
180  case ObjectType::Cylinder:
181  {
182  CylinderVisuPrimitivePtr cylinderObject = CylinderVisuPrimitivePtr::dynamicCast(_object.objectPrimitiveData);
183  //armarx::viz::Cylinder cylinder = armarx::viz::Cylinder(_object.name)
184  objectLayer.add(armarx::viz::Cylinder(_object.name)
185  .radius(cylinderObject->radius)
186  .height(cylinderObject->length)
187  .color(cylinderObject->color.r, cylinderObject->color.g, cylinderObject->color.b)
188  .position(position)
189  .orientation(orientation));
190  }
191  break;
192  }
193  }
194 
195  void SimulatorToArviz::handleObjectFromFile(const ObjectVisuData& _object, armarx::viz::Layer& objectLayer)
196  {
197  const PoseBasePtr& objectPose = _object.objectPoses.at(_object.name);
198 
199  Eigen::Vector3f position(objectPose->position->x,
200  objectPose->position->y,
201  objectPose->position->z);
202  Eigen::Quaternionf orientation(objectPose->orientation->qw,
203  objectPose->orientation->qx,
204  objectPose->orientation->qy,
205  objectPose->orientation->qz);
206 
207  objectLayer.add(armarx::viz::Object(_object.name)
208  .file(_object.project, _object.filename)
209  .position(position)
210  .orientation(orientation));
211  }
212 
213  void SimulatorToArviz::handleObjectFromObjectClass(const ObjectVisuData& _object, armarx::viz::Layer& objectLayer)
214  {
215 
216  const PoseBasePtr& objectPose = _object.objectPoses.at(_object.name);
217 
218  Eigen::Vector3f position(objectPose->position->x,
219  objectPose->position->y,
220  objectPose->position->z);
221  Eigen::Quaternionf orientation(objectPose->orientation->qw,
222  objectPose->orientation->qx,
223  objectPose->orientation->qy,
224  objectPose->orientation->qz);
225  std::string filename = getClassFilename(_object.objectClassName);
226  std::string projectname = "";
227  objectLayer.add(armarx::viz::Object(_object.name)
228  .file(projectname, filename)
229  .position(position)
230  .orientation(orientation));
231 
232  }
233 
234  std::string SimulatorToArviz::getClassFilename(const std::string& className)
235  {
236  auto it = classFilenameCache.find(className);
237  if (it != classFilenameCache.end())
238  {
239  return it->second;
240  }
241  else
242  {
243  std::optional<memoryx::ObjectClassWrapper> objectClass = objectClassSegment.getClass(className);
244  if (!objectClass)
245  {
246  return "";
247  }
249  objectClass->classInMemory->getWrapper<memoryx::EntityWrappers::SimoxObjectWrapper>();
250  std::string filename = wrapper->getManipulationObjectFileName();
251 
252  classFilenameCache[className] = filename;
253 
254  return filename;
255  }
256  }
257 }
armarx::viz::Client::commit
CommitResult commit(StagedCommit const &commit)
Definition: Client.cpp:80
armarx::viz::Cylinder::radius
Cylinder & radius(float r)
Definition: Elements.h:78
armarx::viz::Object::file
Object & file(std::string const &project, std::string const &filename)
Definition: Elements.h:328
memoryx::ObjectClassSegmentWrapper::getClass
std::optional< ObjectClassWrapper > getClass(std::string const &className) const
Definition: ObjectClassSegmentWrapper.cpp:62
armarx::viz::Cylinder::height
Cylinder & height(float h)
Definition: Elements.h:85
armarx::viz::Layer::add
void add(ElementT const &element)
Definition: Layer.h:29
armarx::viz::Sphere
Definition: Elements.h:134
armarx::viz::ElementOps::position
DerivedT & position(float x, float y, float z)
Definition: ElementOps.h:127
IceInternal::Handle< SimoxObjectWrapper >
armarx::viz::Object
Definition: Elements.h:321
armarx::SimulatorToArviz::createPropertyDefinitions
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override
Definition: SimulatorToArviz.cpp:31
armarx::SimulatorToArviz::onDisconnectComponent
void onDisconnectComponent() override
Definition: SimulatorToArviz.cpp:75
armarx::armem::human::Robot
@ Robot
Definition: util.h:14
data
uint8_t data[1]
Definition: EtherCATFrame.h:68
memoryx::ObjectClassSegmentWrapper::initFromProxy
void initFromProxy(memoryx::PriorKnowledgeInterfacePrx const &priorKnowledge, const std::vector< std::string > &datasets)
Definition: ObjectClassSegmentWrapper.cpp:16
ARMARX_DEBUG
#define ARMARX_DEBUG
Definition: Logging.h:177
armarx::viz::Cylinder
Definition: Elements.h:74
armarx::viz::Sphere::radius
Sphere & radius(float r)
Definition: Elements.h:138
armarx::viz::ElementOps::orientation
DerivedT & orientation(Eigen::Quaternionf const &ori)
Definition: ElementOps.h:140
armarx::viz::Box
Definition: Elements.h:51
filename
std::string filename
Definition: VisualizationRobot.cpp:83
armarx::viz::Robot
Definition: Robot.h:10
Eigen::Quaternionf
Quaternion< float, 0 > Quaternionf
Definition: EigenForwardDeclarations.h:61
memoryx::EntityWrappers::SimoxObjectWrapper
SimoxObjectWrapper offers a simplified access to the Simox ManipulationObject (i.e visualization,...
Definition: SimoxObjectWrapper.h:46
armarx::ManagedIceObject::usingTopic
void usingTopic(const std::string &name, bool orderedPublishing=false)
Registers a proxy for subscription after initialization.
Definition: ManagedIceObject.cpp:248
armarx::Component::getConfigIdentifier
std::string getConfigIdentifier()
Retrieve config identifier for this component as set in constructor.
Definition: Component.cpp:74
armarx::ComponentPropertyDefinitions
Default component property definition container.
Definition: Component.h:70
armarx::ArVizComponentPluginUser::arviz
armarx::viz::Client arviz
Definition: ArVizComponentPlugin.h:43
armarx::SimulatorToArviz::reportSceneUpdated
void reportSceneUpdated(const SceneVisuData &scene, const ::Ice::Current &=::Ice::emptyCurrent) override
Definition: SimulatorToArviz.cpp:85
armarx::Quaternion< float, 0 >
IceUtil::Handle< class PropertyDefinitionContainer >
armarx::viz::Box::size
Box & size(Eigen::Vector3f const &s)
Definition: Elements.h:55
armarx::viz::ElementOps::color
DerivedT & color(Color color)
Definition: ElementOps.h:195
armarx::SimulatorToArviz::onConnectComponent
void onConnectComponent() override
Definition: SimulatorToArviz.cpp:65
SimulatorToArviz.h
armarx::SimulatorToArviz::onExitComponent
void onExitComponent() override
Definition: SimulatorToArviz.cpp:80
armarx::SimulatorToArviz::getDefaultName
std::string getDefaultName() const override
Definition: SimulatorToArviz.cpp:47
armarx::SimulatorToArviz::onInitComponent
void onInitComponent() override
Definition: SimulatorToArviz.cpp:53
armarx::viz::Client::layer
Layer layer(std::string const &name) const
Definition: Client.cpp:73
armarx::ManagedIceObject::getProxy
Ice::ObjectPrx getProxy(long timeoutMs=0, bool waitForScheduler=true) const
Returns the proxy of this object (optionally it waits for the proxy)
Definition: ManagedIceObject.cpp:393
armarx::viz::Layer
Definition: Layer.h:12
armarx::SimulatorToArviz::SimulatorToArviz
SimulatorToArviz()
Definition: SimulatorToArviz.cpp:42
armarx::ManagedIceObject::usingProxy
bool usingProxy(const std::string &name, const std::string &endpoints="")
Registers a proxy for retrieval after initialization and adds it to the dependency list.
Definition: ManagedIceObject.cpp:151
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:28