XMLSceneImporter.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 MemoryX::XMLSceneImporter
17 * @author (kozlov at kit dot edu)
18 * @date 2012
19 * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
20 * GNU General Public License
21 */
22 
23 #include "XMLSceneImporter.h"
24 
25 #include <filesystem>
26 #include <fstream>
27 
29 
31 
32 namespace fs = std::filesystem;
33 
34 namespace memoryx
35 {
36  void
38  {
39  usingProxy("WorkingMemory");
40  usingProxy("LongtermMemory");
41  }
42 
43  void
45  {
46  ARMARX_INFO << "Starting XMLSceneImporter";
47 
48  memoryPrx = getProxy<WorkingMemoryInterfacePrx>("WorkingMemory");
49  objectInstancesMemoryPrx = memoryPrx->getObjectInstancesSegment();
50 
51  longtermMemoryPrx = getProxy<LongtermMemoryInterfacePrx>("LongtermMemory");
52 
53  const std::string sceneFile = getProperty<std::string>("SceneFile").getValue();
54  const std::string snapshotName = getProperty<std::string>("SnapshotName").getValue();
55  targetUnit = getProperty<LengthUnit>("TargetLengthUnit").getValue();
56 
57  importXMLSnapshot(sceneFile);
58 
59  if (longtermMemoryPrx->saveWorkingMemorySnapshot(snapshotName, memoryPrx))
60  {
61  ARMARX_INFO << "Snapshot " << snapshotName << " saved. Import complete!";
62  }
63  }
64 
65  void
66  XMLSceneImporter::importXMLSnapshot(const std::string& fileName)
67  {
68  fs::path fpath(fileName);
69 
70  if (!fs::exists(fpath))
71  {
72  ARMARX_ERROR << "File " << fileName << " does not exist! Aborting.";
73  return;
74  }
75 
76  std::ifstream infile(fpath.c_str());
77  std::string XMLString((std::istreambuf_iterator<char>(infile)),
78  std::istreambuf_iterator<char>());
79 
80  // parse the specified scene file
82 
83  try
84  {
85  XMLDoc.parse<0>(const_cast<char*>(XMLString.c_str()));
86  }
87  catch (rapidxml::parse_error& e)
88  {
89  ARMARX_ERROR << "Error parsing XML file: " << e.what();
90  return;
91  }
92 
93  // check if it contains a toplevel <scene> tag
94  rapidxml::xml_node<>* rootNode = XMLDoc.first_node("scene");
95 
96  if (!rootNode)
97  {
98  ARMARX_ERROR << "Malformed XML-file: <scene> Tag is missing";
99  return;
100  }
101 
102  // the next child tag must be <nodes>
103  rapidxml::xml_node<>* sceneNodes = rootNode->first_node("nodes");
104 
105  if (!sceneNodes)
106  {
107  ARMARX_ERROR << "Malformed XML-file: <scene> must be followed by a <nodes> tag.";
108  return;
109  }
110 
111  // iterate over all <node> tags and import them into the objectInstance memory
112  for (rapidxml::xml_node<>* entityNode = sceneNodes->first_node("node"); entityNode;
113  entityNode = entityNode->next_sibling("node"))
114  {
115  const ObjectInstanceBasePtr newObjectInstance = createObjectInstanceFromXML(entityNode);
116 
117  if (newObjectInstance)
118  {
119  objectInstancesMemoryPrx->addEntity(newObjectInstance);
120  }
121  }
122 
123  ARMARX_INFO << "File successfully imported: " << fileName;
124  }
125 
126  ObjectInstanceBasePtr
127  XMLSceneImporter::createObjectInstanceFromXML(rapidxml::xml_node<>* xmlNode)
128  {
129  if (!xmlNode)
130  {
131  return ObjectInstancePtr();
132  }
133 
134  const std::string name(xmlNode->first_attribute("name")->value());
135 
136  ObjectInstancePtr objectInstance = new ObjectInstance(name);
137  objectInstance->setExistenceCertainty(1.0f);
138 
139  // assume object class = object name for now
140  objectInstance->setClass(name, 1.0f);
141 
142  // load position
143  armarx::FramedPositionBasePtr entityPosition = positionFromXML(xmlNode);
144 
145  if (!entityPosition)
146  {
147  ARMARX_ERROR << "<position> tag is missing in node " << name;
148  return ObjectInstancePtr();
149  }
150 
151  objectInstance->setPosition(entityPosition);
152 
153  // load rotation
154  armarx::FramedOrientationBasePtr entityOrientation = orientationFromXML(xmlNode);
155 
156  if (entityOrientation)
157  {
158  objectInstance->setOrientation(entityOrientation);
159  }
160 
161  return objectInstance;
162  }
163 
164  armarx::FramedPositionBasePtr
165  XMLSceneImporter::positionFromXML(rapidxml::xml_node<>* xmlNode)
166  {
167  rapidxml::xml_node<>* positionNode = xmlNode->first_node("position");
168 
169  if (!positionNode)
170  {
171  return armarx::FramedPositionPtr();
172  }
173 
174  Eigen::Vector3f position;
175  std::istringstream(positionNode->first_attribute("x")->value()) >> position[0];
176  std::istringstream(positionNode->first_attribute("y")->value()) >> position[1];
177  std::istringstream(positionNode->first_attribute("z")->value()) >> position[2];
178 
179  float scaleFactor = scaleFactorFromPositionXML(positionNode);
180  position *= scaleFactor;
181 
182  return new armarx::FramedPosition(position, armarx::GlobalFrame, "");
183  }
184 
185  armarx::FramedOrientationBasePtr
186  XMLSceneImporter::orientationFromXML(rapidxml::xml_node<>* xmlNode)
187  {
188  rapidxml::xml_node<>* quaternion = xmlNode->first_node("rotation");
189 
190  if (!quaternion)
191  {
193  }
194 
195  Eigen::Quaternionf orientation;
196  std::istringstream(quaternion->first_attribute("qx")->value()) >> orientation.x();
197  std::istringstream(quaternion->first_attribute("qy")->value()) >> orientation.y();
198  std::istringstream(quaternion->first_attribute("qz")->value()) >> orientation.z();
199  std::istringstream(quaternion->first_attribute("qw")->value()) >> orientation.w();
200 
201  return new armarx::FramedOrientation(
202  orientation.toRotationMatrix(), armarx::GlobalFrame, "");
203  }
204 
205  float
206  XMLSceneImporter::scaleFactorFromPositionXML(rapidxml::xml_node<>* positionNode)
207  {
208  std::string unitstring = "METER"; // assume meter if no <unit> tag is available
209 
210  rapidxml::xml_attribute<char>* unitAttribute = positionNode->first_attribute("unit");
211 
212  if (unitAttribute)
213  {
214  unitstring = std::string(unitAttribute->value());
215  }
216 
217  float sourceFactor;
218 
219  if (unitstring == "METER")
220  {
221  sourceFactor = 1.f;
222  }
223 
224  if (unitstring == "CENTIMETER")
225  {
226  sourceFactor = 100.f;
227  }
228  else if (unitstring == "MILLIMETER")
229  {
230  sourceFactor = 1000.f;
231  }
232  else
233  {
234  std::stringstream strstr;
235  strstr << __FILE__ << " : " << __LINE__ << " (" << __FUNCTION__
236  << "): unknown unitstring '" << unitstring << "'";
237  throw std::invalid_argument{strstr.str()};
238  }
239 
240  float targetFactor;
241 
242  switch (targetUnit)
243  {
244  case eMETER:
245  targetFactor = 1.f;
246  break;
247 
248  case eCENTIMETER:
249  targetFactor = 100.f;
250  break;
251 
252  case eMILLIMETER:
253  targetFactor = 1000.f;
254  break;
255  default:
256  std::stringstream strstr;
257  strstr << __FILE__ << " : " << __LINE__ << " (" << __FUNCTION__
258  << "): unknown targetUnit '" << targetUnit << "'";
259  throw std::invalid_argument{strstr.str()};
260  }
261 
262  return targetFactor / sourceFactor;
263  }
264 
265 } // namespace memoryx
rapidxml::parse_error::what
const char * what() const noexcept override
Gets human readable description of error.
Definition: rapidxml.hpp:88
rapidxml::xml_node::next_sibling
xml_node< Ch > * next_sibling(const Ch *name=nullptr, std::size_t name_size=0, bool case_sensitive=true) const
Gets next sibling node, optionally matching node name.
Definition: rapidxml.hpp:1189
JSONObject.h
armarx::GlobalFrame
const std::string GlobalFrame
Definition: FramedPose.h:65
memoryx
VirtualRobot headers.
Definition: CommonPlacesTester.cpp:48
memoryx::eCENTIMETER
@ eCENTIMETER
Definition: XMLSceneImporter.h:39
rapidxml::xml_node::first_attribute
xml_attribute< Ch > * first_attribute(const Ch *name=nullptr, std::size_t name_size=0, bool case_sensitive=true) const
Gets first attribute of node, optionally matching attribute name.
Definition: rapidxml.hpp:1224
memoryx::XMLSceneImporter::onConnectComponent
void onConnectComponent() override
Pure virtual hook for the subclass.
Definition: XMLSceneImporter.cpp:44
memoryx::eMILLIMETER
@ eMILLIMETER
Definition: XMLSceneImporter.h:40
rapidxml::xml_attribute< char >
memoryx::XMLSceneImporter::onInitComponent
void onInitComponent() override
Pure virtual hook for the subclass.
Definition: XMLSceneImporter.cpp:37
XMLSceneImporter.h
memoryx::ObjectInstancePtr
IceInternal::Handle< ObjectInstance > ObjectInstancePtr
Definition: ObjectInstance.h:43
armarx::FramedPositionPtr
IceInternal::Handle< FramedPosition > FramedPositionPtr
Definition: FramedPose.h:149
ARMARX_ERROR
#define ARMARX_ERROR
Definition: Logging.h:196
rapidxml::xml_node::first_node
xml_node< Ch > * first_node(const Ch *name=nullptr, std::size_t name_size=0, bool case_sensitive=true) const
Gets first child node, optionally matching node name.
Definition: rapidxml.hpp:1084
rapidxml::xml_document
This class represents root of the DOM hierarchy.
Definition: rapidxml.hpp:144
rapidxml::xml_node
Class representing a node of XML document.
Definition: rapidxml.hpp:140
armarx::FramedOrientationPtr
IceInternal::Handle< FramedOrientation > FramedOrientationPtr
Definition: FramedPose.h:207
ObjectInstance.h
armarx::VariantType::FramedOrientation
const VariantTypeId FramedOrientation
Definition: FramedPose.h:39
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:181
armarx::Quaternion< float, 0 >
armarx::VariantType::FramedPosition
const VariantTypeId FramedPosition
Definition: FramedPose.h:38
memoryx::eMETER
@ eMETER
Definition: XMLSceneImporter.h:38
rapidxml::xml_document::parse
void parse(Ch *text)
Parses zero-terminated XML string according to given flags.
Definition: rapidxml.hpp:1681
rapidxml::parse_error
Parse error exception.
Definition: rapidxml.hpp:76
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:154