diff --git a/moveit_ros/move_group/CMakeLists.txt b/moveit_ros/move_group/CMakeLists.txt
index 6c4cdc9521..bd48293e7a 100644
--- a/moveit_ros/move_group/CMakeLists.txt
+++ b/moveit_ros/move_group/CMakeLists.txt
@@ -46,9 +46,11 @@ add_library(
src/default_capabilities/get_group_urdf_capability.cpp
src/default_capabilities/get_planning_scene_service_capability.cpp
src/default_capabilities/kinematics_service_capability.cpp
+ src/default_capabilities/load_geometry_from_file_service_capability.cpp
src/default_capabilities/move_action_capability.cpp
src/default_capabilities/plan_service_capability.cpp
src/default_capabilities/query_planners_service_capability.cpp
+ src/default_capabilities/save_geometry_to_file_service_capability.cpp
src/default_capabilities/state_validation_service_capability.cpp
src/default_capabilities/tf_publisher_capability.cpp)
target_include_directories(
diff --git a/moveit_ros/move_group/default_capabilities_plugin_description.xml b/moveit_ros/move_group/default_capabilities_plugin_description.xml
index 35e379e1f2..785dfc09b4 100644
--- a/moveit_ros/move_group/default_capabilities_plugin_description.xml
+++ b/moveit_ros/move_group/default_capabilities_plugin_description.xml
@@ -78,4 +78,16 @@
+
+
+ Provide a capability which saves the CollisionObjects in the PlanningScene to a .scene file
+
+
+
+
+
+ Provide a capability which loads CollisionObjects into the PlanningScene from a .scene file
+
+
+
diff --git a/moveit_ros/move_group/include/moveit/move_group/capability_names.h b/moveit_ros/move_group/include/moveit/move_group/capability_names.h
index f5bc6cadb2..53af52c223 100644
--- a/moveit_ros/move_group/include/moveit/move_group/capability_names.h
+++ b/moveit_ros/move_group/include/moveit/move_group/capability_names.h
@@ -64,4 +64,8 @@ static const std::string CLEAR_OCTOMAP_SERVICE_NAME =
"clear_octomap"; // name of the service that can be used to clear the octomap
static const std::string GET_URDF_SERVICE_NAME =
"get_urdf"; // name of the service that can be used to request the urdf of a planning group
+static const std::string SAVE_GEOMETRY_TO_FILE_SERVICE_NAME =
+ "save_geometry_to_file"; // name of the service that can be used to save CollisionObjects in a PlanningScene to a file
+static const std::string LOAD_GEOMETRY_FROM_FILE_SERVICE_NAME =
+ "load_geometry_from_file"; // name of the service that can be used to load CollisionObjects to a PlanningScene from a file
} // namespace move_group
diff --git a/moveit_ros/move_group/src/default_capabilities/load_geometry_from_file_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/load_geometry_from_file_service_capability.cpp
new file mode 100644
index 0000000000..93a7b51520
--- /dev/null
+++ b/moveit_ros/move_group/src/default_capabilities/load_geometry_from_file_service_capability.cpp
@@ -0,0 +1,85 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2024, PickNik Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of PickNik Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+/* Author: Bilal Gill */
+
+#include "load_geometry_from_file_service_capability.h"
+
+#include
+#include
+#include
+
+#include
+
+namespace move_group
+{
+
+namespace
+{
+rclcpp::Logger getLogger()
+{
+ return moveit::getLogger("load_geometry_from_file_service");
+}
+} // namespace
+
+LoadGeometryFromFileService::LoadGeometryFromFileService() : MoveGroupCapability(LOAD_GEOMETRY_FROM_FILE_SERVICE_NAME)
+{
+}
+
+void LoadGeometryFromFileService::initialize()
+{
+ load_geometry_service_ = context_->moveit_cpp_->getNode()->create_service(
+ LOAD_GEOMETRY_FROM_FILE_SERVICE_NAME,
+ [this](const std::shared_ptr& req,
+ const std::shared_ptr& res) {
+ std::ifstream file(req->file_path_and_name);
+ if (!file.is_open())
+ {
+ RCLCPP_ERROR(getLogger(), "Unable to open file %s for loading CollisionObjects",
+ req->file_path_and_name.c_str());
+ res->success = false;
+ return;
+ }
+ planning_scene_monitor::LockedPlanningSceneRW locked_ps(context_->planning_scene_monitor_);
+ locked_ps->loadGeometryFromStream(file);
+ file.close();
+ res->success = true;
+ } // End of callback function
+ );
+}
+} // namespace move_group
+
+#include
+
+PLUGINLIB_EXPORT_CLASS(move_group::LoadGeometryFromFileService, move_group::MoveGroupCapability)
diff --git a/moveit_ros/move_group/src/default_capabilities/load_geometry_from_file_service_capability.h b/moveit_ros/move_group/src/default_capabilities/load_geometry_from_file_service_capability.h
new file mode 100644
index 0000000000..095aba5465
--- /dev/null
+++ b/moveit_ros/move_group/src/default_capabilities/load_geometry_from_file_service_capability.h
@@ -0,0 +1,62 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2024, PickNik Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of PickNik Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+/* Author: Bilal Gill */
+
+#pragma once
+
+#include
+#include
+
+namespace move_group
+{
+/**
+ * @brief Move group capability to save CollisionObjects in a PlanningScene to a .scene file
+ *
+ */
+class LoadGeometryFromFileService : public MoveGroupCapability
+{
+public:
+ LoadGeometryFromFileService();
+
+ /**
+ * @brief Initializes service when plugin is loaded
+ *
+ */
+ void initialize() override;
+
+private:
+ rclcpp::Service::SharedPtr load_geometry_service_;
+};
+} // namespace move_group
diff --git a/moveit_ros/move_group/src/default_capabilities/save_geometry_to_file_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/save_geometry_to_file_service_capability.cpp
new file mode 100644
index 0000000000..0053459bce
--- /dev/null
+++ b/moveit_ros/move_group/src/default_capabilities/save_geometry_to_file_service_capability.cpp
@@ -0,0 +1,85 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2024, PickNik Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of PickNik Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+/* Author: Bilal Gill */
+
+#include "save_geometry_to_file_service_capability.h"
+
+#include
+#include
+#include
+
+#include
+
+namespace move_group
+{
+
+namespace
+{
+rclcpp::Logger getLogger()
+{
+ return moveit::getLogger("save_geometry_to_file_service");
+}
+} // namespace
+
+SaveGeometryToFileService::SaveGeometryToFileService() : MoveGroupCapability(SAVE_GEOMETRY_TO_FILE_SERVICE_NAME)
+{
+}
+
+void SaveGeometryToFileService::initialize()
+{
+ save_geometry_service_ = context_->moveit_cpp_->getNode()->create_service(
+ SAVE_GEOMETRY_TO_FILE_SERVICE_NAME,
+ [this](const std::shared_ptr& req,
+ const std::shared_ptr& res) {
+ std::ofstream file(req->file_path_and_name);
+ if (!file.is_open())
+ {
+ RCLCPP_ERROR(getLogger(), "Unable to open file %s for saving CollisionObjects",
+ req->file_path_and_name.c_str());
+ res->success = false;
+ return;
+ }
+ planning_scene_monitor::LockedPlanningSceneRO locked_ps(context_->planning_scene_monitor_);
+ locked_ps->saveGeometryToStream(file);
+ file.close();
+ res->success = true;
+ } // End of callback function
+ );
+}
+} // namespace move_group
+
+#include
+
+PLUGINLIB_EXPORT_CLASS(move_group::SaveGeometryToFileService, move_group::MoveGroupCapability)
diff --git a/moveit_ros/move_group/src/default_capabilities/save_geometry_to_file_service_capability.h b/moveit_ros/move_group/src/default_capabilities/save_geometry_to_file_service_capability.h
new file mode 100644
index 0000000000..30fa21c75b
--- /dev/null
+++ b/moveit_ros/move_group/src/default_capabilities/save_geometry_to_file_service_capability.h
@@ -0,0 +1,62 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2024, PickNik Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of PickNik Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+/* Author: Bilal Gill */
+
+#pragma once
+
+#include
+#include
+
+namespace move_group
+{
+/**
+ * @brief Move group capability to save CollisionObjects in a PlanningScene to a .scene file
+ *
+ */
+class SaveGeometryToFileService : public MoveGroupCapability
+{
+public:
+ SaveGeometryToFileService();
+
+ /**
+ * @brief Initializes service when plugin is loaded
+ *
+ */
+ void initialize() override;
+
+private:
+ rclcpp::Service::SharedPtr save_geometry_service_;
+};
+} // namespace move_group
diff --git a/moveit_ros/move_group/src/move_group.cpp b/moveit_ros/move_group/src/move_group.cpp
index e67ad74963..bb9629d191 100644
--- a/moveit_ros/move_group/src/move_group.cpp
+++ b/moveit_ros/move_group/src/move_group.cpp
@@ -62,6 +62,8 @@ rclcpp::Logger getLogger()
// These capabilities are loaded unless listed in disable_capabilities
// clang-format off
static const char* const DEFAULT_CAPABILITIES[] = {
+ "move_group/LoadGeometryFromFileService",
+ "move_group/SaveGeometryToFileService",
"move_group/GetUrdfService",
"move_group/MoveGroupCartesianPathService",
"move_group/MoveGroupKinematicsService",