diff --git a/moveit_py/moveit/core/planning_scene.pyi b/moveit_py/moveit/core/planning_scene.pyi index e5f2363780..9cc633ddcb 100644 --- a/moveit_py/moveit/core/planning_scene.pyi +++ b/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 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 index 5df1c18f30..e55b0f15d4 100644 --- a/moveit_py/src/moveit/moveit_core/planning_scene/planning_scene.cpp +++ b/moveit_py/src/moveit/moveit_core/planning_scene/planning_scene.cpp @@ -38,6 +38,33 @@ #include #include +#include + +namespace +{ +bool saveGeometryToFile(std::shared_ptr& 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, + 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