Visu.cpp
Go to the documentation of this file.
1 #include "Visu.h"
2 
3 #include <algorithm>
4 #include <exception>
5 #include <string>
6 
7 #include <Eigen/Geometry>
8 
9 #include <SimoxUtility/algorithm/get_map_keys_values.h>
10 #include <SimoxUtility/algorithm/string/string_tools.h>
11 #include <SimoxUtility/math/pose.h>
12 #include <SimoxUtility/math/rescale.h>
13 #include <VirtualRobot/Nodes/Sensor.h>
14 #include <VirtualRobot/XML/RobotIO.h>
15 
16 #include <ArmarXCore/core/logging/Logging.h>
17 #include <ArmarXCore/core/time/CycleUtil.h>
18 #include <ArmarXCore/core/time/TimeUtil.h>
19 #include <ArmarXCore/interface/core/PackagePath.h>
20 
21 #include <RobotAPI/components/ArViz/Client/Elements.h>
22 #include <RobotAPI/libraries/armem/core/Time.h>
23 #include <RobotAPI/libraries/armem_robot_state/server/description/Segment.h>
24 #include <RobotAPI/libraries/armem_robot_state/server/localization/Segment.h>
25 #include <RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h>
26 #include <RobotAPI/libraries/armem_robot_state/server/proprioception/SensorValues.h>
27 
28 #include "combine.h"
29 
30 namespace armarx::armem::server::robot_state
31 {
32 
33  Visu::Visu(const description::Segment& descriptionSegment,
34  const proprioception::Segment& proprioceptionSegment,
35  const localization::Segment& localizationSegment) :
36  descriptionSegment(descriptionSegment),
37  proprioceptionSegment(proprioceptionSegment),
38  localizationSegment(localizationSegment)
39  {
40  Logging::setTag("Visu");
41  }
42 
43  void
44  Visu::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
45  {
46  defs->optional(
47  p.enabled, prefix + "enabled", "Enable or disable visualization of objects.");
48  defs->optional(p.frequencyHz, prefix + "frequenzyHz", "Frequency of visualization.");
49  defs->optional(p.framesEnabled,
50  prefix + "framesEnabled",
51  "Enable or disable visualization of frames.");
52  defs->optional(p.forceTorque.enabled,
53  prefix + "forceTorque.enabled",
54  "Enable or disable visualization of force torque sensors.");
55  defs->optional(p.forceTorque.forceScale,
56  prefix + "forceTorque.forceScale",
57  "Scaling of force arrows.");
58  defs->optional(p.collisionModelEnabled,
59  prefix + "collisionModelEnabled",
60  "Enable visualization of collision model.");
61  }
62 
63  void
64  Visu::init()
65  {
66  }
67 
68  void
69  Visu::connect(const viz::Client& arviz, DebugObserverInterfacePrx debugObserver)
70  {
71  this->arviz = arviz;
72  if (debugObserver)
73  {
74  bool batchMode = true;
75  this->debugObserver = DebugObserverHelper("RobotStateMemory", debugObserver, batchMode);
76  }
77 
78  if (updateTask)
79  {
80  updateTask->stop();
81  updateTask->join();
82  updateTask = nullptr;
83  }
84  updateTask = new SimpleRunningTask<>([this]() { this->visualizeRun(); });
85  updateTask->start();
86  }
87 
88  void
89  Visu::visualizeRobots(viz::Layer& layer,
90  const armem::robot_state::Robots& robots,
91  bool useColModel)
92  {
93  for (const armem::robot_state::Robot& robot : robots)
94  {
95  const armarx::data::PackagePath xmlPath = robot.description.xml.serialize();
96 
97  const auto* robotModel = getRobotModel(robot.description);
98 
99  std::map<std::string, float> jointMap = robot.config.jointMap;
100 
101  // we override the jointMap with the joint values from the robot model if it has not been set explicitly
102  if (robotModel != nullptr)
103  {
104  for (const auto& [jointName, jointValue] : robotModel->model->getJointValues())
105  {
106  if (jointMap.count(jointName) == 0)
107  {
108  jointMap[jointName] = jointValue;
109  }
110  }
111  }
112 
113  // clang-format off
114  viz::Robot robotVisu = viz::Robot(robot.description.name)
115  .file(xmlPath.package, xmlPath.package + "/" + xmlPath.path)
116  .joints(jointMap)
117  .pose(robot.config.globalPose);
118 
119  if (not useColModel)
120  {
121  robotVisu.useFullModel();
122  }
123  else
124  {
125  robotVisu.useCollisionModel();
126  }
127  // clang-format on
128 
129  layer.add(robotVisu);
130  }
131  }
132 
133  void
134  Visu::visualizeFrames(
135  viz::Layer& layerFrames,
136  const std::unordered_map<std::string, std::vector<Eigen::Isometry3f>>& frames)
137  {
138  for (const auto& [robotName, robotFrames] : frames)
139  {
140  Eigen::Isometry3f pose = Eigen::Isometry3f::Identity();
141 
142  int i = 0;
143  for (const auto& frame : robotFrames)
144  {
145  Eigen::Isometry3f from = pose;
146  Eigen::Isometry3f to = pose * frame;
147 
148  pose = to;
149 
150  layerFrames.add(viz::Arrow(robotName + std::to_string(++i))
151  .fromTo(from.translation(), to.translation()));
152  }
153  }
154  }
155 
156  void
157  Visu::visualizeFramesIndividual(
158  viz::Layer& layerFrames,
159  const std::unordered_map<std::string, std::vector<Eigen::Isometry3f>>& frames)
160  {
161 
162  std::vector<std::string> FRAME_NAMES{/*world*/ "map", "odom", "robot"};
163 
164  for (const auto& [robotName, robotFrames] : frames)
165  {
166  int i = 0;
167  for (const auto& frame : robotFrames)
168  {
169  layerFrames.add(
170  viz::Pose(robotName + FRAME_NAMES.at(i++)).pose(frame.matrix()).scale(3));
171  }
172  }
173  }
174 
175  void
176  Visu::visualizeRun()
177  {
178  CycleUtil cycle(static_cast<int>(1000 / p.frequencyHz));
179  while (updateTask and not updateTask->isStopped())
180  {
181  if (p.enabled)
182  {
183  const Time timestamp = Time::Now();
184  ARMARX_DEBUG << "Visu task at " << armem::toStringMilliSeconds(timestamp);
185 
186  try
187  {
188  visualizeOnce(timestamp);
189  }
190  catch (const std::out_of_range& e)
191  {
192  ARMARX_WARNING << "Caught std::out_of_range while visualizing robots: \n"
193  << e.what();
194  }
195  catch (const std::exception& e)
196  {
197  ARMARX_WARNING << "Caught exception while visualizing robots: \n" << e.what();
198  }
199  catch (...)
200  {
201  ARMARX_WARNING << "Caught unknown exception while visualizing robots.";
202  }
203 
204  if (debugObserver.has_value())
205  {
206  debugObserver->sendDebugObserverBatch();
207  }
208  }
209  cycle.waitForCycleDuration();
210  }
211  }
212 
213  void
214  Visu::visualizeOnce(const Time& timestamp)
215  {
216  TIMING_START(tVisuTotal);
217 
218  // TODO(fabian.reister): use timestamp
219 
220  // Get data.
221  TIMING_START(tVisuGetData);
222 
223  TIMING_START(tRobotDescriptions);
224  const description::RobotDescriptionMap robotDescriptions =
225  descriptionSegment.getRobotDescriptionsLocking(timestamp);
226  TIMING_END_STREAM(tRobotDescriptions, ARMARX_DEBUG);
227 
228  TIMING_START(tGlobalPoses);
229  const auto globalPoses = localizationSegment.getRobotGlobalPosesLocking(timestamp);
230  TIMING_END_STREAM(tGlobalPoses, ARMARX_DEBUG);
231 
232  TIMING_START(tRobotFramePoses);
233  const auto frames = localizationSegment.getRobotFramePosesLocking(timestamp);
234  TIMING_END_STREAM(tRobotFramePoses, ARMARX_DEBUG);
235 
236  TIMING_START(tSensorValues);
237  const proprioception::SensorValuesMap sensorValues =
238  proprioceptionSegment.getSensorValuesLocking(timestamp,
239  debugObserver ? &*debugObserver : nullptr);
240  TIMING_END_STREAM(tSensorValues, ARMARX_DEBUG);
241 
242  TIMING_END_STREAM(tVisuGetData, ARMARX_DEBUG);
243 
244 
245  // Build layers.
246  TIMING_START(tVisuBuildLayers);
247 
248  // We need all 3 information:
249  // - robot description
250  // - global pose
251  // - joint positions
252  // => this is nothing but an armem::Robot
253  ARMARX_DEBUG << "Combining robot ..."
254  << "\n- " << robotDescriptions.size() << " descriptions"
255  << "\n- " << globalPoses.size() << " global poses"
256  << "\n- " << sensorValues.size() << " joint positions";
257 
258  const armem::robot_state::Robots robots =
259  combine(robotDescriptions, globalPoses, sensorValues, timestamp);
260 
261  ARMARX_DEBUG << "Visualize " << robots.size() << " robots ...";
262  std::vector<viz::Layer> layers;
263 
264  {
265  viz::Layer& layer = layers.emplace_back(arviz.layer("Robots"));
266  visualizeRobots(layer, robots, false);
267  }
268 
269  if (p.collisionModelEnabled)
270  {
271  viz::Layer& layer = layers.emplace_back(arviz.layer("RobotsCol"));
272  visualizeRobots(layer, robots, true);
273  }
274 
275  ARMARX_DEBUG << "Visualize frames ...";
276  if (p.framesEnabled)
277  {
278  viz::Layer& layer = layers.emplace_back(arviz.layer("Localization Frames"));
279  visualizeFrames(layer, frames);
280  }
281 
282  TIMING_END_STREAM(tVisuBuildLayers, ARMARX_DEBUG);
283 
284  {
285  viz::Layer& layer = layers.emplace_back(arviz.layer("Localization Frames Individual"));
286  visualizeFramesIndividual(layer, frames);
287  }
288 
289  if (p.forceTorque.enabled)
290  {
291  viz::Layer& layerForceTorque = layers.emplace_back(arviz.layer("Force Torque"));
292  std::stringstream log;
293 
294  for (const armem::robot_state::Robot& robot : robots)
295  {
296  const std::string& robotName = robot.description.name;
297 
298  auto itSensorValues = sensorValues.find(robotName);
299  if (itSensorValues == sensorValues.end())
300  {
301  log << "\nNo robot '" << robotName << "' in sensor values. Available are: ";
302  log << simox::alg::join(simox::alg::get_keys(sensorValues), ", ");
303  continue;
304  }
305 
306  visualizeForceTorque(layerForceTorque, robot, itSensorValues->second);
307  }
308 
309  if (not log.str().empty())
310  {
311  ARMARX_INFO << "While visualizing robot force torques: " << log.str();
312  }
313  }
314 
315 
316  // Commit layers.
317 
318  ARMARX_DEBUG << "Commit visualization ...";
319  TIMING_START(tVisuCommit);
320  arviz.commit(layers);
321  TIMING_END_STREAM(tVisuCommit, ARMARX_DEBUG);
322 
323  TIMING_END_STREAM(tVisuTotal, ARMARX_DEBUG);
324 
325  if (debugObserver.has_value())
326  {
327  const std::string p = "Visu | ";
328  debugObserver->setDebugObserverDatafield(p + "t Total (ms)",
329  tVisuTotal.toMilliSecondsDouble());
330  debugObserver->setDebugObserverDatafield(p + "t 1 Get Data (ms)",
331  tVisuGetData.toMilliSecondsDouble());
332  debugObserver->setDebugObserverDatafield(p + "t 1.1 Descriptions (ms)",
333  tRobotDescriptions.toMilliSecondsDouble());
334  debugObserver->setDebugObserverDatafield(p + "t 1.2 Global Poses (ms)",
335  tGlobalPoses.toMilliSecondsDouble());
336  debugObserver->setDebugObserverDatafield(p + "t 1.3 Frames (ms)",
337  tRobotFramePoses.toMilliSecondsDouble());
338  debugObserver->setDebugObserverDatafield(p + "t 1.4 Sensor Values (ms)",
339  tSensorValues.toMilliSecondsDouble());
340  debugObserver->setDebugObserverDatafield(p + "t 2 Build Layers (ms)",
341  tVisuBuildLayers.toMilliSecondsDouble());
342  debugObserver->setDebugObserverDatafield(p + "t 3 Commit (ms)",
343  tVisuCommit.toMilliSecondsDouble());
344  }
345  }
346 
347  Visu::RobotModel*
348  Visu::getRobotModel(const armem::robot_state::description::RobotDescription& robotDescription)
349  {
350  const std::string& robotName = robotDescription.name;
351 
352  RobotModel* robotModel = nullptr;
353  if (auto it = models.find(robotName); it != models.end())
354  {
355  robotModel = &it->second;
356  }
357  else
358  {
359  const std::filesystem::path robotPath = robotDescription.xml.toSystemPath();
360  ARMARX_INFO << "Loading robot model for '" << robotName << "' from " << robotPath
361  << " for visualization.";
362  auto [it2, _] = models.emplace(robotName, robotPath);
363  robotModel = &it2->second;
364  }
365  return robotModel;
366  }
367 
368  void
369  Visu::visualizeForceTorque(viz::Layer& layer,
370  const armem::robot_state::Robot& robot,
371  const proprioception::SensorValues& sensorValues)
372  {
373 
374  Visu::RobotModel* robotModel = getRobotModel(robot.description);
375 
376  if (robotModel == nullptr)
377  {
378  // ARMARX_WARNING << "Robot model is null";
379  return;
380  }
381 
382  robotModel->model->setJointValues(robot.config.jointMap);
383  robotModel->model->setGlobalPose(robot.config.globalPose.matrix());
384 
385  const proprioception::ForceTorqueValuesMap& forceTorques =
386  sensorValues.forceTorqueValuesMap;
387 
388  for (const auto& [side, ft] : forceTorques)
389  {
390  ARMARX_CHECK(side == RobotNameHelper::LocationLeft or
391  side == RobotNameHelper::LocationRight)
392  << side;
393 
394  const std::string forceTorqueSensorName =
395  robotModel->nameHelper->getArm(side).getForceTorqueSensor();
396 
397  const Eigen::Matrix4f forceTorqueSensorPose =
398  robotModel->model->getSensor(forceTorqueSensorName)->getGlobalPose();
399 
400  const std::string xyz = "XYZ";
401 
402  const Eigen::Vector3f from = simox::math::position(forceTorqueSensorPose);
403  for (int i = 0; i < 3; ++i)
404  {
405  simox::Color color =
406  simox::Color((255 * Eigen::Matrix3i::Identity().col(i)).eval());
407  color.a = 128;
408 
409  // Force arrows.
410  const float length = p.forceTorque.forceScale * ft.force(i);
411  const float width =
412  std::min(simox::math::rescale(std::abs(length), 0.F, 100.F, 1.F, 10.F), 10.F);
413 
414  const Eigen::Vector3f to =
415  from + length * simox::math::orientation(forceTorqueSensorPose).col(i);
416 
417  std::stringstream key;
418  ARMARX_CHECK_FITS_SIZE(i, xyz.size());
419  key << side << " Force " << xyz.at(i);
420  layer.add(viz::Arrow(key.str()).fromTo(from, to).color(color).width(width));
421 
422  // TODO: Torque circle arrows.
423  }
424  }
425  }
426 
427  Visu::RobotModel::RobotModel(const std::filesystem::path& robotPath) :
428  model{
429  VirtualRobot::RobotIO::loadRobot(robotPath, VirtualRobot::RobotIO::eStructure),
430  },
431  nameHelper{
432  RobotNameHelper::Create(robotPath),
433  }
434  {
435  }
436 
437 } // namespace armarx::armem::server::robot_state
armarx::RobotNameHelper::LocationLeft
static const std::string LocationLeft
Definition: RobotNameHelper.h:170
armarx::SimpleRunningTask
Usage:
Definition: TaskUtil.h:85
armarx::viz::Client::commit
CommitResult commit(StagedCommit const &commit)
Definition: Client.cpp:89
TIMING_START
#define TIMING_START(name)
Definition: TimeUtil.h:289
armarx::armem::server::robot_state::Visu::init
void init()
Definition: Visu.cpp:64
armarx::armem::robot_state::Robot
Definition: types.h:126
armarx::armem::server::robot_state::combine
armem::robot_state::Robots combine(const description::RobotDescriptionMap &robotDescriptions, const localization::RobotPoseMap &globalPoses, const proprioception::SensorValuesMap &sensorValues, const armem::Time &timestamp)
Definition: combine.cpp:15
ARMARX_CHECK_FITS_SIZE
#define ARMARX_CHECK_FITS_SIZE(number, size)
Check whether number is nonnegative (>= 0) and less than size. If it is not, throw an ExpressionExcep...
Definition: ExpressionException.h:159
robots
std::vector< VirtualRobot::RobotPtr > robots
Definition: VisualizationRobot.cpp:88
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:650
armarx::core::time::DateTime::Now
static DateTime Now()
Definition: DateTime.cpp:51
armarx::armem::server::robot_state::proprioception::Segment
Definition: Segment.h:47
armarx::armem::server::robot_state::localization::Segment
Definition: Segment.h:42
armarx::armem::server::robot_state::description::Segment
Definition: Segment.h:43
armarx::armem::server::robot_state::Visu::Visu
Visu(const description::Segment &descriptionSegment, const proprioception::Segment &proprioceptionSegment, const localization::Segment &localizationSegment)
Definition: Visu.cpp:33
armarx::armem::server::robot_state::proprioception::ForceTorqueValuesMap
std::unordered_map< std::string, ForceTorqueValues > ForceTorqueValuesMap
Definition: forward_declarations.h:84
armarx::viz::Arrow
Definition: Elements.h:196
armarx::RobotNameHelper::LocationRight
static const std::string LocationRight
Definition: RobotNameHelper.h:171
combine.h
armarx::viz::Layer::add
void add(ElementT const &element)
Definition: Layer.h:31
ARMARX_CHECK
#define ARMARX_CHECK(expression)
Shortcut for ARMARX_CHECK_EXPRESSION.
Definition: ExpressionException.h:82
Elements.h
TIMING_END_STREAM
#define TIMING_END_STREAM(name, os)
Definition: TimeUtil.h:310
armarx::armem::server::robot_state
Definition: RobotStateMemory.cpp:40
armarx::viz::Robot::useFullModel
Robot & useFullModel()
Definition: Robot.h:42
armarx::armem::robot_state::Robots
std::vector< Robot > Robots
Definition: forward_declarations.h:52
armarx::armem::server::robot_state::Visu::connect
void connect(const viz::Client &arviz, DebugObserverInterfacePrx debugObserver=nullptr)
Definition: Visu.cpp:69
GfxTL::Identity
void Identity(MatrixXX< N, N, T > *a)
Definition: MatrixXX.h:570
Color
uint32_t Color
RGBA color.
Definition: color.h:8
armarx::abs
std::vector< T > abs(const std::vector< T > &v)
Definition: VectorHelpers.h:281
armarx::viz::Arrow::width
Arrow & width(float w)
Definition: Elements.h:211
armarx::armem::human::Robot
@ Robot
Definition: util.h:17
ARMARX_DEBUG
#define ARMARX_DEBUG
Definition: Logging.h:184
armarx::armem::server::robot_state::localization::Segment::getRobotGlobalPosesLocking
RobotPoseMap getRobotGlobalPosesLocking(const armem::Time &timestamp) const
Definition: Segment.cpp:108
Segment.h
armarx::viz::Robot::useCollisionModel
Robot & useCollisionModel()
Definition: Robot.h:34
armarx::armem::toStringMilliSeconds
std::string toStringMilliSeconds(const Time &time, int decimals=3)
Returns time as e.g.
Definition: Time.cpp:11
armarx::viz::Pose
Definition: Elements.h:178
armarx::viz::Robot
Definition: Robot.h:10
armarx::armem::Time
armarx::core::time::DateTime Time
Definition: forward_declarations.h:13
armarx::to_string
const std::string & to_string(const std::string &s)
Definition: StringHelpers.h:41
armarx::armem::server::robot_state::proprioception::SensorValuesMap
std::unordered_map< std::string, SensorValues > SensorValuesMap
Definition: forward_declarations.h:85
CycleUtil.h
armarx::viz::Arrow::fromTo
Arrow & fromTo(const Eigen::Vector3f &from, const Eigen::Vector3f &to)
Definition: Elements.h:219
Segment.h
TimeUtil.h
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:181
Segment.h
armarx::armem::server::robot_state::description::RobotDescriptionMap
std::unordered_map< std::string, armarx::armem::robot_state::description::RobotDescription > RobotDescriptionMap
Definition: forward_declarations.h:67
armarx::viz::ElementOps::pose
DerivedT & pose(Eigen::Matrix4f const &pose)
Definition: ElementOps.h:176
armarx::armem::server::robot_state::localization::Segment::getRobotFramePosesLocking
RobotFramePoseMap getRobotFramePosesLocking(const armem::Time &timestamp) const
Definition: Segment.cpp:71
armarx::armem::server::robot_state::description::Segment::getRobotDescriptionsLocking
RobotDescriptionMap getRobotDescriptionsLocking(const armem::Time &timestamp) const
Definition: Segment.cpp:276
armarx::viz::ElementOps::scale
DerivedT & scale(Eigen::Vector3f scale)
Definition: ElementOps.h:254
IceUtil::Handle< class PropertyDefinitionContainer >
armarx::viz::ElementOps::color
DerivedT & color(Color color)
Definition: ElementOps.h:218
IceInternal::ProxyHandle<::IceProxy::armarx::DebugObserverInterface >
F
Definition: ExportDialogControllerTest.cpp:18
Time.h
armarx::RobotNameHelper::Create
static RobotNameHelperPtr Create(const RobotInfoNodePtr &robotInfo, const StatechartProfilePtr &profile)
Definition: RobotNameHelper.cpp:107
armarx::armem::server::robot_state::proprioception::Segment::getSensorValuesLocking
SensorValuesMap getSensorValuesLocking(const armem::Time &timestamp, DebugObserverHelper *debugObserver=nullptr) const
Definition: Segment.cpp:78
armarx::Logging::setTag
void setTag(const LogTag &tag)
Definition: Logging.cpp:54
Logging.h
min
T min(T t1, T t2)
Definition: gdiam.h:44
ARMARX_WARNING
#define ARMARX_WARNING
Definition: Logging.h:193
armarx::viz::Client::layer
Layer layer(std::string const &name) const
Definition: Client.cpp:80
armarx::viz::Client
Definition: Client.h:117
Visu.h
armarx::viz::Layer
Definition: Layer.h:12
armarx::armem::server::robot_state::Visu::defineProperties
void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string &prefix="visu.")
Definition: Visu.cpp:44
armarx::DebugObserverHelper
Brief description of class DebugObserverHelper.
Definition: DebugObserverHelper.h:51
SensorValues.h