VisualizationRobot.cpp
Go to the documentation of this file.
1 #include "VisualizationRobot.h"
2 
3 #include <fstream>
4 #include <regex>
5 
6 #include <VirtualRobot/SceneObject.h>
7 #include <VirtualRobot/Visualization/CoinVisualization/CoinVisualization.h>
8 #include <VirtualRobot/XML/RobotIO.h>
9 
13 
14 namespace armarx::viz::coin
15 {
16  namespace
17  {
19  loadRobot(std::string const& project, std::string const& filename)
20  {
22 
23  if (filename.empty())
24  {
25  ARMARX_INFO << deactivateSpam() << "No filename provided for robot.";
26  return nullptr;
27  }
28 
29  std::string fullFilename;
30  // Suppress warnings when full path is specified (is discouraged, but may happen).
31  if (project.empty() && std::filesystem::path(filename).is_absolute())
32  {
34  << "You specified the absolute path to the robot file:"
35  << "\n\t'" << filename << "'"
36  << "\nConsider specifying the containing ArmarX package and relative "
37  "data path instead to "
38  << "improve portability to other systems.";
39  }
40  // We need to always check that the file is readable otherwise, VirtualRobot::RobotIO::loadRobot crashes
42  if (!ArmarXDataPath::SearchReadableFile(filename, fullFilename))
43  {
45  << "Unable to find readable file for name: " << filename;
46  return nullptr;
47  }
48 
49  try
50  {
51  // Always load full model. Visualization can choose between full and collision model
52  VirtualRobot::RobotIO::RobotDescription loadMode = VirtualRobot::RobotIO::eFull;
53  // This check is always true since it does not compare the flag to anything ==> if (true)
54  // Deactivated for now
55  // if (data::ModelDrawStyle::COLLISION)
56  // {
57  // loadMode = VirtualRobot::RobotIO::eCollisionModel;
58  // }
59  ARMARX_INFO << "Loading robot from " << fullFilename;
60  result = VirtualRobot::RobotIO::loadRobot(fullFilename, loadMode);
61  if (result)
62  {
63  result->setThreadsafe(false);
64  result->setPropagatingJointValuesEnabled(true);
65  }
66  else
67  {
68  ARMARX_WARNING << "Could not load robot from file: " << fullFilename
69  << "\nReason: loadRobot() returned nullptr";
70  }
71  }
72  catch (std::exception const& ex)
73  {
74  ARMARX_WARNING << "Could not load robot from file: " << fullFilename
75  << "\nReason: " << ex.what();
76  }
77 
78  return result;
79  }
80 
81  struct RobotInstancePool
82  {
83  std::string project;
84  std::string filename;
85  std::size_t usedInstances = 0;
86  std::vector<VirtualRobot::RobotPtr> robots;
87  };
88 
89  static std::vector<RobotInstancePool> robotCache;
90 
91  LoadedRobot
92  getRobotFromCache(std::string const& project, std::string const& filename)
93  {
94  // We can use a global variable, since this code is only executed in the GUI thread
95 
96  LoadedRobot result;
97  result.project = project;
98  result.filename = filename;
99 
100  for (RobotInstancePool& instancePool : robotCache)
101  {
102  if (instancePool.project == project && instancePool.filename == filename)
103  {
104  // There are two possibilities here:
105  if (instancePool.usedInstances < instancePool.robots.size())
106  {
107  // 1) We have still unused instances in the pool ==> Just return one
108  ARMARX_DEBUG << "Reusing robot instance from cache " << VAROUT(project)
109  << ", " << VAROUT(filename);
110  result.robot = instancePool.robots[instancePool.usedInstances];
111  instancePool.usedInstances += 1;
112  }
113 
114  else
115  {
116  // 2) We do not have unused instances in the pool ==> Clone one
117  ARMARX_DEBUG << "Cloning robot from cache " << VAROUT(project) << ", "
118  << VAROUT(filename);
119 
120  if (instancePool.robots.size() > 0)
121  {
122  VirtualRobot::RobotPtr const& robotToClone =
123  instancePool.robots.front();
124  float scaling = 1.0f;
125  bool preventCloningMeshesIfScalingIs1 = true;
126  result.robot = robotToClone->clone(
127  nullptr, scaling, preventCloningMeshesIfScalingIs1);
128 
129  // Insert the cloned robot into the instance pool
130  instancePool.robots.push_back(result.robot);
131  instancePool.usedInstances += 1;
132  }
133  else
134  {
135  ARMARX_WARNING << "Encountered empty robot instance pool while trying "
136  "to clone new instance"
137  << "\nRobot: " << VAROUT(project) << ", "
138  << VAROUT(filename)
139  << "\nUsed instances: " << instancePool.usedInstances
140  << "\nRobots: " << instancePool.robots.size();
141  }
142  }
143  return result;
144  }
145  }
146 
147  ARMARX_DEBUG << "Loading robot from file " << VAROUT(project) << ", "
148  << VAROUT(filename);
149  result.robot = loadRobot(project, filename);
150  if (result.robot)
151  {
152  RobotInstancePool& instancePool = robotCache.emplace_back();
153  instancePool.project = project;
154  instancePool.filename = filename;
155  instancePool.robots.push_back(result.robot);
156  instancePool.usedInstances = 1;
157  }
158  else
159  {
160  ARMARX_WARNING << deactivateSpam(5) << "Robot " << VAROUT(project) << ", "
161  << VAROUT(filename) << "could not be loaded!";
162  }
163  return result;
164  }
165  } // namespace
166 
168  {
169  for (RobotInstancePool& instancePool : robotCache)
170  {
171  if (instancePool.project == loaded.project && instancePool.filename == loaded.filename)
172  {
173  ARMARX_DEBUG << "Removing robot from chace " << VAROUT(loaded.project) << ", "
174  << VAROUT(loaded.filename);
175  std::vector<VirtualRobot::RobotPtr>& robots = instancePool.robots;
176  auto robotIter = std::find(robots.begin(), robots.end(), loaded.robot);
177  if (robotIter != robots.end())
178  {
179  // Do not erase the robot, but rather move it to the back
180  // We can later reuse the unused instances at the back of the vector
181  std::swap(*robotIter, robots.back());
182  if (instancePool.usedInstances > 0)
183  {
184  instancePool.usedInstances -= 1;
185  }
186  else
187  {
188  ARMARX_WARNING << "Expected there to be at least one used instance "
189  << "while trying to put robot instance back into the pool"
190  << "\nRobot: " << VAROUT(loaded.project) << ", "
192  << "\nUsed instances: " << instancePool.usedInstances;
193  }
194  }
195  }
196  }
197  }
198 
199  bool
201  {
202  IceUtil::Time time_start = IceUtil::Time::now();
203  (void)time_start;
204 
205  bool robotChanged =
206  loaded.project != element.project || loaded.filename != element.filename;
207  if (robotChanged)
208  {
209  // The robot file changed, so reload the robot
210  loaded = getRobotFromCache(element.project, element.filename);
211  }
212  if (!loaded.robot)
213  {
215  << "Robot will not visualized since it could not be loaded."
216  << "\nID: " << element.id << "\nProject: " << element.project
217  << "\nFilename: " << element.filename;
218  return true;
219  }
220  //IceUtil::Time time_load = IceUtil::Time::now();
221 
222  bool drawStyleChanged = loadedDrawStyle != element.drawStyle;
223  if (robotChanged || drawStyleChanged)
224  {
225  recreateVisualizationNodes(element.drawStyle);
226  loadedDrawStyle = element.drawStyle;
227  }
228  //IceUtil::Time time_style = IceUtil::Time::now();
229 
230  // Set pose, configuration and so on
231 
232  VirtualRobot::Robot& robot = *loaded.robot;
233 
234  // We do not need to update the global pose in the robot
235  // Since we only visualize the results, we can simply use the
236  // pose from the Element base class.
237 
238  // Eigen::Matrix4f pose = defrost(element.pose);
239  // robot.setGlobalPose(pose, false);
240 
241  // Check joint values for changes
242 
243  for (const auto& pair : element.jointValues)
244  {
245  std::string const& nodeName = pair.first;
246  float newJointValue = pair.second;
247  VirtualRobot::RobotNodePtr robotNode = robot.getRobotNode(nodeName);
248 
249  if (robotNode == nullptr)
250  {
251  continue;
252  }
253 
254  const float oldJointValue = robotNode->getJointValue();
255  const float diff = std::abs(newJointValue - oldJointValue);
256  const bool jointValuesChanged = diff > 0.001f;
257  if (jointValuesChanged)
258  {
259  // Only set the joint values if they changed
260  // This call causes internal updates to the visualization even if joint angles are the same!
261  robot.setJointValues(element.jointValues);
262  break;
263  }
264  }
265  //IceUtil::Time time_set = IceUtil::Time::now();
266 
267  if (loadedDrawStyle & data::ModelDrawStyle::OVERRIDE_COLOR)
268  {
269  if (loadedColor.r != element.color.r || loadedColor.g != element.color.g ||
270  loadedColor.b != element.color.b || loadedColor.a != element.color.a)
271  {
272  int numChildren = node->getNumChildren();
273  for (int i = 0; i < numChildren; i++)
274  {
275  SoSeparator* nodeSep = static_cast<SoSeparator*>(node->getChild(i));
276  // The first entry must be a SoMaterial (see recreateVisualizationNodes)
277  SoMaterial* m = dynamic_cast<SoMaterial*>(nodeSep->getChild(0));
278  if (!m)
279  {
280  ARMARX_WARNING << "Error at node with index: " << i;
281  continue;
282  }
283 
284  auto color = element.color;
285  const float conv = 1.0f / 255.0f;
286  float a = color.a * conv;
287  SbColor coinColor(conv * color.r, conv * color.g, conv * color.b);
288  m->diffuseColor = coinColor;
289  m->ambientColor = coinColor;
290  m->transparency = 1.0f - a;
291  m->setOverride(true);
292  }
293  loadedColor = element.color;
294  }
295  }
296  //IceUtil::Time time_color = IceUtil::Time::now();
297 
298  // Setting the joint angles takes > 0.5 ms! This is unexpected...
299  // ARMARX_INFO << "Total: " << (time_color - time_start).toMilliSecondsDouble()
300  // << "\nLoad: " << (time_load - time_start).toMilliSecondsDouble()
301  // << "\nStyle: " << (time_style - time_load).toMilliSecondsDouble()
302  // << "\nSet: " << (time_set - time_style).toMilliSecondsDouble()
303  // << "\nColor: " << (time_color - time_set).toMilliSecondsDouble();
304 
305  return true;
306  }
307 
308  void
310  {
311  VirtualRobot::SceneObject::VisualizationType visuType = VirtualRobot::SceneObject::Full;
312  if (drawStyle & data::ModelDrawStyle::COLLISION)
313  {
314  visuType = VirtualRobot::SceneObject::Collision;
315  }
316 
317  node->removeAllChildren();
318 
319  auto& robot = *loaded.robot;
320  for (size_t i = 0; i < robot.getRobotNodes().size(); ++i)
321  {
322  VirtualRobot::RobotNodePtr robNode = robot.getRobotNodes()[i];
323  SoSeparator* nodeSep = new SoSeparator;
324 
325  // This material is used to color the nodes individually
326  // We require it to be the first node in the separator for updates
327  SoMaterial* nodeMat = new SoMaterial;
328  nodeMat->setOverride(false);
329  nodeSep->addChild(nodeMat);
330 
331  auto robNodeVisu = robNode->getVisualization<VirtualRobot::CoinVisualization>(visuType);
332  if (robNodeVisu)
333  {
334  SoNode* sepRobNode = robNodeVisu->getCoinVisualization();
335 
336  if (sepRobNode)
337  {
338  nodeSep->addChild(sepRobNode);
339  }
340  }
341 
342  node->addChild(nodeSep);
343  }
344  }
345 
346  void
348  {
349  robotCache.clear();
350  }
351 } // namespace armarx::viz::coin
armarx::eFull
@ eFull
Definition: MorphingItem.h:35
robots
std::vector< VirtualRobot::RobotPtr > robots
Definition: VisualizationRobot.cpp:86
armarx::ArmarXDataPath::FindPackageAndAddDataPath
static bool FindPackageAndAddDataPath(const std::string &packageName)
Search for the package and add its data path if it was found.
Definition: ArmarXDataPath.cpp:829
armarx::viz::coin::clearRobotCache
void clearRobotCache()
Definition: VisualizationRobot.cpp:347
armarx::viz::coin::VisualizationRobot::~VisualizationRobot
~VisualizationRobot()
Definition: VisualizationRobot.cpp:167
usedInstances
std::size_t usedInstances
Definition: VisualizationRobot.cpp:85
armarx::viz::coin::VisualizationRobot::recreateVisualizationNodes
void recreateVisualizationNodes(int drawStyle)
Definition: VisualizationRobot.cpp:309
project
std::string project
Definition: VisualizationRobot.cpp:83
armarx::viz::coin::VisualizationRobot::loadedDrawStyle
int loadedDrawStyle
Definition: VisualizationRobot.h:29
armarx::armem::client::util::swap
void swap(SubscriptionHandle &first, SubscriptionHandle &second)
Definition: SubscriptionHandle.cpp:66
deactivateSpam
SpamFilterDataPtr deactivateSpam(SpamFilterDataPtr const &spamFilter, float deactivationDurationSec, const std::string &identifier, bool deactivate)
Definition: Logging.cpp:72
armarx::ctrlutil::a
double a(double t, double a0, double j)
Definition: CtrlUtil.h:45
armarx::abs
std::vector< T > abs(const std::vector< T > &v)
Definition: VectorHelpers.h:253
armarx::viz::coin::TypedElementVisualization< SoSeparator >::node
NodeType * node
Definition: ElementVisualizer.h:68
armarx::viz::coin::LoadedRobot::filename
std::string filename
Definition: VisualizationRobot.h:14
armarx::armem::human::Robot
@ Robot
Definition: util.h:14
ARMARX_DEBUG
#define ARMARX_DEBUG
Definition: Logging.h:177
armarx::viz::coin::VisualizationRobot::ElementType
data::ElementRobot ElementType
Definition: VisualizationRobot.h:20
armarx::viz::coin::VisualizationRobot::update
bool update(ElementType const &element)
Definition: VisualizationRobot.cpp:200
filename
std::string filename
Definition: VisualizationRobot.cpp:84
armarx::viz::coin::LoadedRobot::project
std::string project
Definition: VisualizationRobot.h:13
armarx::armem::Time
armarx::core::time::DateTime Time
Definition: forward_declarations.h:13
VisualizationRobot.h
armarx::ArmarXDataPath::SearchReadableFile
static bool SearchReadableFile(const std::string &querryFileName, std::string &resultFileName, bool verbose=true)
Definition: ArmarXDataPath.cpp:218
CMakePackageFinder.h
armarx::viz::coin::VisualizationRobot::loadedColor
armarx::viz::data::Color loadedColor
Definition: VisualizationRobot.h:30
armarx::viz::coin
Definition: ElementVisualizer.cpp:11
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:174
VAROUT
#define VAROUT(x)
Definition: StringHelpers.h:182
armarx::viz::coin::LoadedRobot::robot
VirtualRobot::RobotPtr robot
Definition: VisualizationRobot.h:15
armarx::viz::coin::VisualizationRobot::loaded
LoadedRobot loaded
Definition: VisualizationRobot.h:28
Logging.h
ARMARX_WARNING
#define ARMARX_WARNING
Definition: Logging.h:186
ArmarXDataPath.h
VirtualRobot::RobotPtr
std::shared_ptr< class Robot > RobotPtr
Definition: Bus.h:18