Robowflex  v0.1
Making MoveIt Easy
yaml.cpp
Go to the documentation of this file.
1 /* Author: Zachary Kingston */
2 
3 #include <cstdint>
4 
5 #include <algorithm>
6 #include <string>
7 
8 #include <boost/algorithm/hex.hpp>
9 #include <boost/iostreams/device/back_inserter.hpp>
10 #include <boost/iostreams/filter/zlib.hpp>
11 #include <boost/iostreams/filtering_stream.hpp>
12 
13 #include <robowflex_library/geometry.h>
14 #include <robowflex_library/io.h>
15 #include <robowflex_library/tf.h>
16 #include <robowflex_library/io/yaml.h>
17 #include <robowflex_library/macros.h>
18 #include <robowflex_library/yaml.h>
19 
20 using namespace robowflex;
21 
22 namespace
23 {
24  static std::string boolToString(bool b)
25  {
26  return b ? "true" : "false";
27  }
28 
29  static bool nodeToBool(const YAML::Node &n)
30  {
31  std::string s = n.as<std::string>();
32  std::transform(s.begin(), s.end(), s.begin(), ::tolower);
33  return s == "true";
34  }
35 
36  static bool isHeaderEmpty(const std_msgs::Header &h)
37  {
38  return h.seq == 0 && h.stamp.isZero() && h.frame_id == "world";
39  }
40 
41  static std_msgs::Header getDefaultHeader()
42  {
43  std_msgs::Header msg;
44  msg.frame_id = "world";
45  return msg;
46  }
47 
48  static unsigned int nodeToCollisionObject(const YAML::Node &n)
49  {
50  try
51  {
52  std::string s = n.as<std::string>();
53  std::transform(s.begin(), s.end(), s.begin(), ::tolower);
54 
55  if (s == "move")
56  return moveit_msgs::CollisionObject::MOVE;
57  if (s == "remove")
58  return moveit_msgs::CollisionObject::REMOVE;
59  if (s == "append")
60  return moveit_msgs::CollisionObject::APPEND;
61 
62  return moveit_msgs::CollisionObject::ADD;
63  }
64  catch (const YAML::BadConversion &e)
65  {
66  // Sometimes it is specified as the int.
67  int op = n.as<int>();
68  switch (op)
69  {
70  case 0:
71  return moveit_msgs::CollisionObject::ADD;
72  case 1:
73  return moveit_msgs::CollisionObject::REMOVE;
74  case 2:
75  return moveit_msgs::CollisionObject::APPEND;
76  case 3:
77  return moveit_msgs::CollisionObject::MOVE;
78  default:
79  return moveit_msgs::CollisionObject::ADD;
80  }
81  }
82  }
83 
84  static std::string primitiveTypeToString(const shape_msgs::SolidPrimitive &shape)
85  {
86  switch (shape.type)
87  {
88  case shape_msgs::SolidPrimitive::BOX:
89  return "box";
90  break;
91  case shape_msgs::SolidPrimitive::SPHERE:
92  return "sphere";
93  break;
94  case shape_msgs::SolidPrimitive::CYLINDER:
95  return "cylinder";
96  break;
97  case shape_msgs::SolidPrimitive::CONE:
98  return "cone";
99  break;
100  default:
101  return "invalid";
102  break;
103  }
104  }
105 
106  static void nodeToPrimitiveType(const YAML::Node &n, shape_msgs::SolidPrimitive &shape)
107  {
108  std::string s = n.as<std::string>();
109  std::transform(s.begin(), s.end(), s.begin(), ::tolower);
110 
111  if (s == "sphere")
112  shape.type = shape_msgs::SolidPrimitive::SPHERE;
113  else if (s == "cylinder")
114  shape.type = shape_msgs::SolidPrimitive::CYLINDER;
115  else if (s == "cone")
116  shape.type = shape_msgs::SolidPrimitive::CONE;
117  else if (s == "box")
118  shape.type = shape_msgs::SolidPrimitive::BOX;
119  else
120  shape.type = n.as<int>();
121  }
122 
123  static bool isVector3Zero(const geometry_msgs::Vector3 &v)
124  {
125  return v.x == 0 && v.y == 0 && v.z == 0;
126  }
127 
128  static bool isConstraintEmpty(const moveit_msgs::Constraints &c)
129  {
130  return c.joint_constraints.empty() //
131  && c.position_constraints.empty() //
132  && c.orientation_constraints.empty() //
133  && c.visibility_constraints.empty();
134  }
135 
136  static std::string compressHex(const std::vector<int8_t> &v)
137  {
138  std::vector<char> compress;
139  {
140  boost::iostreams::filtering_ostream fos;
141  fos.push(boost::iostreams::zlib_compressor());
142  fos.push(boost::iostreams::back_inserter(compress));
143 
144  for (const auto &i : v)
145  fos << i;
146  }
147 
148  std::string result;
149  boost::algorithm::hex(compress.begin(), compress.end(), std::back_inserter(result));
150 
151  return result;
152  }
153 
154  static std::vector<int8_t> decompressHex(const std::string &hex)
155  {
156  std::vector<int8_t> unhexed;
157  boost::algorithm::unhex(hex, std::back_inserter(unhexed));
158 
159  std::vector<char> decompress;
160  {
161  boost::iostreams::filtering_ostream fos;
162  fos.push(boost::iostreams::zlib_decompressor());
163  fos.push(boost::iostreams::back_inserter(decompress));
164 
165  for (const auto &i : unhexed)
166  fos << i;
167  }
168 
169  return std::vector<int8_t>(decompress.begin(), decompress.end());
170  }
171 } // namespace
172 
173 namespace YAML
174 {
175 #if ROBOWFLEX_AT_LEAST_KINETIC
176 #else
177  // Copied from YAML_DEFINE_CONVERT_STREAMABLE
178  Node convert<signed char>::encode(const signed char &rhs)
179  {
180  std::stringstream stream;
181  stream.precision(std::numeric_limits<signed char>::digits10 + 1);
182  stream << rhs;
183  return Node(stream.str());
184  }
185 
186  bool convert<signed char>::decode(const Node &node, signed char &rhs)
187  {
188  if (node.Type() != NodeType::Scalar)
189  return false;
190 
191  const std::string &input = node.Scalar();
192  std::stringstream stream(input);
193  stream.unsetf(std::ios::dec);
194  if ((stream >> rhs) && (stream >> std::ws).eof())
195  return true;
196  if (std::numeric_limits<signed char>::has_infinity)
197  {
198  if (conversion::IsInfinity(input))
199  {
200  rhs = std::numeric_limits<signed char>::infinity();
201  return true;
202  }
203  else if (conversion::IsNegativeInfinity(input))
204  {
205  rhs = -std::numeric_limits<signed char>::infinity();
206  return true;
207  }
208  }
209 
210  if (std::numeric_limits<signed char>::has_quiet_NaN && conversion::IsNaN(input))
211  {
212  rhs = std::numeric_limits<signed char>::quiet_NaN();
213  return true;
214  }
215 
216  return false;
217  }
218 #endif
219 
220  Node convert<moveit_msgs::PlanningScene>::encode(const moveit_msgs::PlanningScene &rhs)
221  {
222  Node node;
223  node["name"] = rhs.name;
224  node["robot_state"] = rhs.robot_state;
225  node["robot_model_name"] = rhs.robot_model_name;
226  node["fixed_frame_transforms"] = rhs.fixed_frame_transforms;
227  node["allowed_collision_matrix"] = rhs.allowed_collision_matrix;
228 
229  // node["link_padding"] = rhs.link_padding;
230  // node["link_padding"].SetStyle(YAML::EmitterStyle::Flow);
231 
232  // node["link_scale"] = rhs.link_scale;
233  // node["link_scale"].SetStyle(YAML::EmitterStyle::Flow);
234 
235  // node["object_colors"] = rhs.object_colors;
236 
237  if (!rhs.world.collision_objects.empty() || !rhs.world.octomap.octomap.data.empty())
238  node["world"] = rhs.world;
239 
240  if (rhs.is_diff)
241  node["is_diff"] = boolToString(rhs.is_diff);
242 
243  return node;
244  }
245 
246  bool convert<moveit_msgs::PlanningScene>::decode(const Node &node, moveit_msgs::PlanningScene &rhs)
247  {
248  rhs = moveit_msgs::PlanningScene();
249 
250  if (IO::isNode(node["name"]))
251  rhs.name = node["name"].as<std::string>();
252 
253  if (IO::isNode(node["robot_state"]))
254  rhs.robot_state = node["robot_state"].as<moveit_msgs::RobotState>();
255 
256  if (IO::isNode(node["robot_model_name"]))
257  rhs.robot_model_name = node["robot_model_name"].as<std::string>();
258 
259  if (IO::isNode(node["fixed_frame_transforms"]))
260  rhs.fixed_frame_transforms =
261  node["fixed_frame_transforms"].as<std::vector<geometry_msgs::TransformStamped>>();
262 
263  if (IO::isNode(node["allowed_collision_matrix"]))
264  rhs.allowed_collision_matrix =
265  node["allowed_collision_matrix"].as<moveit_msgs::AllowedCollisionMatrix>();
266 
267  if (IO::isNode(node["link_padding"]))
268  rhs.link_padding = node["link_padding"].as<std::vector<moveit_msgs::LinkPadding>>();
269 
270  if (IO::isNode(node["link_scale"]))
271  rhs.link_scale = node["link_scale"].as<std::vector<moveit_msgs::LinkScale>>();
272 
273  if (IO::isNode(node["object_colors"]))
274  rhs.object_colors = node["object_colors"].as<std::vector<moveit_msgs::ObjectColor>>();
275 
276  if (IO::isNode(node["world"]))
277  rhs.world = node["world"].as<moveit_msgs::PlanningSceneWorld>();
278 
279  if (IO::isNode(node["is_diff"]))
280  rhs.is_diff = nodeToBool(node["is_diff"]);
281 
282  return true;
283  }
284 
285  Node convert<moveit_msgs::RobotState>::encode(const moveit_msgs::RobotState &rhs)
286  {
287  Node node;
288 
289  if (!rhs.joint_state.name.empty())
290  node["joint_state"] = rhs.joint_state;
291 
292  if (!rhs.multi_dof_joint_state.joint_names.empty())
293  node["multi_dof_joint_state"] = rhs.multi_dof_joint_state;
294 
295  if (!rhs.attached_collision_objects.empty())
296  node["attached_collision_objects"] = rhs.attached_collision_objects;
297 
298  if (rhs.is_diff)
299  node["is_diff"] = boolToString(rhs.is_diff);
300 
301  return node;
302  }
303 
304  bool convert<moveit_msgs::RobotState>::decode(const Node &node, moveit_msgs::RobotState &rhs)
305  {
306  rhs = moveit_msgs::RobotState();
307 
308  if (IO::isNode(node["joint_state"]))
309  rhs.joint_state = node["joint_state"].as<sensor_msgs::JointState>();
310 
311  if (IO::isNode(node["multi_dof_joint_state"]))
312  rhs.multi_dof_joint_state = node["multi_dof_joint_state"].as<sensor_msgs::MultiDOFJointState>();
313 
314  if (IO::isNode(node["attached_collision_objects"]))
315  rhs.attached_collision_objects =
316  node["attached_collision_objects"].as<std::vector<moveit_msgs::AttachedCollisionObject>>();
317 
318  if (IO::isNode(node["is_diff"]))
319  rhs.is_diff = nodeToBool(node["is_diff"]);
320 
321  return true;
322  }
323 
324  Node convert<geometry_msgs::TransformStamped>::encode(const geometry_msgs::TransformStamped &rhs)
325  {
326  Node node;
327 
328  if (!isHeaderEmpty(rhs.header))
329  node["header"] = rhs.header;
330 
331  node["child_frame_id"] = rhs.child_frame_id;
332  node["transform"] = rhs.transform;
333  return node;
334  }
335 
336  bool convert<geometry_msgs::TransformStamped>::decode(const Node &node,
337  geometry_msgs::TransformStamped &rhs)
338  {
339  rhs = geometry_msgs::TransformStamped();
340 
341  if (IO::isNode(node["header"]))
342  rhs.header = node["header"].as<std_msgs::Header>();
343  else
344  rhs.header = getDefaultHeader();
345 
346  if (IO::isNode(node["child_frame_id"]))
347  rhs.child_frame_id = node["child_frame_id"].as<std::string>();
348 
349  if (IO::isNode(node["transform"]))
350  rhs.transform = node["transform"].as<geometry_msgs::Transform>();
351 
352  return true;
353  }
354 
355  Node convert<std_msgs::Header>::encode(const std_msgs::Header &rhs)
356  {
357  Node node;
358  if (rhs.seq != 0)
359  node["seq"] = rhs.seq;
360 
361  if (!rhs.stamp.isZero())
362  {
363  node["stamp"]["secs"] = rhs.stamp.sec;
364  node["stamp"]["nsecs"] = rhs.stamp.nsec;
365  }
366 
367  if (rhs.frame_id != "world" && rhs.frame_id != "/world")
368  node["frame_id"] = rhs.frame_id;
369 
370  return node;
371  }
372 
373  bool convert<std_msgs::Header>::decode(const Node &node, std_msgs::Header &rhs)
374  {
375  rhs = std_msgs::Header();
376  rhs.frame_id = "world";
377 
378  if (IO::isNode(node["seq"]))
379  rhs.seq = node["seq"].as<int>();
380 
381  if (IO::isNode(node["stamp"]))
382  {
383  try
384  {
385  rhs.stamp.sec = node["stamp"]["sec"].as<int>();
386  rhs.stamp.nsec = node["stamp"]["nsec"].as<int>();
387  }
388  catch (YAML::InvalidNode &e)
389  {
390  rhs.stamp.sec = node["stamp"]["secs"].as<int>();
391  rhs.stamp.nsec = node["stamp"]["nsecs"].as<int>();
392  }
393  }
394 
395  if (IO::isNode(node["frame_id"]))
396  rhs.frame_id = node["frame_id"].as<std::string>();
397 
398  return true;
399  }
400 
401  Node convert<geometry_msgs::Pose>::encode(const geometry_msgs::Pose &rhs)
402  {
403  Node node;
404  node["position"] = rhs.position;
405  node["orientation"] = rhs.orientation;
406  return node;
407  }
408 
409  bool convert<geometry_msgs::Pose>::decode(const Node &node, geometry_msgs::Pose &rhs)
410  {
411  rhs = geometry_msgs::Pose();
412 
413  if (IO::isNode(node["position"]))
414  rhs.position = node["position"].as<geometry_msgs::Point>();
415 
416  if (IO::isNode(node["orientation"]))
417  rhs.orientation = node["orientation"].as<geometry_msgs::Quaternion>();
418 
419  return true;
420  }
421 
422  Node convert<geometry_msgs::PoseStamped>::encode(const geometry_msgs::PoseStamped &rhs)
423  {
424  Node node;
425  if (!isHeaderEmpty(rhs.header))
426  node["header"] = rhs.header;
427 
428  node["pose"] = rhs.pose;
429 
430  return node;
431  }
432 
433  bool convert<geometry_msgs::PoseStamped>::decode(const Node &node, geometry_msgs::PoseStamped &rhs)
434  {
435  rhs = geometry_msgs::PoseStamped();
436 
437  if (IO::isNode(node["header"]))
438  rhs.header = node["header"].as<std_msgs::Header>();
439  else
440  rhs.header = getDefaultHeader();
441 
442  rhs.pose = node["pose"].as<geometry_msgs::Pose>();
443 
444  return true;
445  }
446 
447  Node convert<geometry_msgs::Transform>::encode(const geometry_msgs::Transform &rhs)
448  {
449  Node node;
450  node["translation"] = rhs.translation;
451  node["rotation"] = rhs.rotation;
452  return node;
453  }
454 
455  bool convert<geometry_msgs::Transform>::decode(const Node &node, geometry_msgs::Transform &rhs)
456  {
457  rhs = geometry_msgs::Transform();
458 
459  if (IO::isNode(node["translation"]))
460  rhs.translation = node["translation"].as<geometry_msgs::Vector3>();
461 
462  if (IO::isNode(node["rotation"]))
463  rhs.rotation = node["rotation"].as<geometry_msgs::Quaternion>();
464 
465  return true;
466  }
467 
468  Node convert<geometry_msgs::Point>::encode(const geometry_msgs::Point &rhs)
469  {
470  Node node;
471  ROBOWFLEX_YAML_FLOW(node);
472 
473  node.push_back(rhs.x);
474  node.push_back(rhs.y);
475  node.push_back(rhs.z);
476  return node;
477  }
478 
479  bool convert<geometry_msgs::Point>::decode(const Node &node, geometry_msgs::Point &rhs)
480  {
481  rhs = geometry_msgs::Point();
482 
483  if (node.IsSequence())
484  {
485  rhs.x = node[0].as<double>();
486  rhs.y = node[1].as<double>();
487  rhs.z = node[2].as<double>();
488  }
489  else
490  {
491  rhs.x = node["x"].as<double>();
492  rhs.y = node["y"].as<double>();
493  rhs.z = node["z"].as<double>();
494  }
495  return true;
496  }
497 
498  Node convert<geometry_msgs::Vector3>::encode(const geometry_msgs::Vector3 &rhs)
499  {
500  Node node;
501  ROBOWFLEX_YAML_FLOW(node);
502 
503  node.push_back(rhs.x);
504  node.push_back(rhs.y);
505  node.push_back(rhs.z);
506  return node;
507  }
508 
509  bool convert<geometry_msgs::Vector3>::decode(const Node &node, geometry_msgs::Vector3 &rhs)
510  {
511  rhs = geometry_msgs::Vector3();
512 
513  if (node.IsSequence())
514  {
515  rhs.x = node[0].as<double>();
516  rhs.y = node[1].as<double>();
517  rhs.z = node[2].as<double>();
518  }
519  else
520  {
521  rhs.x = node["x"].as<double>();
522  rhs.y = node["y"].as<double>();
523  rhs.z = node["z"].as<double>();
524  }
525 
526  return true;
527  }
528 
529  Node convert<geometry_msgs::Quaternion>::encode(const geometry_msgs::Quaternion &rhs)
530  {
531  Node node;
532  ROBOWFLEX_YAML_FLOW(node);
533 
534  node.push_back(rhs.x);
535  node.push_back(rhs.y);
536  node.push_back(rhs.z);
537  node.push_back(rhs.w);
538  return node;
539  }
540 
541  bool convert<geometry_msgs::Quaternion>::decode(const Node &node, geometry_msgs::Quaternion &rhs)
542  {
543  rhs = geometry_msgs::Quaternion();
544 
545  if (node.IsSequence())
546  {
547  rhs.x = node[0].as<double>();
548  rhs.y = node[1].as<double>();
549  rhs.z = node[2].as<double>();
550  rhs.w = node[3].as<double>();
551  }
552  else
553  {
554  rhs.x = node["x"].as<double>();
555  rhs.y = node["y"].as<double>();
556  rhs.z = node["z"].as<double>();
557  rhs.w = node["w"].as<double>();
558  }
559  return true;
560  }
561 
562  Node convert<geometry_msgs::Twist>::encode(const geometry_msgs::Twist &rhs)
563  {
564  Node node;
565  node["linear"] = rhs.linear;
566  node["angular"] = rhs.angular;
567  return node;
568  }
569 
570  bool convert<geometry_msgs::Twist>::decode(const Node &node, geometry_msgs::Twist &rhs)
571  {
572  rhs = geometry_msgs::Twist();
573 
574  rhs.linear = node["linear"].as<geometry_msgs::Vector3>();
575  rhs.angular = node["angular"].as<geometry_msgs::Vector3>();
576 
577  return true;
578  }
579 
580  Node convert<geometry_msgs::Wrench>::encode(const geometry_msgs::Wrench &rhs)
581  {
582  Node node;
583  node["force"] = rhs.force;
584  node["torque"] = rhs.torque;
585  return node;
586  }
587 
588  bool convert<geometry_msgs::Wrench>::decode(const Node &node, geometry_msgs::Wrench &rhs)
589  {
590  rhs = geometry_msgs::Wrench();
591 
592  rhs.force = node["force"].as<geometry_msgs::Vector3>();
593  rhs.torque = node["torque"].as<geometry_msgs::Vector3>();
594 
595  return true;
596  }
597 
598  Node convert<sensor_msgs::JointState>::encode(const sensor_msgs::JointState &rhs)
599  {
600  Node node;
601 
602  if (!isHeaderEmpty(rhs.header))
603  node["header"] = rhs.header;
604 
605  if (!rhs.name.empty())
606  {
607  node["name"] = rhs.name;
608  ROBOWFLEX_YAML_FLOW(node["name"]);
609  }
610 
611  if (!rhs.position.empty())
612  {
613  node["position"] = rhs.position;
614  ROBOWFLEX_YAML_FLOW(node["position"]);
615  }
616 
617  if (!rhs.velocity.empty())
618  {
619  node["velocity"] = rhs.velocity;
620  ROBOWFLEX_YAML_FLOW(node["velocity"]);
621  }
622 
623  if (!rhs.effort.empty())
624  {
625  node["effort"] = rhs.effort;
626  ROBOWFLEX_YAML_FLOW(node["effort"]);
627  }
628 
629  return node;
630  }
631 
632  bool convert<sensor_msgs::JointState>::decode(const Node &node, sensor_msgs::JointState &rhs)
633  {
634  rhs = sensor_msgs::JointState();
635 
636  if (IO::isNode(node["header"]))
637  rhs.header = node["header"].as<std_msgs::Header>();
638  else
639  rhs.header = getDefaultHeader();
640 
641  if (IO::isNode(node["name"]))
642  rhs.name = node["name"].as<std::vector<std::string>>();
643 
644  if (IO::isNode(node["position"]))
645  rhs.position = node["position"].as<std::vector<double>>();
646 
647  if (IO::isNode(node["velocity"]))
648  rhs.velocity = node["velocity"].as<std::vector<double>>();
649 
650  if (IO::isNode(node["effort"]))
651  rhs.effort = node["effort"].as<std::vector<double>>();
652 
653  return true;
654  }
655 
656  Node convert<sensor_msgs::MultiDOFJointState>::encode(const sensor_msgs::MultiDOFJointState &rhs)
657  {
658  Node node;
659 
660  if (!isHeaderEmpty(rhs.header))
661  node["header"] = rhs.header;
662 
663  node["joint_names"] = rhs.joint_names;
664  ROBOWFLEX_YAML_FLOW(node["joint_names"]);
665 
666  node["transforms"] = rhs.transforms;
667  ROBOWFLEX_YAML_FLOW(node["transforms"]);
668 
669  node["twist"] = rhs.twist;
670  ROBOWFLEX_YAML_FLOW(node["twist"]);
671 
672  node["wrench"] = rhs.wrench;
673  ROBOWFLEX_YAML_FLOW(node["wrench"]);
674  return node;
675  }
676 
677  bool convert<sensor_msgs::MultiDOFJointState>::decode(const Node &node,
678  sensor_msgs::MultiDOFJointState &rhs)
679  {
680  rhs = sensor_msgs::MultiDOFJointState();
681 
682  if (IO::isNode(node["header"]))
683  rhs.header = node["header"].as<std_msgs::Header>();
684  else
685  rhs.header = getDefaultHeader();
686 
687  if (IO::isNode(node["joint_names"]))
688  rhs.joint_names = node["joint_names"].as<std::vector<std::string>>();
689 
690  if (IO::isNode(node["transforms"]))
691  rhs.transforms = node["transforms"].as<std::vector<geometry_msgs::Transform>>();
692 
693  if (IO::isNode(node["twist"]))
694  rhs.twist = node["twist"].as<std::vector<geometry_msgs::Twist>>();
695 
696  if (IO::isNode(node["wrench"]))
697  rhs.wrench = node["wrench"].as<std::vector<geometry_msgs::Wrench>>();
698 
699  return true;
700  }
701 
702  Node
703  convert<moveit_msgs::AttachedCollisionObject>::encode(const moveit_msgs::AttachedCollisionObject &rhs)
704  {
705  Node node;
706  node["link_name"] = rhs.link_name;
707  node["object"] = rhs.object;
708 
709  if (!rhs.touch_links.empty())
710  node["touch_links"] = rhs.touch_links;
711 
712  if (!rhs.detach_posture.points.empty())
713  node["detach_posture"] = rhs.detach_posture;
714 
715  node["weight"] = rhs.weight;
716  return node;
717  }
718 
719  bool convert<moveit_msgs::AttachedCollisionObject>::decode(const Node &node,
720  moveit_msgs::AttachedCollisionObject &rhs)
721  {
722  rhs = moveit_msgs::AttachedCollisionObject();
723 
724  if (IO::isNode(node["link_name"]))
725  rhs.link_name = node["link_name"].as<std::string>();
726 
727  if (IO::isNode(node["object"]))
728  rhs.object = node["object"].as<moveit_msgs::CollisionObject>();
729 
730  if (IO::isNode(node["touch_links"]))
731  rhs.touch_links = node["touch_links"].as<std::vector<std::string>>();
732 
733  if (IO::isNode(node["detach_posture"]))
734  rhs.detach_posture = node["detach_posture"].as<trajectory_msgs::JointTrajectory>();
735 
736  if (IO::isNode(node["weight"]))
737  rhs.weight = node["weight"].as<double>();
738 
739  return true;
740  }
741 
742  Node convert<trajectory_msgs::JointTrajectory>::encode(const trajectory_msgs::JointTrajectory &rhs)
743  {
744  Node node;
745 
746  if (!isHeaderEmpty(rhs.header))
747  node["header"] = rhs.header;
748 
749  node["joint_names"] = rhs.joint_names;
750  ROBOWFLEX_YAML_FLOW(node["joint_names"]);
751  node["points"] = rhs.points;
752  return node;
753  }
754 
755  bool convert<trajectory_msgs::JointTrajectory>::decode(const Node &node,
756  trajectory_msgs::JointTrajectory &rhs)
757  {
758  rhs = trajectory_msgs::JointTrajectory();
759 
760  if (IO::isNode(node["header"]))
761  rhs.header = node["header"].as<std_msgs::Header>();
762  else
763  rhs.header = getDefaultHeader();
764 
765  if (IO::isNode(node["joint_names"]))
766  rhs.joint_names = node["joint_names"].as<std::vector<std::string>>();
767 
768  if (IO::isNode(node["points"]))
769  rhs.points = node["points"].as<std::vector<trajectory_msgs::JointTrajectoryPoint>>();
770 
771  return true;
772  }
773 
774  Node
775  convert<trajectory_msgs::JointTrajectoryPoint>::encode(const trajectory_msgs::JointTrajectoryPoint &rhs)
776  {
777  Node node;
778 
779  if (!rhs.positions.empty())
780  {
781  node["positions"] = rhs.positions;
782  ROBOWFLEX_YAML_FLOW(node["positions"]);
783  }
784 
785  if (!rhs.velocities.empty())
786  {
787  node["velocities"] = rhs.velocities;
788  ROBOWFLEX_YAML_FLOW(node["velocities"]);
789  }
790 
791  if (!rhs.accelerations.empty())
792  {
793  node["accelerations"] = rhs.accelerations;
794  ROBOWFLEX_YAML_FLOW(node["accelerations"]);
795  }
796 
797  if (!rhs.effort.empty())
798  {
799  node["effort"] = rhs.effort;
800  ROBOWFLEX_YAML_FLOW(node["effort"]);
801  }
802 
803  node["time_from_start"] = rhs.time_from_start;
804 
805  return node;
806  }
807 
808  bool convert<trajectory_msgs::JointTrajectoryPoint>::decode(const Node &node,
809  trajectory_msgs::JointTrajectoryPoint &rhs)
810  {
811  if (IO::isNode(node["positions"]))
812  rhs.positions = node["positions"].as<std::vector<double>>();
813 
814  if (IO::isNode(node["velocities"]))
815  rhs.velocities = node["velocities"].as<std::vector<double>>();
816 
817  if (IO::isNode(node["accelerations"]))
818  rhs.accelerations = node["accelerations"].as<std::vector<double>>();
819 
820  if (IO::isNode(node["effort"]))
821  rhs.effort = node["effort"].as<std::vector<double>>();
822 
823  if (IO::isNode(node["time_from_start"]))
824  rhs.time_from_start = node["time_from_start"].as<ros::Duration>();
825 
826  return true;
827  }
828 
829  Node convert<moveit_msgs::CollisionObject>::encode(const moveit_msgs::CollisionObject &rhs)
830  {
831  Node node;
832 
833  if (!isHeaderEmpty(rhs.header))
834  node["header"] = rhs.header;
835 
836  node["id"] = rhs.id;
837 
838 #if ROBOWFLEX_MOVEIT_VERSION >= ROBOWFLEX_MOVEIT_VERSION_COMPUTE(1, 1, 6)
839  node["pose"] = rhs.pose;
840 #endif
841 
842  if (!rhs.type.key.empty())
843  node["type"] = rhs.type;
844 
845  if (!rhs.primitives.empty())
846  {
847  node["primitives"] = rhs.primitives;
848  node["primitive_poses"] = rhs.primitive_poses;
849  }
850 
851  if (!rhs.meshes.empty())
852  {
853  node["meshes"] = rhs.meshes;
854  node["mesh_poses"] = rhs.mesh_poses;
855  }
856 
857  if (!rhs.planes.empty())
858  {
859  node["planes"] = rhs.planes;
860  node["plane_poses"] = rhs.plane_poses;
861  }
862 
863  std::string s;
864  switch (rhs.operation)
865  {
866  case moveit_msgs::CollisionObject::REMOVE:
867  s = "remove";
868  break;
869  case moveit_msgs::CollisionObject::APPEND:
870  s = "append";
871  break;
872  case moveit_msgs::CollisionObject::MOVE:
873  s = "move";
874  break;
875  default:
876  return node;
877  }
878 
879  node["operation"] = s;
880  return node;
881  }
882 
883  bool convert<moveit_msgs::CollisionObject>::decode(const Node &node, moveit_msgs::CollisionObject &rhs)
884  {
885  RobotPose pose = TF::identity();
886  geometry_msgs::Pose pose_msg = TF::poseEigenToMsg(pose);
887  rhs = moveit_msgs::CollisionObject();
888 
889  if (IO::isNode(node["header"]))
890  rhs.header = node["header"].as<std_msgs::Header>();
891  else
892  rhs.header = getDefaultHeader();
893 
894  if (IO::isNode(node["id"]))
895  rhs.id = node["id"].as<std::string>();
896 
897  if (IO::isNode(node["pose"]))
898  {
899  pose_msg = node["pose"].as<geometry_msgs::Pose>();
900  pose = TF::poseMsgToEigen(pose_msg);
901  }
902 
903 #if ROBOWFLEX_MOVEIT_VERSION >= ROBOWFLEX_MOVEIT_VERSION_COMPUTE(1, 1, 6)
904  rhs.pose = pose_msg;
905 #endif
906 
907  if (IO::isNode(node["type"]))
908  rhs.type = node["type"].as<object_recognition_msgs::ObjectType>();
909 
910  if (IO::isNode(node["primitives"]))
911  {
912  rhs.primitives = node["primitives"].as<std::vector<shape_msgs::SolidPrimitive>>();
913  rhs.primitive_poses = node["primitive_poses"].as<std::vector<geometry_msgs::Pose>>();
914 
915  // If loading a newer format, add pose to include offset
916 #if ROBOWFLEX_MOVEIT_VERSION < ROBOWFLEX_MOVEIT_VERSION_COMPUTE(1, 1, 6)
917  for (auto &primitive_pose : rhs.primitive_poses)
918  primitive_pose = TF::poseEigenToMsg(pose * TF::poseMsgToEigen(primitive_pose));
919 #endif
920  }
921 
922  if (IO::isNode(node["meshes"]))
923  {
924  rhs.meshes = node["meshes"].as<std::vector<shape_msgs::Mesh>>();
925  rhs.mesh_poses = node["mesh_poses"].as<std::vector<geometry_msgs::Pose>>();
926 
927  // If loading a newer format, add pose to include offset
928 #if ROBOWFLEX_MOVEIT_VERSION < ROBOWFLEX_MOVEIT_VERSION_COMPUTE(1, 1, 6)
929  for (auto &mesh_pose : rhs.mesh_poses)
930  mesh_pose = TF::poseEigenToMsg(pose * TF::poseMsgToEigen(mesh_pose));
931 #endif
932  }
933 
934  if (IO::isNode(node["planes"]))
935  {
936  rhs.planes = node["planes"].as<std::vector<shape_msgs::Plane>>();
937  rhs.plane_poses = node["plane_poses"].as<std::vector<geometry_msgs::Pose>>();
938 
939  // If loading a newer format, add pose to include offset
940 #if ROBOWFLEX_MOVEIT_VERSION < ROBOWFLEX_MOVEIT_VERSION_COMPUTE(1, 1, 6)
941  for (auto &plane_pose : rhs.plane_poses)
942  plane_pose = TF::poseEigenToMsg(pose * TF::poseMsgToEigen(plane_pose));
943 #endif
944  }
945 
946  if (IO::isNode(node["operation"]))
947  rhs.operation = nodeToCollisionObject(node["operation"]);
948 
949  return true;
950  }
951 
952  Node convert<object_recognition_msgs::ObjectType>::encode(const object_recognition_msgs::ObjectType &rhs)
953  {
954  Node node;
955  node["key"] = rhs.key;
956  node["db"] = rhs.db;
957  return node;
958  }
959 
960  bool convert<object_recognition_msgs::ObjectType>::decode(const Node &node,
961  object_recognition_msgs::ObjectType &rhs)
962  {
963  rhs = object_recognition_msgs::ObjectType();
964 
965  if (IO::isNode(node["key"]))
966  rhs.key = node["key"].as<std::string>();
967 
968  if (IO::isNode(node["db"]))
969  rhs.db = node["db"].as<std::string>();
970 
971  return true;
972  }
973 
974  Node convert<moveit_msgs::LinkPadding>::encode(const moveit_msgs::LinkPadding &rhs)
975  {
976  Node node;
977  node["link_name"] = rhs.link_name;
978  node["padding"] = rhs.padding;
979  return node;
980  }
981 
982  bool convert<moveit_msgs::LinkPadding>::decode(const Node &node, moveit_msgs::LinkPadding &rhs)
983  {
984  rhs = moveit_msgs::LinkPadding();
985 
986  if (IO::isNode(node["link_name"]))
987  rhs.link_name = node["link_name"].as<std::string>();
988 
989  if (IO::isNode(node["padding"]))
990  rhs.padding = node["padding"].as<double>();
991 
992  return true;
993  }
994 
995  Node convert<moveit_msgs::LinkScale>::encode(const moveit_msgs::LinkScale &rhs)
996  {
997  Node node;
998  node["link_name"] = rhs.link_name;
999  node["scale"] = rhs.scale;
1000  return node;
1001  }
1002 
1003  bool convert<moveit_msgs::LinkScale>::decode(const Node &node, moveit_msgs::LinkScale &rhs)
1004  {
1005  rhs = moveit_msgs::LinkScale();
1006 
1007  if (IO::isNode(node["link_name"]))
1008  rhs.link_name = node["link_name"].as<std::string>();
1009 
1010  if (IO::isNode(node["scale"]))
1011  rhs.scale = node["scale"].as<double>();
1012 
1013  return true;
1014  }
1015 
1016  Node convert<moveit_msgs::ObjectColor>::encode(const moveit_msgs::ObjectColor &rhs)
1017  {
1018  Node node;
1019  node["id"] = rhs.id;
1020  node["color"] = rhs.color;
1021  return node;
1022  }
1023 
1024  bool convert<moveit_msgs::ObjectColor>::decode(const Node &node, moveit_msgs::ObjectColor &rhs)
1025  {
1026  rhs = moveit_msgs::ObjectColor();
1027 
1028  if (IO::isNode(node["id"]))
1029  rhs.id = node["id"].as<std::string>();
1030 
1031  if (IO::isNode(node["color"]))
1032  rhs.color = node["color"].as<std_msgs::ColorRGBA>();
1033 
1034  return true;
1035  }
1036 
1037  Node convert<std_msgs::ColorRGBA>::encode(const std_msgs::ColorRGBA &rhs)
1038  {
1039  Node node;
1040  ROBOWFLEX_YAML_FLOW(node);
1041 
1042  node.push_back(rhs.r);
1043  node.push_back(rhs.g);
1044  node.push_back(rhs.b);
1045  node.push_back(rhs.a);
1046  return node;
1047  }
1048 
1049  bool convert<std_msgs::ColorRGBA>::decode(const Node &node, std_msgs::ColorRGBA &rhs)
1050  {
1051  rhs = std_msgs::ColorRGBA();
1052 
1053  rhs.r = node[0].as<double>();
1054  rhs.g = node[1].as<double>();
1055  rhs.b = node[2].as<double>();
1056  rhs.a = node[3].as<double>();
1057  return true;
1058  }
1059 
1060  Node convert<moveit_msgs::AllowedCollisionMatrix>::encode(const moveit_msgs::AllowedCollisionMatrix &rhs)
1061  {
1062  Node node;
1063  node["entry_names"] = rhs.entry_names;
1064  ROBOWFLEX_YAML_FLOW(node["entry_names"]);
1065 
1066  node["entry_values"] = rhs.entry_values;
1067 
1068  if (!rhs.default_entry_values.empty())
1069  {
1070  node["default_entry_names"] = rhs.entry_names;
1071  ROBOWFLEX_YAML_FLOW(node["default_entry_names"]);
1072 
1073  std::vector<std::string> default_entry_values;
1074  for (const auto &b : rhs.default_entry_values)
1075  default_entry_values.emplace_back(boolToString(b));
1076 
1077  node["default_entry_values"] = default_entry_values;
1078  ROBOWFLEX_YAML_FLOW(node["default_entry_values"]);
1079  }
1080 
1081  return node;
1082  }
1083 
1084  bool convert<moveit_msgs::AllowedCollisionMatrix>::decode(const Node &node,
1085  moveit_msgs::AllowedCollisionMatrix &rhs)
1086  {
1087  rhs = moveit_msgs::AllowedCollisionMatrix();
1088 
1089  if (IO::isNode(node["entry_names"]))
1090  rhs.entry_names = node["entry_names"].as<std::vector<std::string>>();
1091 
1092  if (IO::isNode(node["entry_values"]))
1093  rhs.entry_values = node["entry_values"].as<std::vector<moveit_msgs::AllowedCollisionEntry>>();
1094 
1095  if (IO::isNode(node["default_entry_names"]))
1096  rhs.default_entry_names = node["default_entry_names"].as<std::vector<std::string>>();
1097 
1098  if (IO::isNode(node["default_entry_values"]))
1099  {
1100  const auto &dev = node["default_entry_values"];
1101  for (const auto &b : dev)
1102  rhs.default_entry_values.push_back(nodeToBool(b));
1103  }
1104 
1105  return true;
1106  }
1107 
1108  Node convert<moveit_msgs::AllowedCollisionEntry>::encode(const moveit_msgs::AllowedCollisionEntry &rhs)
1109  {
1110  Node node;
1111  std::vector<std::string> enabled;
1112  for (const auto &b : rhs.enabled)
1113  enabled.emplace_back(boolToString(b));
1114 
1115  node = enabled;
1116  ROBOWFLEX_YAML_FLOW(node);
1117  return node;
1118  }
1119 
1120  bool convert<moveit_msgs::AllowedCollisionEntry>::decode(const Node &node,
1121  moveit_msgs::AllowedCollisionEntry &rhs)
1122  {
1123  rhs = moveit_msgs::AllowedCollisionEntry();
1124 
1125  for (const auto &b : node)
1126  rhs.enabled.push_back(nodeToBool(b));
1127 
1128  return true;
1129  }
1130 
1131  Node convert<moveit_msgs::PlanningSceneWorld>::encode(const moveit_msgs::PlanningSceneWorld &rhs)
1132  {
1133  Node node;
1134 
1135  if (!rhs.collision_objects.empty())
1136  node["collision_objects"] = rhs.collision_objects;
1137 
1138  if (!rhs.octomap.octomap.data.empty())
1139  node["octomap"] = rhs.octomap;
1140 
1141  return node;
1142  }
1143 
1144  bool convert<moveit_msgs::PlanningSceneWorld>::decode(const Node &node,
1145  moveit_msgs::PlanningSceneWorld &rhs)
1146  {
1147  rhs = moveit_msgs::PlanningSceneWorld();
1148 
1149  if (IO::isNode(node["collision_objects"]))
1150  rhs.collision_objects = node["collision_objects"].as<std::vector<moveit_msgs::CollisionObject>>();
1151 
1152  if (IO::isNode(node["octomap"]))
1153  rhs.octomap = node["octomap"].as<octomap_msgs::OctomapWithPose>();
1154 
1155  return true;
1156  }
1157 
1158  Node convert<octomap_msgs::Octomap>::encode(const octomap_msgs::Octomap &rhs)
1159  {
1160  Node node;
1161 
1162  if (!isHeaderEmpty(rhs.header))
1163  node["header"] = rhs.header;
1164 
1165  node["binary"] = boolToString(rhs.binary);
1166  node["id"] = rhs.id;
1167  node["resolution"] = rhs.resolution;
1168  node["data"] = compressHex(rhs.data);
1169 
1170  return node;
1171  }
1172 
1173  bool convert<octomap_msgs::Octomap>::decode(const Node &node, octomap_msgs::Octomap &rhs)
1174  {
1175  rhs = octomap_msgs::Octomap();
1176 
1177  if (IO::isNode(node["header"]))
1178  rhs.header = node["header"].as<std_msgs::Header>();
1179  else
1180  rhs.header = getDefaultHeader();
1181 
1182  if (IO::isNode(node["binary"]))
1183  rhs.binary = nodeToBool(node["binary"]);
1184 
1185  if (IO::isNode(node["id"]))
1186  rhs.id = node["id"].as<std::string>();
1187 
1188  if (IO::isNode(node["resolution"]))
1189  rhs.resolution = node["resolution"].as<double>();
1190 
1191  if (IO::isNode(node["data"]))
1192  {
1193  // Load old octomap formats / direct YAML output
1194  if (node["data"].IsSequence())
1195  {
1196  auto temp = node["data"].as<std::vector<int>>();
1197  rhs.data = std::vector<int8_t>(temp.begin(), temp.end());
1198  }
1199  else
1200  {
1201  auto temp = node["data"].as<std::string>();
1202  rhs.data = decompressHex(temp);
1203  }
1204  }
1205 
1206  return true;
1207  }
1208 
1209  Node convert<octomap_msgs::OctomapWithPose>::encode(const octomap_msgs::OctomapWithPose &rhs)
1210  {
1211  Node node;
1212 
1213  if (!isHeaderEmpty(rhs.header))
1214  node["header"] = rhs.header;
1215 
1216  node["origin"] = rhs.origin;
1217  node["octomap"] = rhs.octomap;
1218  return node;
1219  }
1220 
1221  bool convert<octomap_msgs::OctomapWithPose>::decode(const Node &node, octomap_msgs::OctomapWithPose &rhs)
1222  {
1223  rhs = octomap_msgs::OctomapWithPose();
1224 
1225  if (IO::isNode(node["header"]))
1226  rhs.header = node["header"].as<std_msgs::Header>();
1227  else
1228  rhs.header = getDefaultHeader();
1229 
1230  if (IO::isNode(node["origin"]))
1231  rhs.origin = node["origin"].as<geometry_msgs::Pose>();
1232 
1233  if (IO::isNode(node["octomap"]))
1234  rhs.octomap = node["octomap"].as<octomap_msgs::Octomap>();
1235 
1236  return true;
1237  }
1238 
1239  Node convert<ros::Time>::encode(const ros::Time &rhs)
1240  {
1241  Node node;
1242  node = rhs.toSec();
1243  return node;
1244  }
1245 
1246  bool convert<ros::Time>::decode(const Node &node, ros::Time &rhs)
1247  {
1248  rhs.fromSec(node.as<double>());
1249  return true;
1250  }
1251 
1252  Node convert<ros::Duration>::encode(const ros::Duration &rhs)
1253  {
1254  Node node;
1255  node = rhs.toSec();
1256  return node;
1257  }
1258 
1259  bool convert<ros::Duration>::decode(const Node &node, ros::Duration &rhs)
1260  {
1261  if (node.Type() == NodeType::Scalar) {
1262  rhs.fromSec(node.as<double>());
1263  return true;
1264  }
1265  try
1266  {
1267  rhs.sec = node["sec"].as<int>();
1268  rhs.nsec = node["nsec"].as<int>();
1269  }
1270  catch (YAML::InvalidNode &e)
1271  {
1272  rhs.sec = node["secs"].as<int>();
1273  rhs.nsec = node["nsecs"].as<int>();
1274  }
1275  return true;
1276  }
1277 
1278  Node convert<shape_msgs::SolidPrimitive>::encode(const shape_msgs::SolidPrimitive &rhs)
1279  {
1280  Node node;
1281  node["type"] = primitiveTypeToString(rhs);
1282  node["dimensions"] = rhs.dimensions;
1283  ROBOWFLEX_YAML_FLOW(node["dimensions"]);
1284  return node;
1285  }
1286 
1287  bool convert<shape_msgs::SolidPrimitive>::decode(const Node &node, shape_msgs::SolidPrimitive &rhs)
1288  {
1289  rhs = shape_msgs::SolidPrimitive();
1290  if (IO::isNode(node["type"]))
1291  nodeToPrimitiveType(node["type"], rhs);
1292 
1293  if (IO::isNode(node["dimensions"]))
1294  rhs.dimensions = node["dimensions"].as<std::vector<double>>();
1295 
1296  return true;
1297  }
1298 
1299  Node convert<shape_msgs::Mesh>::encode(const shape_msgs::Mesh &rhs)
1300  {
1301  Node node;
1302  node["triangles"] = rhs.triangles;
1303  node["vertices"] = rhs.vertices;
1304 
1305  return node;
1306  }
1307 
1308  bool convert<shape_msgs::Mesh>::decode(const Node &node, shape_msgs::Mesh &rhs)
1309  {
1310  rhs = shape_msgs::Mesh();
1311  if (IO::isNode(node["resource"]))
1312  {
1313  std::string resource = node["resource"].as<std::string>();
1314  Eigen::Vector3d dimensions{1, 1, 1};
1315 
1316  if (IO::isNode(node["dimensions"]))
1317  {
1318  Eigen::Vector3d load(node["dimensions"].as<std::vector<double>>().data());
1319  dimensions = load;
1320  }
1321 
1322  Geometry mesh(Geometry::ShapeType::Type::MESH, dimensions, resource);
1323  rhs = mesh.getMeshMsg();
1324  }
1325  else
1326  {
1327  if (IO::isNode(node["triangles"]))
1328  rhs.triangles = node["triangles"].as<std::vector<shape_msgs::MeshTriangle>>();
1329  if (IO::isNode(node["vertices"]))
1330  rhs.vertices = node["vertices"].as<std::vector<geometry_msgs::Point>>();
1331  }
1332  return true;
1333  }
1334 
1335  Node convert<shape_msgs::MeshTriangle>::encode(const shape_msgs::MeshTriangle &rhs)
1336  {
1337  Node node;
1338  node.push_back(rhs.vertex_indices[0]);
1339  node.push_back(rhs.vertex_indices[1]);
1340  node.push_back(rhs.vertex_indices[2]);
1341  ROBOWFLEX_YAML_FLOW(node);
1342  return node;
1343  }
1344 
1345  bool convert<shape_msgs::MeshTriangle>::decode(const Node &node, shape_msgs::MeshTriangle &rhs)
1346  {
1347  rhs.vertex_indices[0] = node[0].as<double>();
1348  rhs.vertex_indices[1] = node[1].as<double>();
1349  rhs.vertex_indices[2] = node[2].as<double>();
1350  return true;
1351  }
1352 
1353  Node convert<shape_msgs::Plane>::encode(const shape_msgs::Plane &rhs)
1354  {
1355  Node node;
1356  node["coef"].push_back(rhs.coef[0]);
1357  node["coef"].push_back(rhs.coef[1]);
1358  node["coef"].push_back(rhs.coef[2]);
1359  node["coef"].push_back(rhs.coef[3]);
1360  ROBOWFLEX_YAML_FLOW(node["coef"]);
1361  return node;
1362  }
1363 
1364  bool convert<shape_msgs::Plane>::decode(const Node &node, shape_msgs::Plane &rhs)
1365  {
1366  rhs.coef[0] = node["coef"][0].as<double>();
1367  rhs.coef[1] = node["coef"][1].as<double>();
1368  rhs.coef[2] = node["coef"][2].as<double>();
1369  rhs.coef[3] = node["coef"][3].as<double>();
1370  return true;
1371  }
1372 
1373  Node convert<moveit_msgs::WorkspaceParameters>::encode(const moveit_msgs::WorkspaceParameters &rhs)
1374  {
1375  Node node;
1376  if (!isHeaderEmpty(rhs.header))
1377  node["header"] = rhs.header;
1378 
1379  node["min_corner"] = rhs.min_corner;
1380  node["max_corner"] = rhs.max_corner;
1381  return node;
1382  }
1383 
1384  bool convert<moveit_msgs::WorkspaceParameters>::decode(const Node &node,
1385  moveit_msgs::WorkspaceParameters &rhs)
1386  {
1387  rhs = moveit_msgs::WorkspaceParameters();
1388 
1389  if (IO::isNode(node["header"]))
1390  rhs.header = node["header"].as<std_msgs::Header>();
1391  else
1392  rhs.header = getDefaultHeader();
1393 
1394  rhs.min_corner = node["min_corner"].as<geometry_msgs::Vector3>();
1395  rhs.max_corner = node["max_corner"].as<geometry_msgs::Vector3>();
1396  return true;
1397  }
1398 
1399  Node convert<moveit_msgs::Constraints>::encode(const moveit_msgs::Constraints &rhs)
1400  {
1401  Node node;
1402 
1403  if (!rhs.name.empty())
1404  node["name"] = rhs.name;
1405 
1406  if (!rhs.joint_constraints.empty())
1407  node["joint_constraints"] = rhs.joint_constraints;
1408 
1409  if (!rhs.position_constraints.empty())
1410  node["position_constraints"] = rhs.position_constraints;
1411 
1412  if (!rhs.orientation_constraints.empty())
1413  node["orientation_constraints"] = rhs.orientation_constraints;
1414 
1415  if (!rhs.visibility_constraints.empty())
1416  node["visibility_constraints"] = rhs.visibility_constraints;
1417 
1418  return node;
1419  }
1420 
1421  bool convert<moveit_msgs::Constraints>::decode(const Node &node, moveit_msgs::Constraints &rhs)
1422  {
1423  rhs = moveit_msgs::Constraints();
1424 
1425  if (IO::isNode(node["name"]))
1426  rhs.name = node["name"].as<std::string>();
1427 
1428  if (IO::isNode(node["joint_constraints"]))
1429  rhs.joint_constraints = node["joint_constraints"].as<std::vector<moveit_msgs::JointConstraint>>();
1430 
1431  if (IO::isNode(node["position_constraints"]))
1432  rhs.position_constraints =
1433  node["position_constraints"].as<std::vector<moveit_msgs::PositionConstraint>>();
1434 
1435  if (IO::isNode(node["orientation_constraints"]))
1436  rhs.orientation_constraints =
1437  node["orientation_constraints"].as<std::vector<moveit_msgs::OrientationConstraint>>();
1438 
1439  if (IO::isNode(node["visibility_constraints"]))
1440  rhs.visibility_constraints =
1441  node["visibility_constraints"].as<std::vector<moveit_msgs::VisibilityConstraint>>();
1442 
1443  return true;
1444  }
1445 
1446  Node convert<moveit_msgs::JointConstraint>::encode(const moveit_msgs::JointConstraint &rhs)
1447  {
1448  Node node;
1449  node["joint_name"] = rhs.joint_name;
1450  node["position"] = rhs.position;
1451 
1452  if (rhs.tolerance_above > std::numeric_limits<double>::epsilon())
1453  node["tolerance_above"] = rhs.tolerance_above;
1454 
1455  if (rhs.tolerance_below > std::numeric_limits<double>::epsilon())
1456  node["tolerance_below"] = rhs.tolerance_below;
1457 
1458  if (rhs.weight < 1)
1459  node["weight"] = rhs.weight;
1460 
1461  return node;
1462  }
1463 
1464  bool convert<moveit_msgs::JointConstraint>::decode(const Node &node, moveit_msgs::JointConstraint &rhs)
1465  {
1466  rhs.joint_name = node["joint_name"].as<std::string>();
1467  rhs.position = node["position"].as<double>();
1468 
1469  if (IO::isNode(node["tolerance_above"]))
1470  rhs.tolerance_above = node["tolerance_above"].as<double>();
1471  else
1472  rhs.tolerance_above = std::numeric_limits<double>::epsilon();
1473 
1474  if (IO::isNode(node["tolerance_below"]))
1475  rhs.tolerance_below = node["tolerance_below"].as<double>();
1476  else
1477  rhs.tolerance_below = std::numeric_limits<double>::epsilon();
1478 
1479  if (IO::isNode(node["weight"]))
1480  rhs.weight = node["weight"].as<double>();
1481  else
1482  rhs.weight = 1;
1483 
1484  return true;
1485  }
1486 
1487  Node convert<moveit_msgs::PositionConstraint>::encode(const moveit_msgs::PositionConstraint &rhs)
1488  {
1489  Node node;
1490  if (!isHeaderEmpty(rhs.header))
1491  node["header"] = rhs.header;
1492 
1493  node["link_name"] = rhs.link_name;
1494 
1495  if (!isVector3Zero(rhs.target_point_offset))
1496  node["target_point_offset"] = rhs.target_point_offset;
1497 
1498  node["constraint_region"] = rhs.constraint_region;
1499 
1500  if (rhs.weight < 1)
1501  node["weight"] = rhs.weight;
1502 
1503  return node;
1504  }
1505 
1506  bool convert<moveit_msgs::PositionConstraint>::decode(const Node &node,
1507  moveit_msgs::PositionConstraint &rhs)
1508  {
1509  rhs = moveit_msgs::PositionConstraint();
1510  rhs.weight = 1;
1511 
1512  if (IO::isNode(node["header"]))
1513  rhs.header = node["header"].as<std_msgs::Header>();
1514  else
1515  rhs.header = getDefaultHeader();
1516 
1517  rhs.link_name = node["link_name"].as<std::string>();
1518  if (IO::isNode(node["target_point_offset"]))
1519  rhs.target_point_offset = node["target_point_offset"].as<geometry_msgs::Vector3>();
1520 
1521  rhs.constraint_region = node["constraint_region"].as<moveit_msgs::BoundingVolume>();
1522  if (IO::isNode(node["weight"]))
1523  rhs.weight = node["weight"].as<double>();
1524  else
1525  rhs.weight = 1;
1526 
1527  return true;
1528  }
1529 
1530  Node convert<moveit_msgs::OrientationConstraint>::encode(const moveit_msgs::OrientationConstraint &rhs)
1531  {
1532  Node node;
1533 
1534  if (!isHeaderEmpty(rhs.header))
1535  node["header"] = rhs.header;
1536 
1537  node["orientation"] = rhs.orientation;
1538  node["link_name"] = rhs.link_name;
1539 
1540  node["absolute_x_axis_tolerance"] = rhs.absolute_x_axis_tolerance;
1541  node["absolute_y_axis_tolerance"] = rhs.absolute_y_axis_tolerance;
1542  node["absolute_z_axis_tolerance"] = rhs.absolute_z_axis_tolerance;
1543 
1544  if (rhs.weight < 1)
1545  node["weight"] = rhs.weight;
1546 
1547  return node;
1548  }
1549 
1550  bool convert<moveit_msgs::OrientationConstraint>::decode(const Node &node,
1551  moveit_msgs::OrientationConstraint &rhs)
1552  {
1553  if (IO::isNode(node["header"]))
1554  rhs.header = node["header"].as<std_msgs::Header>();
1555  else
1556  rhs.header = getDefaultHeader();
1557 
1558  rhs.orientation = node["orientation"].as<geometry_msgs::Quaternion>();
1559  rhs.link_name = node["link_name"].as<std::string>();
1560 
1561  rhs.absolute_x_axis_tolerance = node["absolute_x_axis_tolerance"].as<double>();
1562  rhs.absolute_y_axis_tolerance = node["absolute_y_axis_tolerance"].as<double>();
1563  rhs.absolute_z_axis_tolerance = node["absolute_z_axis_tolerance"].as<double>();
1564 
1565  if (IO::isNode(node["weight"]))
1566  rhs.weight = node["weight"].as<double>();
1567  else
1568  rhs.weight = 1;
1569 
1570  return true;
1571  }
1572 
1573  Node convert<moveit_msgs::VisibilityConstraint>::encode(const moveit_msgs::VisibilityConstraint &rhs)
1574  {
1575  Node node;
1576  node["target_radius"] = rhs.target_radius;
1577  node["target_pose"] = rhs.target_pose;
1578  node["cone_sides"] = rhs.cone_sides;
1579  node["sensor_pose"] = rhs.sensor_pose;
1580  node["max_view_angle"] = rhs.max_view_angle;
1581  node["max_range_angle"] = rhs.max_range_angle;
1582  node["sensor_view_direction"] = rhs.sensor_view_direction;
1583  node["weight"] = rhs.weight;
1584  return node;
1585  }
1586 
1587  bool convert<moveit_msgs::VisibilityConstraint>::decode(const Node &node,
1588  moveit_msgs::VisibilityConstraint &rhs)
1589  {
1590  rhs = moveit_msgs::VisibilityConstraint();
1591 
1592  rhs.target_radius = node["target_radius"].as<double>();
1593  rhs.target_pose = node["target_pose"].as<geometry_msgs::PoseStamped>();
1594  rhs.cone_sides = node["cone_sides"].as<int>();
1595  rhs.sensor_pose = node["sensor_pose"].as<geometry_msgs::PoseStamped>();
1596  rhs.max_view_angle = node["max_view_angle"].as<double>();
1597  rhs.max_range_angle = node["max_range_angle"].as<double>();
1598  rhs.sensor_view_direction = node["sensor_view_direction"].as<int>();
1599  rhs.weight = node["weight"].as<double>();
1600 
1601  return true;
1602  }
1603 
1604  Node convert<moveit_msgs::BoundingVolume>::encode(const moveit_msgs::BoundingVolume &rhs)
1605  {
1606  Node node;
1607 
1608  if (!rhs.primitives.empty())
1609  {
1610  node["primitives"] = rhs.primitives;
1611  node["primitive_poses"] = rhs.primitive_poses;
1612  }
1613 
1614  if (!rhs.meshes.empty())
1615  {
1616  node["meshes"] = rhs.meshes;
1617  node["mesh_poses"] = rhs.mesh_poses;
1618  }
1619 
1620  return node;
1621  }
1622 
1623  bool convert<moveit_msgs::BoundingVolume>::decode(const Node &node, moveit_msgs::BoundingVolume &rhs)
1624  {
1625  rhs = moveit_msgs::BoundingVolume();
1626 
1627  if (IO::isNode(node["primitives"]))
1628  {
1629  rhs.primitives = node["primitives"].as<std::vector<shape_msgs::SolidPrimitive>>();
1630  rhs.primitive_poses = node["primitive_poses"].as<std::vector<geometry_msgs::Pose>>();
1631  }
1632 
1633  if (IO::isNode(node["meshes"]))
1634  {
1635  rhs.meshes = node["meshes"].as<std::vector<shape_msgs::Mesh>>();
1636  rhs.mesh_poses = node["mesh_poses"].as<std::vector<geometry_msgs::Pose>>();
1637  }
1638 
1639  return true;
1640  }
1641 
1642  Node convert<moveit_msgs::TrajectoryConstraints>::encode(const moveit_msgs::TrajectoryConstraints &rhs)
1643  {
1644  Node node;
1645  node["constraints"] = rhs.constraints;
1646  return node;
1647  }
1648 
1649  bool convert<moveit_msgs::TrajectoryConstraints>::decode(const Node &node,
1650  moveit_msgs::TrajectoryConstraints &rhs)
1651  {
1652  rhs.constraints = node["constraints"].as<std::vector<moveit_msgs::Constraints>>();
1653  return true;
1654  }
1655 
1656  Node convert<moveit_msgs::MotionPlanRequest>::encode(const moveit_msgs::MotionPlanRequest &rhs)
1657  {
1658  Node node;
1659 
1660  if (!(isHeaderEmpty(rhs.workspace_parameters.header) &&
1661  isVector3Zero(rhs.workspace_parameters.min_corner) &&
1662  isVector3Zero(rhs.workspace_parameters.max_corner)))
1663  node["workspace_parameters"] = rhs.workspace_parameters;
1664 
1665  node["start_state"] = rhs.start_state;
1666 
1667  if (!rhs.goal_constraints.empty())
1668  node["goal_constraints"] = rhs.goal_constraints;
1669 
1670  if (!isConstraintEmpty(rhs.path_constraints))
1671  node["path_constraints"] = rhs.path_constraints;
1672 
1673  if (!rhs.trajectory_constraints.constraints.empty())
1674  node["trajectory_constraints"] = rhs.trajectory_constraints;
1675 
1676  if (!rhs.planner_id.empty())
1677  node["planner_id"] = rhs.planner_id;
1678 
1679  if (!rhs.group_name.empty())
1680  node["group_name"] = rhs.group_name;
1681 
1682  if (rhs.num_planning_attempts != 0)
1683  node["num_planning_attempts"] = rhs.num_planning_attempts;
1684 
1685  if (rhs.allowed_planning_time != 0)
1686  node["allowed_planning_time"] = rhs.allowed_planning_time;
1687 
1688  if (rhs.max_velocity_scaling_factor < 1)
1689  node["max_velocity_scaling_factor"] = rhs.max_velocity_scaling_factor;
1690 
1691  if (rhs.max_acceleration_scaling_factor < 1)
1692  node["max_acceleration_scaling_factor"] = rhs.max_acceleration_scaling_factor;
1693 
1694  return node;
1695  }
1696 
1697  bool convert<moveit_msgs::MotionPlanRequest>::decode(const Node &node,
1698  moveit_msgs::MotionPlanRequest &rhs)
1699  {
1700  rhs = moveit_msgs::MotionPlanRequest();
1701 
1702  if (IO::isNode(node["workspace_parameters"]))
1703  rhs.workspace_parameters = node["workspace_parameters"].as<moveit_msgs::WorkspaceParameters>();
1704 
1705  if (IO::isNode(node["start_state"]))
1706  rhs.start_state = node["start_state"].as<moveit_msgs::RobotState>();
1707 
1708  if (IO::isNode(node["goal_constraints"]))
1709  rhs.goal_constraints = node["goal_constraints"].as<std::vector<moveit_msgs::Constraints>>();
1710 
1711  if (IO::isNode(node["path_constraints"]))
1712  rhs.path_constraints = node["path_constraints"].as<moveit_msgs::Constraints>();
1713 
1714  if (IO::isNode(node["trajectory_constraints"]))
1715  rhs.trajectory_constraints =
1716  node["trajectory_constraints"].as<moveit_msgs::TrajectoryConstraints>();
1717 
1718  if (IO::isNode(node["planner_id"]))
1719  rhs.planner_id = node["planner_id"].as<std::string>();
1720 
1721  if (IO::isNode(node["group_name"]))
1722  rhs.group_name = node["group_name"].as<std::string>();
1723 
1724  if (IO::isNode(node["num_planning_attempts"]))
1725  rhs.num_planning_attempts = node["num_planning_attempts"].as<int>();
1726  else
1727  rhs.num_planning_attempts = 0;
1728 
1729  if (IO::isNode(node["allowed_planning_time"]))
1730  rhs.allowed_planning_time = node["allowed_planning_time"].as<double>();
1731  else
1732  rhs.allowed_planning_time = 0;
1733 
1734  if (IO::isNode(node["max_velocity_scaling_factor"]))
1735  rhs.max_velocity_scaling_factor = node["max_velocity_scaling_factor"].as<double>();
1736  else
1737  rhs.max_velocity_scaling_factor = 1;
1738 
1739  if (IO::isNode(node["max_acceleration_scaling_factor"]))
1740  rhs.max_acceleration_scaling_factor = node["max_acceleration_scaling_factor"].as<double>();
1741  else
1742  rhs.max_acceleration_scaling_factor = 1;
1743 
1744  return true;
1745  }
1746 
1747  Node convert<trajectory_msgs::MultiDOFJointTrajectory>::encode(
1748  const trajectory_msgs::MultiDOFJointTrajectory &rhs)
1749  {
1750  Node node;
1751 
1752  if (!isHeaderEmpty(rhs.header))
1753  node["header"] = rhs.header;
1754 
1755  node["joint_names"] = rhs.joint_names;
1756  node["points"] = rhs.points;
1757 
1758  return node;
1759  }
1760 
1761  bool convert<trajectory_msgs::MultiDOFJointTrajectory>::decode(
1762  const Node &node, trajectory_msgs::MultiDOFJointTrajectory &rhs)
1763  {
1764  rhs = trajectory_msgs::MultiDOFJointTrajectory();
1765 
1766  if (IO::isNode(node["header"]))
1767  rhs.header = node["header"].as<std_msgs::Header>();
1768 
1769  if (IO::isNode(node["joint_names"]))
1770  rhs.joint_names = node["joint_names"].as<std::vector<std::string>>();
1771 
1772  if (IO::isNode(node["points"]))
1773  rhs.points = node["points"].as<std::vector<trajectory_msgs::MultiDOFJointTrajectoryPoint>>();
1774 
1775  return true;
1776  }
1777 
1778  Node convert<trajectory_msgs::MultiDOFJointTrajectoryPoint>::encode(
1779  const trajectory_msgs::MultiDOFJointTrajectoryPoint &rhs)
1780  {
1781  Node node;
1782 
1783  if (!rhs.transforms.empty())
1784  node["transforms"] = rhs.transforms;
1785 
1786  if (!rhs.velocities.empty())
1787  node["velocities"] = rhs.velocities;
1788 
1789  if (!rhs.accelerations.empty())
1790  node["accelerations"] = rhs.accelerations;
1791 
1792  node["time_from_start"] = rhs.time_from_start;
1793 
1794  return node;
1795  }
1796 
1797  bool convert<trajectory_msgs::MultiDOFJointTrajectoryPoint>::decode(
1798  const Node &node, trajectory_msgs::MultiDOFJointTrajectoryPoint &rhs)
1799  {
1800  rhs = trajectory_msgs::MultiDOFJointTrajectoryPoint();
1801 
1802  if (IO::isNode(node["transforms"]))
1803  rhs.transforms = node["transforms"].as<std::vector<geometry_msgs::Transform>>();
1804 
1805  if (IO::isNode(node["velocities"]))
1806  rhs.velocities = node["velocities"].as<std::vector<geometry_msgs::Twist>>();
1807 
1808  if (IO::isNode(node["accelerations"]))
1809  rhs.accelerations = node["accelerations"].as<std::vector<geometry_msgs::Twist>>();
1810 
1811  rhs.time_from_start = node["time_from_start"].as<ros::Duration>();
1812  return true;
1813  }
1814 
1815  Node convert<moveit_msgs::RobotTrajectory>::encode(const moveit_msgs::RobotTrajectory &rhs)
1816  {
1817  Node node;
1818 
1819  if (!rhs.joint_trajectory.points.empty())
1820  node["joint_trajectory"] = rhs.joint_trajectory;
1821 
1822  if (!rhs.multi_dof_joint_trajectory.points.empty())
1823  node["multi_dof_joint_trajectory"] = rhs.multi_dof_joint_trajectory;
1824 
1825  return node;
1826  }
1827 
1828  bool convert<moveit_msgs::RobotTrajectory>::decode(const Node &node, moveit_msgs::RobotTrajectory &rhs)
1829  {
1830  rhs = moveit_msgs::RobotTrajectory();
1831 
1832  if (IO::isNode(node["joint_trajectory"]))
1833  rhs.joint_trajectory = node["joint_trajectory"].as<trajectory_msgs::JointTrajectory>();
1834 
1835  if (IO::isNode(node["multi_dof_joint_trajectory"]))
1836  rhs.multi_dof_joint_trajectory =
1837  node["multi_dof_joint_trajectory"].as<trajectory_msgs::MultiDOFJointTrajectory>();
1838 
1839  return true;
1840  }
1841 } // namespace YAML
1842 
1843 namespace robowflex
1844 {
1845  namespace IO
1846  {
1847  bool isNode(const YAML::Node &node)
1848  {
1849  try
1850  {
1851  bool r = node.IsDefined() and not node.IsNull();
1852  if (r)
1853  try
1854  {
1855  r = node.as<std::string>() != "~";
1856  }
1857  catch (std::exception &e)
1858  {
1859  }
1860 
1861  return r;
1862  }
1863  catch (YAML::InvalidNode &e)
1864  {
1865  return false;
1866  }
1867  }
1868 
1869  moveit_msgs::RobotState robotStateFromNode(const YAML::Node &node)
1870  {
1871  return node.as<moveit_msgs::RobotState>();
1872  }
1873 
1874  YAML::Node toNode(const geometry_msgs::Pose &msg)
1875  {
1876  YAML::Node node;
1877  node = msg;
1878  return node;
1879  }
1880 
1881  geometry_msgs::Pose poseFromNode(const YAML::Node &node)
1882  {
1883  return node.as<geometry_msgs::Pose>();
1884  }
1885 
1886  YAML::Node toNode(const moveit_msgs::PlanningScene &msg)
1887  {
1888  YAML::Node node;
1889  node = msg;
1890  return node;
1891  }
1892 
1893  YAML::Node toNode(const moveit_msgs::MotionPlanRequest &msg)
1894  {
1895  YAML::Node node;
1896  node = msg;
1897  return node;
1898  }
1899 
1900  YAML::Node toNode(const moveit_msgs::RobotTrajectory &msg)
1901  {
1902  YAML::Node node;
1903  node = msg;
1904  return node;
1905  }
1906 
1907  YAML::Node toNode(const moveit_msgs::RobotState &msg)
1908  {
1909  YAML::Node node;
1910  node = msg;
1911  return node;
1912  }
1913 
1914  bool fromYAMLFile(moveit_msgs::PlanningScene &msg, const std::string &file)
1915  {
1916  return IO::YAMLFileToMessage(msg, file);
1917  }
1918 
1919  bool fromYAMLFile(moveit_msgs::MotionPlanRequest &msg, const std::string &file)
1920  {
1921  return IO::YAMLFileToMessage(msg, file);
1922  }
1923 
1924  bool fromYAMLFile(moveit_msgs::RobotState &msg, const std::string &file)
1925  {
1926  return IO::YAMLFileToMessage(msg, file);
1927  }
1928  } // namespace IO
1929 } // namespace robowflex
std::back_inserter
T back_inserter(T... args)
std::string::begin
T begin(T... args)
robowflex::Geometry
A class that manages both solid and mesh geometry for various parts of the motion planning system.
Definition: geometry.h:36
robowflex::Geometry::getMeshMsg
const shape_msgs::Mesh getMeshMsg() const
Gets the message form of mesh geometry.
Definition: geometry.cpp:265
std::vector::data
T data(T... args)
std::vector::emplace_back
T emplace_back(T... args)
std::string::end
T end(T... args)
std::numeric_limits::epsilon
T epsilon(T... args)
std::numeric_limits::infinity
T infinity(T... args)
ROBOWFLEX_YAML_FLOW
#define ROBOWFLEX_YAML_FLOW(n)
Definition: macros.h:54
YAML
Definition: yaml.cpp:174
robowflex::IO::isNode
bool isNode(const YAML::Node &node)
Checks if a key exists within a YAML node.
Definition: yaml.cpp:1847
robowflex::IO::fromYAMLFile
bool fromYAMLFile(moveit_msgs::PlanningScene &msg, const std::string &file)
Loads a planning scene from a YAML file.
Definition: yaml.cpp:1914
robowflex::IO::robotStateFromNode
moveit_msgs::RobotState robotStateFromNode(const YAML::Node &node)
Converts a robot state YAML to a robot_state message.
Definition: yaml.cpp:1869
robowflex::IO::poseFromNode
geometry_msgs::Pose poseFromNode(const YAML::Node &node)
Converts a pose YAML to a goemetry message.
Definition: yaml.cpp:1881
robowflex::IO::toNode
YAML::Node toNode(const geometry_msgs::Pose &msg)
Converts a pose message to YAML.
Definition: yaml.cpp:1874
robowflex::IO::YAMLFileToMessage
bool YAMLFileToMessage(T &msg, const std::string &file)
Load a message (or YAML convertable object) from a file.
Definition: robowflex_library/include/robowflex_library/io.h:191
robowflex::TF::poseMsgToEigen
RobotPose poseMsgToEigen(const geometry_msgs::Pose &msg)
Converts a pose message to RobotPose.
Definition: tf.cpp:114
robowflex::TF::poseEigenToMsg
geometry_msgs::Pose poseEigenToMsg(const RobotPose &pose)
Converts an RobotPose to a pose message.
Definition: tf.cpp:120
robowflex::TF::identity
RobotPose identity()
Creates the Identity pose.
Definition: tf.cpp:9
robowflex
Main namespace. Contains all library classes and functions.
Definition: scene.cpp:25
robowflex::RobotPose
std::decay< decltype(std::declval< moveit::core::Transforms >().getTransform("")) >::type RobotPose
A pose (point in SE(3)) used in various functions. Defined from what MoveIt! uses.
Definition: adapter.h:24
std::stringstream::precision
T precision(T... args)
std::vector::push_back
T push_back(T... args)
std::numeric_limits::quiet_NaN
T quiet_NaN(T... args)
io.h
std::stringstream::str
T str(T... args)
std::transform
T transform(T... args)
std::stringstream::unsetf
T unsetf(T... args)
std::ws
T ws(T... args)