Support message conversions

tesseract-robotics · Nov 10, 2023 · b9c3a02 · b9c3a02
1 parent 09f911d
commit b9c3a02
Show file tree

Hide file tree

Showing 3 changed files with 20 additions and 0 deletions.
diff --git a/tesseract_common/include/tesseract_common/allowed_collision_matrix.h b/tesseract_common/include/tesseract_common/allowed_collision_matrix.h
@@ -115,6 +115,15 @@ class AllowedCollisionMatrix
     lookup_table_.insert(acm.getAllAllowedCollisions().begin(), acm.getAllAllowedCollisions().end());
   }
 
+  /**
+   * @brief Reserve space for the internal data storage
+   * @param size The size to reserve
+   */
+  void reserveAllowedCollisionMatrix(std::size_t size)
+  {
+    lookup_table_.reserve(size);
+  }
+
   friend std::ostream& operator<<(std::ostream& os, const AllowedCollisionMatrix& acm)
   {
     for (const auto& pair : acm.getAllAllowedCollisions())

diff --git a/tesseract_scene_graph/include/tesseract_scene_graph/graph.h b/tesseract_scene_graph/include/tesseract_scene_graph/graph.h
@@ -347,6 +347,12 @@ class SceneGraph
    */
   JointLimits::ConstPtr getJointLimits(const std::string& name);
 
+  /**
+   * @brief Set the allowed collision matrix
+   * @param acm The allowed collision matrix to assign
+   */
+  void setAllowedCollisionMatrix(tesseract_common::AllowedCollisionMatrix::Ptr acm);
+
   /**
    * @brief Disable collision between two collision objects
    * @param link_name1 Collision object name

diff --git a/tesseract_scene_graph/src/graph.cpp b/tesseract_scene_graph/src/graph.cpp
@@ -624,6 +624,11 @@ JointLimits::ConstPtr SceneGraph::getJointLimits(const std::string& name)
   return found->second.first->limits;
 }
 
+void SceneGraph::setAllowedCollisionMatrix(tesseract_common::AllowedCollisionMatrix::Ptr acm)
+{
+  acm_ = std::move(acm);
+}
+
 void SceneGraph::addAllowedCollision(const std::string& link_name1,
                                      const std::string& link_name2,
                                      const std::string& reason)