Component.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::cluttered_scene_generator
17  * @author Patrick Hegemann ( 335495-patrickhegemann at users dot noreply dot gitlab dot com )
18  * @date 2022
19  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
20  * GNU General Public License
21  */
22 
23 #include "Component.h"
24 
27 #include <ArmarXSimulation/interface/simulator/SimulatorInterface.h>
28 
35 
39 
40 #include <SimoxUtility/json/io.h>
41 #include <SimoxUtility/math/pose/pose.h>
42 
43 #include <Eigen/src/Core/Matrix.h>
44 #include <Eigen/src/Geometry/Quaternion.h>
45 
46 #include <algorithm>
47 #include <iterator>
48 
50 {
51  const std::string
52  Component::defaultName = "ClutteredSceneGenerator";
53 
56  {
58 
59  // Use (and depend on) another component (passing the ComponentInterfacePrx).
60  def->component(simulator_);
61  def->component(objectMemoryToSimulation_);
62 
63  // Add a required property. (The component won't start without a value being set.)
64  // def->required(properties.boxLayerName, "p.box.LayerName", "Name of the box layer in ArViz.");
65 
66  def->optional(properties_.out.packageName, "p.out.packageName");
67  def->optional(properties_.out.packagePath, "p.out.packagePath");
68 
69  def->optional(properties_.gen.initialSeed, "p.initialSeed", "Initial random seed for scene generation");
70 
71  def->optional(properties_.gen.nScenes, "p.gen.nScenes", "Number of scenes to generate");
72  def->optional(properties_.gen.nObjects, "p.gen.nObjects", "Number of objects to generate");
73  def->optional(properties_.gen.verticalSpacing, "p.gen.verticalSpacing", "Vertical spacing between objects as they are generated");
74  // def->op
75 
76  return def;
77  }
78 
79  void
81  {
82 
83  }
84 
85  void
87  {
89  // Do things after connecting to topics and components.
90 
91  /* (Requires the armarx::ArVizComponentPluginUser.)
92  // Draw boxes in ArViz.
93  // (Before starting any threads, we don't need to lock mutexes.)
94  drawBoxes(properties, arviz);
95  */
96 
97  /* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
98  // Setup the remote GUI.
99  {
100  createRemoteGuiTab();
101  RemoteGui_startRunningTask();
102  }
103  */
104 
105  scene_generation::ObjectSet objectClasses = getObjectClasses();
106 
107  // TODO(patrick.hegemann): Refactor
109  {
110  .objectSets = {objectClasses},
111  .amountObjects = properties_.gen.nObjects,
112  .objectSpacingZ = properties_.gen.verticalSpacing,
113  .minObjectX = -200,
114  .minObjectY = 800,
115  .maxObjectX = 200,
116  .maxObjectY = 1200,
117  .minObjectZ = 1000,
118  };
119 
120  for (uint i = 0; i < properties_.gen.nScenes; ++i)
121  {
122  simulator_->reInitialize();
123  objectMemoryToSimulation_->synchronizeSimulator();
124 
125  int seed = properties_.gen.initialSeed + static_cast<int>(i);
126  generateScene(config, seed);
127 
128  objects::Scene scene = getScene();
129 
130  if (!properties_.out.packageName.empty() && !properties_.out.packagePath.empty())
131  {
132  armarx::PackagePath path(properties_.out.packageName, properties_.out.packagePath);
133  std::filesystem::path sceneFileName = path.toSystemPath() / ("scene_" + std::to_string(i) + ".json");
134  storeScene(scene, sceneFileName);
135  }
136  }
137  }
138 
139  void
141  {
142  }
143 
144  void
146  {
147  }
148 
149  std::string
151  {
152  return Component::defaultName;
153  }
154 
155  std::string
157  {
158  return Component::defaultName;
159  }
160 
162  {
163  return {packageFileLocation.package, packageFileLocation.relativePath};
164  }
165 
166  scene_generation::ObjectSet Component::getObjectClasses()
167  {
168  ARMARX_TRACE;
169  scene_generation::ObjectSet objectSet;
170 
171  ObjectFinder f;
172  std::vector<ObjectInfo> objectInfos = f.findAllObjectsOfDataset("KIT");
173 
174  auto makeObjectSource = [&](const ObjectInfo& info) {
176  .path = toPackagePath(info.simoxXML()),
178  .objectID = info.id()
179  };
180  };
181 
182  std::transform(objectInfos.begin(), objectInfos.end(),
183  std::back_inserter(objectSet.objects),
184  makeObjectSource);
185 
186  return objectSet;
187  }
188 
189  void Component::generateScene(const scene_generation::ClutteredSceneGenerator::Config& config, const int seed)
190  {
191  ARMARX_TRACE;
192  scene_generation::ClutteredSceneGenerator gen(simulator_, config);
193  gen.generateScene(seed);
194  }
195 
196  objects::Scene Component::getScene()
197  {
198  ARMARX_TRACE;
199 
200  objects::Scene scene;
201  armarx::SceneVisuData sceneData = simulator_->getScene();
202 
203  auto makeSceneObject = [](const armarx::ObjectVisuData& simObject) {
204  const armarx::ObjectID objectID(simObject.name);
205  // const armarx::ObjectID objectID(simObject.project, simObject.objectClassName, simObject.name);
206  Eigen::Matrix4f objectPose {armarx::fromIce(simObject.objectPoses.at(simObject.name))};
207 
208  return objects::SceneObject {
209  .className = objectID.getClassID().str(),
210  .instanceName = objectID.instanceName(),
211  .collection = objectID.dataset(),
212  .position = simox::math::position(objectPose),
213  .orientation = Eigen::Quaternionf(simox::math::orientation(objectPose)),
214  .isStatic = false,
215  .jointValues = {}
216  };
217  };
218 
219  std::transform(sceneData.objects.begin(), sceneData.objects.end(),
220  std::back_inserter(scene.objects),
221  makeSceneObject);
222 
223  return scene;
224  }
225 
226  void Component::storeScene(const objects::Scene& scene, const std::filesystem::path& path)
227  {
228  ARMARX_TRACE;
229 
230  ARMARX_INFO << "Storing scene snapshot at: \n\t" << path;
231  try
232  {
233  simox::json::write(path, scene, 2);
234  }
235  catch (const simox::json::error::JsonError& e)
236  {
237  ARMARX_WARNING << "Storing scene snapshot failed: \n" << e.what();
238  }
239  }
240 
241  /* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
242  void
243  Component::createRemoteGuiTab()
244  {
245  using namespace armarx::RemoteGui::Client;
246 
247  // Setup the widgets.
248 
249  tab.boxLayerName.setValue(properties.boxLayerName);
250 
251  tab.numBoxes.setValue(properties.numBoxes);
252  tab.numBoxes.setRange(0, 100);
253 
254  tab.drawBoxes.setLabel("Draw Boxes");
255 
256  // Setup the layout.
257 
258  GridLayout grid;
259  int row = 0;
260  {
261  grid.add(Label("Box Layer"), {row, 0}).add(tab.boxLayerName, {row, 1});
262  ++row;
263 
264  grid.add(Label("Num Boxes"), {row, 0}).add(tab.numBoxes, {row, 1});
265  ++row;
266 
267  grid.add(tab.drawBoxes, {row, 0}, {2, 1});
268  ++row;
269  }
270 
271  VBoxLayout root = {grid, VSpacer()};
272  RemoteGui_createTab(getName(), root, &tab);
273  }
274 
275 
276  void
277  Component::RemoteGui_update()
278  {
279  if (tab.boxLayerName.hasValueChanged() || tab.numBoxes.hasValueChanged())
280  {
281  std::scoped_lock lock(propertiesMutex);
282  properties.boxLayerName = tab.boxLayerName.getValue();
283  properties.numBoxes = tab.numBoxes.getValue();
284 
285  {
286  setDebugObserverDatafield("numBoxes", properties.numBoxes);
287  setDebugObserverDatafield("boxLayerName", properties.boxLayerName);
288  sendDebugObserverBatch();
289  }
290  }
291  if (tab.drawBoxes.wasClicked())
292  {
293  // Lock shared variables in methods running in seperate threads
294  // and pass them to functions. This way, the called functions do
295  // not need to think about locking.
296  std::scoped_lock lock(propertiesMutex, arvizMutex);
297  drawBoxes(properties, arviz);
298  }
299  }
300  */
301 
302 
303  /* (Requires the armarx::ArVizComponentPluginUser.)
304  void
305  Component::drawBoxes(const Component::Properties& p, viz::Client& arviz)
306  {
307  // Draw something in ArViz (requires the armarx::ArVizComponentPluginUser.
308  // See the ArVizExample in RobotAPI for more examples.
309 
310  viz::Layer layer = arviz.layer(p.boxLayerName);
311  for (int i = 0; i < p.numBoxes; ++i)
312  {
313  layer.add(viz::Box("box_" + std::to_string(i))
314  .position(Eigen::Vector3f(i * 100, 0, 0))
315  .size(20).color(simox::Color::blue()));
316  }
317  arviz.commit(layer);
318  }
319  */
320 
321  // TODO(patrick.hegemann): enable after migration:
322  // ARMARX_REGISTER_COMPONENT_EXECUTABLE(Component, Component::GetDefaultName());
323 
324 } // namespace armarx::simulation::components::cluttered_scene_generator
armarx::simulation::components::cluttered_scene_generator::Component::GetDefaultName
static std::string GetDefaultName()
Get the component's default name.
Definition: Component.cpp:156
Component.h
armarx::ObjectID
A known object ID of the form "Dataset/ClassName" or "Dataset/ClassName/InstanceName".
Definition: ObjectID.h:11
armarx::simulation::scene_generation::ObjectSource
Definition: SimulatedObject.h:44
armarx::simulation::components::cluttered_scene_generator::Component::onExitComponent
void onExitComponent() override
Definition: Component.cpp:145
armarx::simulation::components::cluttered_scene_generator::Component::createPropertyDefinitions
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override
Definition: Component.cpp:55
armarx::simulation::scene_generation::ClutteredSceneGenerator::Config
Definition: ClutteredSceneGenerator.h:52
Pose.h
armarx::simulation::scene_generation::ObjectSet::objects
std::vector< ObjectSource > objects
Definition: SimulatedObject.h:54
armarx::simulation::components::cluttered_scene_generator::Component::getDefaultName
std::string getDefaultName() const override
Definition: Component.cpp:150
armarx::simulation::scene_generation::AsObject
@ AsObject
Definition: SimulatedObject.h:39
armarx::simulation::components::cluttered_scene_generator
Definition: Component.cpp:49
armarx::PackageFileLocation
Definition: ObjectInfo.h:22
armarx::PackageFileLocation::relativePath
std::string relativePath
Relative to the package's data directory.
Definition: ObjectInfo.h:28
Scene.h
armarx::simulation::scene_generation::ObjectSet
Definition: SimulatedObject.h:52
ARMARX_TRACE
#define ARMARX_TRACE
Definition: trace.h:69
armarx::simulation::scene_generation::ObjectSource::path
armarx::PackagePath path
Definition: SimulatedObject.h:46
armarx::ObjectFinder
Used to find objects in the ArmarX objects repository [1] (formerly [2]).
Definition: ObjectFinder.h:23
json_conversions.h
armarx::ObjectFinder::findAllObjectsOfDataset
std::vector< ObjectInfo > findAllObjectsOfDataset(const std::string &dataset, bool checkPaths=true) const
Definition: ObjectFinder.cpp:201
armarx::PackagePath::toSystemPath
static std::filesystem::path toSystemPath(const data::PackagePath &pp)
Definition: PackagePath.cpp:54
ObjectID.h
ObjectInfo.h
Eigen::Quaternionf
Quaternion< float, 0 > Quaternionf
Definition: EigenForwardDeclarations.h:61
SimulatedObject.h
armarx::to_string
const std::string & to_string(const std::string &s)
Definition: StringHelpers.h:40
armarx::fromIce
void fromIce(const std::map< IceKeyT, IceValueT > &iceMap, boost::container::flat_map< CppKeyT, CppValueT > &cppMap)
Definition: ice_conversions_boost_templates.h:26
armarx::objects::Scene
Definition: Scene.h:56
armarx::simulation::components::cluttered_scene_generator::toPackagePath
armarx::PackagePath toPackagePath(const armarx::PackageFileLocation &packageFileLocation)
Definition: Component.cpp:161
armarx::PackageFileLocation::package
std::string package
Name of the ArmarX package.
Definition: ObjectInfo.h:25
ExpressionException.h
armarx::Component::getConfigIdentifier
std::string getConfigIdentifier()
Retrieve config identifier for this component as set in constructor.
Definition: Component.cpp:74
Decoupled.h
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:601
armarx::simulation::components::cluttered_scene_generator::Component::onInitComponent
void onInitComponent() override
Definition: Component.cpp:80
armarx::ComponentPropertyDefinitions
Default component property definition container.
Definition: Component.h:70
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:174
IceUtil::Handle< class PropertyDefinitionContainer >
armarx::simulation::scene_generation::ClutteredSceneGenerator::Config::objectSets
std::vector< ObjectSet > objectSets
Definition: ClutteredSceneGenerator.h:55
armarx::transform
auto transform(const Container< InputT, Alloc > &in, OutputT(*func)(InputT const &)) -> Container< OutputT, typename std::allocator_traits< Alloc >::template rebind_alloc< OutputT > >
Convenience function (with less typing) to transform a container of type InputT into the same contain...
Definition: algorithm.h:315
armarx::simulation::components::cluttered_scene_generator::Component::onDisconnectComponent
void onDisconnectComponent() override
Definition: Component.cpp:140
armarx::simulation::components::cluttered_scene_generator::Component::onConnectComponent
void onConnectComponent() override
Definition: Component.cpp:86
armarx::aron::write
requires data::isWriter< WriterT > void write(WriterT &aron_w, const Eigen::Matrix< EigenT, rows, cols, options > &input, typename WriterT::ReturnType &ret, const armarx::aron::Path &aron_p=armarx::aron::Path())
Definition: eigen.h:130
ClutteredSceneGenerator.h
ARMARX_WARNING
#define ARMARX_WARNING
Definition: Logging.h:186
armarx::PackagePath
Definition: PackagePath.h:55
armarx::ObjectInfo
Accessor for the object files.
Definition: ObjectInfo.h:37
ObjectFinder.h
PackagePath.h