Add SaveGeometryToFile and LoadGeometryFromFile services (#2973)

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(

moveit_ros/move_group/default_capabilities_plugin_description.xml

@@ -78,4 +78,16 @@
     </description>
   </class>
 
+  <class name="move_group/SaveGeometryToFileService" type="move_group::SaveGeometryToFileService" base_class_type="move_group::MoveGroupCapability">
+    <description>
+      Provide a capability which saves the CollisionObjects in the PlanningScene to a .scene file
+    </description>
+  </class>
+
+  <class name="move_group/LoadGeometryFromFileService" type="move_group::LoadGeometryFromFileService" base_class_type="move_group::MoveGroupCapability">
+    <description>
+      Provide a capability which loads CollisionObjects into the PlanningScene from a .scene file
+    </description>
+  </class>
+
 </library>

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

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 <moveit/moveit_cpp/moveit_cpp.h>
+#include <moveit/move_group/capability_names.h>
+#include <moveit/utils/logger.hpp>
+
+#include <fstream>
+
+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<moveit_msgs::srv::LoadGeometryFromFile>(
+      LOAD_GEOMETRY_FROM_FILE_SERVICE_NAME,
+      [this](const std::shared_ptr<moveit_msgs::srv::LoadGeometryFromFile::Request>& req,
+             const std::shared_ptr<moveit_msgs::srv::LoadGeometryFromFile::Response>& 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/class_list_macros.hpp>
+
+PLUGINLIB_EXPORT_CLASS(move_group::LoadGeometryFromFileService, move_group::MoveGroupCapability)

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 <moveit/move_group/move_group_capability.h>
+#include <moveit_msgs/srv/load_geometry_from_file.hpp>
+
+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<moveit_msgs::srv::LoadGeometryFromFile>::SharedPtr load_geometry_service_;
+};
+}  // namespace move_group

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 <moveit/moveit_cpp/moveit_cpp.h>
+#include <moveit/move_group/capability_names.h>
+#include <moveit/utils/logger.hpp>
+
+#include <fstream>
+
+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<moveit_msgs::srv::SaveGeometryToFile>(
+      SAVE_GEOMETRY_TO_FILE_SERVICE_NAME,
+      [this](const std::shared_ptr<moveit_msgs::srv::SaveGeometryToFile::Request>& req,
+             const std::shared_ptr<moveit_msgs::srv::SaveGeometryToFile::Response>& 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/class_list_macros.hpp>
+
+PLUGINLIB_EXPORT_CLASS(move_group::SaveGeometryToFileService, move_group::MoveGroupCapability)

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 <moveit/move_group/move_group_capability.h>
+#include <moveit_msgs/srv/save_geometry_to_file.hpp>
+
+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<moveit_msgs::srv::SaveGeometryToFile>::SharedPtr save_geometry_service_;
+};
+}  // namespace move_group

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",