Add AllowedCollisionMatrix and CollisionMarginData to yaml extensions (#1152)

Author: Levi-Armstrong

tesseract_common/include/tesseract_common/allowed_collision_matrix.h

@@ -36,6 +36,7 @@ class AllowedCollisionMatrix
   using ConstPtr = std::shared_ptr<const AllowedCollisionMatrix>;
 
   AllowedCollisionMatrix() = default;
+  AllowedCollisionMatrix(const AllowedCollisionEntries& entries);
   virtual ~AllowedCollisionMatrix() = default;
   AllowedCollisionMatrix(const AllowedCollisionMatrix&) = default;
   AllowedCollisionMatrix& operator=(const AllowedCollisionMatrix&) = default;

tesseract_common/include/tesseract_common/yaml_extenstions.h

@@ -34,6 +34,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
 #include <tesseract_common/plugin_info.h>
 #include <tesseract_common/calibration_info.h>
+#include <tesseract_common/collision_margin_data.h>
 
 namespace YAML
 {
@@ -669,6 +670,194 @@ struct convert<tesseract_common::Toolpath>
     return true;
   }
 };
+
+//=========================== CollisionMarginPairOverrideType Enum ===========================
+template <>
+struct convert<tesseract_common::CollisionMarginPairOverrideType>
+{
+  static Node encode(const tesseract_common::CollisionMarginPairOverrideType& rhs)
+  {
+    // LCOV_EXCL_START
+    static const std::map<tesseract_common::CollisionMarginPairOverrideType, std::string> m = {
+      { tesseract_common::CollisionMarginPairOverrideType::NONE, "NONE" },
+      { tesseract_common::CollisionMarginPairOverrideType::MODIFY, "MODIFY" },
+      { tesseract_common::CollisionMarginPairOverrideType::REPLACE, "REPLACE" }
+    };
+    // LCOV_EXCL_STOP
+    return Node(m.at(rhs));
+  }
+
+  static bool decode(const Node& node, tesseract_common::CollisionMarginPairOverrideType& rhs)
+  {
+    // LCOV_EXCL_START
+    static const std::map<std::string, tesseract_common::CollisionMarginPairOverrideType> inv = {
+      { "NONE", tesseract_common::CollisionMarginPairOverrideType::NONE },
+      { "MODIFY", tesseract_common::CollisionMarginPairOverrideType::MODIFY },
+      { "REPLACE", tesseract_common::CollisionMarginPairOverrideType::REPLACE }
+    };
+    // LCOV_EXCL_STOP
+
+    if (!node.IsScalar())
+      return false;
+
+    auto it = inv.find(node.Scalar());
+    if (it == inv.end())
+      return false;
+
+    rhs = it->second;
+    return true;
+  }
+};
+
+//============================ PairsCollisionMarginData ============================
+template <>
+struct convert<tesseract_common::PairsCollisionMarginData>
+{
+  static Node encode(const tesseract_common::PairsCollisionMarginData& rhs)
+  {
+    Node node(NodeType::Map);
+    for (const auto& pair : rhs)
+    {
+      Node key_node(NodeType::Sequence);
+      key_node.push_back(pair.first.first);
+      key_node.push_back(pair.first.second);
+
+      // tell yaml-cpp “emit this sequence in [a, b] inline style”
+      key_node.SetStyle(YAML::EmitterStyle::Flow);
+
+      node[key_node] = pair.second;
+    }
+
+    return node;
+  }
+
+  static bool decode(const Node& node, tesseract_common::PairsCollisionMarginData& rhs)
+  {
+    if (!node.IsMap())
+      return false;
+
+    for (auto it = node.begin(); it != node.end(); ++it)
+    {
+      Node key_node = it->first;
+      if (!key_node.IsSequence() || key_node.size() != 2)
+        return false;
+
+      std::pair<std::string, std::string> key;
+      key.first = key_node[0].as<std::string>();
+      key.second = key_node[1].as<std::string>();
+
+      auto v = it->second.as<double>();
+      rhs.emplace(std::move(key), v);
+    }
+    return true;
+  }
+};
+
+//================================== CollisionMarginPairData =================================
+template <>
+struct convert<tesseract_common::CollisionMarginPairData>
+{
+  static Node encode(const tesseract_common::CollisionMarginPairData& rhs)
+  {
+    const tesseract_common::PairsCollisionMarginData& data = rhs.getCollisionMargins();
+    return Node(data);
+  }
+
+  static bool decode(const Node& node, tesseract_common::CollisionMarginPairData& rhs)
+  {
+    auto data = node.as<tesseract_common::PairsCollisionMarginData>();
+    rhs = tesseract_common::CollisionMarginPairData(data);
+    return true;
+  }
+};
+
+//============================ AllowedCollisionEntries ============================
+template <>
+struct convert<tesseract_common::AllowedCollisionEntries>
+{
+  static Node encode(const tesseract_common::AllowedCollisionEntries& rhs)
+  {
+    Node node(NodeType::Map);
+    for (const auto& pair : rhs)
+    {
+      Node key_node(NodeType::Sequence);
+      key_node.push_back(pair.first.first);
+      key_node.push_back(pair.first.second);
+
+      // tell yaml-cpp “emit this sequence in [a, b] inline style”
+      key_node.SetStyle(YAML::EmitterStyle::Flow);
+
+      node[key_node] = pair.second;
+    }
+
+    return node;
+  }
+
+  static bool decode(const Node& node, tesseract_common::AllowedCollisionEntries& rhs)
+  {
+    if (!node.IsMap())
+      return false;
+
+    for (auto it = node.begin(); it != node.end(); ++it)
+    {
+      Node key_node = it->first;
+      if (!key_node.IsSequence() || key_node.size() != 2)
+        return false;
+
+      std::pair<std::string, std::string> key;
+      key.first = key_node[0].as<std::string>();
+      key.second = key_node[1].as<std::string>();
+
+      auto v = it->second.as<std::string>();
+      rhs.emplace(std::move(key), v);
+    }
+    return true;
+  }
+};
+
+//================================== AllowedCollisionMatrix =================================
+template <>
+struct convert<tesseract_common::AllowedCollisionMatrix>
+{
+  static Node encode(const tesseract_common::AllowedCollisionMatrix& rhs)
+  {
+    const tesseract_common::AllowedCollisionEntries& data = rhs.getAllAllowedCollisions();
+    return Node(data);
+  }
+
+  static bool decode(const Node& node, tesseract_common::AllowedCollisionMatrix& rhs)
+  {
+    auto data = node.as<tesseract_common::AllowedCollisionEntries>();
+    rhs = tesseract_common::AllowedCollisionMatrix(data);
+    return true;
+  }
+};
+
+//============================== std::unordered_map<std::string, bool> =============================
+template <>
+struct convert<std::unordered_map<std::string, bool>>
+{
+  static Node encode(const std::unordered_map<std::string, bool>& rhs)
+  {
+    Node node(NodeType::Map);
+    for (const auto& pair : rhs)
+      node[pair.first] = pair.second;
+
+    return node;
+  }
+
+  static bool decode(const Node& node, std::unordered_map<std::string, bool>& rhs)
+  {
+    if (!node.IsMap())
+      return false;
+
+    for (auto it = node.begin(); it != node.end(); ++it)
+      rhs[it->first.as<std::string>()] = it->second.as<bool>();
+
+    return true;
+  }
+};
+
 }  // namespace YAML
 
 #endif  // TESSERACT_COMMON_YAML_EXTENSTIONS_H

tesseract_common/src/allowed_collision_matrix.cpp

@@ -57,6 +57,15 @@ bool operator==(const AllowedCollisionEntries& entries_1, const AllowedCollision
   return true;
 }
 
+AllowedCollisionMatrix::AllowedCollisionMatrix(const AllowedCollisionEntries& entries)
+{
+  for (const auto& entry : entries)
+  {
+    auto link_pair = tesseract_common::makeOrderedLinkPair(entry.first.first, entry.first.second);
+    lookup_table_[link_pair] = entry.second;
+  }
+}
+
 void AllowedCollisionMatrix::addAllowedCollision(const std::string& link_name1,
                                                  const std::string& link_name2,
                                                  const std::string& reason)