Suggestion for new dynamic detachable joint plugin

ros_gz_bridge/CMakeLists.txt

@@ -211,6 +211,7 @@ if(BUILD_TESTING)
   target_include_directories(test_ros_gz_interfaces
     PRIVATE
     ${PROJECT_SOURCE_DIR}/include
+    ${PROJECT_SOURCE_DIR}/src
   )
 
   add_library(test_utils

ros_gz_bridge/src/convert/ros_gz_interfaces_TEST.cpp

@@ -16,6 +16,18 @@
 
 #include <ros_gz_bridge/convert/ros_gz_interfaces.hpp>
 
+#include "service_factories/ros_gz_interfaces.hpp"
+
+TEST(AttachDetachServiceTest, FactoryRegistration)
+{
+  auto factory = ros_gz_bridge::get_service_factory__ros_gz_interfaces(
+    "ros_gz_interfaces/srv/AttachDetach",
+    "gz.msgs.AttachDetachRequest",
+    "gz.msgs.Result");
+
+  ASSERT_NE(nullptr, factory);
+}
+
 // A more specific set of tests for the ros_gz_interfaces/msg/ParamVec to
 // to verify behaviors that couldn't easily be captured by the generic test framework
 struct RosToGzTest : public ::testing::Test

ros_gz_bridge/src/service_factories/ros_gz_interfaces.cpp

@@ -16,7 +16,9 @@
 #include <gz/msgs/world_control.pb.h>
 #include <gz/msgs/entity.pb.h>
 #include <gz/msgs/entity_factory.pb.h>
+#include <gz/msgs/dynamic_detachable_joint.pb.h>
 #include <gz/msgs/pose.pb.h>
+#include <gz/msgs/result.pb.h>
 
 #include <memory>
 #include <string>
@@ -26,6 +28,7 @@
 #include "ros_gz_interfaces/srv/delete_entity.hpp"
 #include "ros_gz_interfaces/srv/spawn_entity.hpp"
 #include "ros_gz_interfaces/srv/set_entity_pose.hpp"
+#include "ros_gz_interfaces/srv/attach_detach.hpp"
 #include "ros_gz_bridge/convert/ros_gz_interfaces.hpp"
 
 #include "service_factory.hpp"
@@ -91,6 +94,19 @@ get_service_factory__ros_gz_interfaces(
     >(ros_type_name, "gz.msgs.Pose", "gz.msgs.Boolean");
   }
 
+  if (
+    ros_type_name == "ros_gz_interfaces/srv/AttachDetach" &&
+    (gz_req_type_name.empty() || gz_req_type_name == "gz.msgs.AttachDetachRequest") &&
+    (gz_rep_type_name.empty() || gz_rep_type_name == "gz.msgs.Result"))
+  {
+    return std::make_shared<
+      ServiceFactory<
+        ros_gz_interfaces::srv::AttachDetach,
+        gz::msgs::AttachDetachRequest,
+        gz::msgs::Result>
+    >(ros_type_name, "gz.msgs.AttachDetachRequest", "gz.msgs.Result");
+  }
+
   return nullptr;
 }
 
@@ -131,6 +147,29 @@ convert_ros_to_gz(
   convert_ros_to_gz(ros_req.pose, gz_req);
 }
 
+template<>
+void
+convert_ros_to_gz(
+  const ros_gz_interfaces::srv::AttachDetach::Request & ros_req,
+  gz::msgs::AttachDetachRequest & gz_req)
+{
+  gz_req.set_child_model_name(ros_req.child_model_name);
+  gz_req.set_child_link_name(ros_req.child_link_name);
+  switch (ros_req.command)
+  {
+    case ros_gz_interfaces::srv::AttachDetach::Request::ATTACH:
+      gz_req.set_command(gz::msgs::AttachDetachRequest::ATTACH);
+      break;
+    case ros_gz_interfaces::srv::AttachDetach::Request::DETACH:
+      gz_req.set_command(gz::msgs::AttachDetachRequest::DETACH);
+      break;
+    case ros_gz_interfaces::srv::AttachDetach::Request::COMMAND_UNSPECIFIED:
+    default:
+      gz_req.set_command(gz::msgs::AttachDetachRequest::COMMAND_UNSPECIFIED);
+      break;
+  }
+}
+
 template<>
 void
 convert_gz_to_ros(
@@ -167,6 +206,16 @@ convert_gz_to_ros(
   ros_res.success = gz_rep.data();
 }
 
+template<>
+void
+convert_gz_to_ros(
+  const gz::msgs::Result & gz_rep,
+  ros_gz_interfaces::srv::AttachDetach::Response & ros_res)
+{
+  ros_res.error_code = gz_rep.error_code();
+  ros_res.message = gz_rep.message();
+}
+
 template<>
 bool
 send_response_on_error(ros_gz_interfaces::srv::ControlWorld::Response & ros_res)
@@ -206,4 +255,13 @@ send_response_on_error(ros_gz_interfaces::srv::SetEntityPose::Response & ros_res
   ros_res.success = false;
   return true;
 }
+
+template<>
+bool
+send_response_on_error(ros_gz_interfaces::srv::AttachDetach::Response & ros_res)
+{
+  ros_res.error_code = 1;
+  ros_res.message = "Gazebo bridge error";
+  return true;
+}
 }  // namespace ros_gz_bridge

ros_gz_interfaces/CMakeLists.txt

@@ -47,6 +47,7 @@ set(srv_files
   "srv/DeleteEntity.srv"
   "srv/SetEntityPose.srv"
   "srv/SpawnEntity.srv"
+  "srv/AttachDetach.srv"
 )
 
 rosidl_generate_interfaces(${PROJECT_NAME}

ros_gz_interfaces/srv/AttachDetach.srv

@@ -0,0 +1,9 @@
+string child_model_name
+string child_link_name
+uint8 COMMAND_UNSPECIFIED=0
+uint8 ATTACH=1
+uint8 DETACH=2
+uint8 command
+---
+int32 error_code
+string message