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