Allow editing allowed collision matrix in python + fix get_entry func…

…tion (#2551) Co-authored-by: Sebastian Jahr <[email protected]>
moveit · Nov 27, 2023 · 0742736 · 0742736
1 parent c2292a7
commit 0742736
Show file tree

Hide file tree

Showing 4 changed files with 35 additions and 7 deletions.
diff --git a/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h b/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h
@@ -291,9 +291,13 @@ class MOVEIT_PLANNING_SCENE_EXPORT PlanningScene : public std::enable_shared_fro
   {
     return acm_ ? *acm_ : parent_->getAllowedCollisionMatrix();
   }
+
   /** \brief Get the allowed collision matrix */
   collision_detection::AllowedCollisionMatrix& getAllowedCollisionMatrixNonConst();
 
+  /** \brief Set the allowed collision matrix */
+  void setAllowedCollisionMatrix(const collision_detection::AllowedCollisionMatrix& acm);
+
   /**@}*/
 
   /**

diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp
@@ -558,6 +558,11 @@ collision_detection::AllowedCollisionMatrix& PlanningScene::getAllowedCollisionM
   return *acm_;
 }
 
+void PlanningScene::setAllowedCollisionMatrix(const collision_detection::AllowedCollisionMatrix& acm)
+{
+  getAllowedCollisionMatrixNonConst() = acm;
+}
+
 const moveit::core::Transforms& PlanningScene::getTransforms()
 {
   // Trigger an update of the robot transforms

diff --git a/moveit_py/src/moveit/moveit_core/collision_detection/collision_matrix.cpp b/moveit_py/src/moveit/moveit_core/collision_detection/collision_matrix.cpp
@@ -40,16 +40,34 @@ namespace moveit_py
 {
 namespace bind_collision_detection
 {
-std::pair<bool, collision_detection::AllowedCollision::Type>
-getEntry(const std::shared_ptr<collision_detection::AllowedCollisionMatrix>& acm, const std::string& name1,
-         const std::string& name2)
+// TODO: Create a custom typecaster/revise the current implementation to return std::pair<bool,
+// collision_detection::AllowedCollision::Type>
+std::pair<bool, std::string> getEntry(const collision_detection::AllowedCollisionMatrix& acm, const std::string& name1,
+                                      const std::string& name2)
 {
   // check acm for collision
   collision_detection::AllowedCollision::Type type;
-  bool collision_allowed = acm->getEntry(name1, name2, type);
+  bool collision_allowed = acm.getEntry(name1, name2, type);
+  std::string type_str;
+  if (type == collision_detection::AllowedCollision::Type::NEVER)
+  {
+    type_str = "NEVER";
+  }
+  else if (type == collision_detection::AllowedCollision::Type::ALWAYS)
+  {
+    type_str = "ALWAYS";
+  }
+  else if (type == collision_detection::AllowedCollision::Type::CONDITIONAL)
+  {
+    type_str = "CONDITIONAL";
+  }
+  else
+  {
+    type_str = "UNKNOWN";
+  }
 
   // should return a tuple true/false and the allowed collision type
-  std::pair<bool, collision_detection::AllowedCollision::Type> result = std::make_pair(collision_allowed, type);
+  std::pair<bool, std::string> result = std::make_pair(collision_allowed, type_str);
   return result;
 }
 

diff --git a/moveit_py/src/moveit/moveit_core/planning_scene/planning_scene.cpp b/moveit_py/src/moveit/moveit_core/planning_scene/planning_scene.cpp
@@ -111,8 +111,9 @@ void initPlanningScene(py::module& m)
                     py::return_value_policy::move)
 
       .def_property("transforms", py::overload_cast<>(&planning_scene::PlanningScene::getTransforms), nullptr)
-      .def_property("allowed_collision_matrix", &planning_scene::PlanningScene::getAllowedCollisionMatrix, nullptr,
-                    py::return_value_policy::move)
+      .def_property("allowed_collision_matrix", &planning_scene::PlanningScene::getAllowedCollisionMatrix,
+                    &planning_scene::PlanningScene::setAllowedCollisionMatrix,
+                    py::return_value_policy::reference_internal)
       // methods
       .def("__copy__",
            [](const planning_scene::PlanningScene* self) {