Add pybinds for saving and loading geometry from a .scene file (#2971)

moveit_py/moveit/core/planning_scene.pyi

@@ -19,6 +19,8 @@ class PlanningScene:
     def process_planning_scene_world(self, *args, **kwargs) -> Any: ...
     def remove_all_collision_objects(self, *args, **kwargs) -> Any: ...
     def set_object_color(self, *args, **kwargs) -> Any: ...
+    def save_geometry_to_file(self, file_name_and_path) -> Any: ...
+    def load_geometry_from_file(self, file_name_and_path) -> Any: ...
     def __copy__(self) -> Any: ...
     def __deepcopy__(self) -> Any: ...
     @property

moveit_py/src/moveit/moveit_core/planning_scene/planning_scene.cpp

@@ -38,6 +38,33 @@
 #include <moveit_py/moveit_py_utils/ros_msg_typecasters.h>
 #include <pybind11/operators.h>
 
+#include <fstream>
+
+namespace
+{
+bool saveGeometryToFile(std::shared_ptr<planning_scene::PlanningScene>& planning_scene,
+                        const std::string& file_path_and_name)
+{
+  std::ofstream file(file_path_and_name);
+  if (!file.is_open())
+  {
+    return false;
+  }
+  planning_scene->saveGeometryToStream(file);
+  file.close();
+  return true;
+}
+
+bool loadGeometryFromFile(std::shared_ptr<planning_scene::PlanningScene>& planning_scene,
+                          const std::string& file_path_and_name)
+{
+  std::ifstream file(file_path_and_name);
+  planning_scene->loadGeometryFromStream(file);
+  file.close();
+  return true;
+}
+}  // namespace
+
 namespace moveit_py
 {
 namespace bind_planning_scene
@@ -431,6 +458,28 @@ void initPlanningScene(py::module& m)
 
 	   Returns:
                bool: true if state is in self collision otherwise false.
+           )")
+
+      .def("save_geometry_to_file", &saveGeometryToFile, py::arg("file_path_and_name"),
+           R"(
+           Save the CollisionObjects in the PlanningScene to a file
+
+     Args:
+               file_path_and_name (str): The file to save the CollisionObjects to.
+
+     Returns:
+               bool: true if save to file was successful otherwise false.
+           )")
+
+      .def("load_geometry_from_file", &loadGeometryFromFile, py::arg("file_path_and_name"),
+           R"(
+           Load the CollisionObjects from a file to the PlanningScene
+
+     Args:
+               file_path_and_name (str): The file to load the CollisionObjects from.
+
+     Returns:
+               bool: true if load from file was successful otherwise false.
            )");
 }
 }  // namespace bind_planning_scene