Add SpawnEntity, DeleteEntity, & SetEntityPose Support

ros_gz_bridge/include/ros_gz_bridge/convert/ros_gz_interfaces.hpp

@@ -18,6 +18,7 @@
 // Gazebo Msgs
 #include <gz/msgs/altimeter.pb.h>
 #include <gz/msgs/entity.pb.h>
+#include <gz/msgs/entity_factory.pb.h>
 #include <gz/msgs/joint_wrench.pb.h>
 #include <gz/msgs/contact.pb.h>
 #include <gz/msgs/contacts.pb.h>
@@ -37,6 +38,7 @@
 // ROS 2 messages
 #include <ros_gz_interfaces/msg/altimeter.hpp>
 #include <ros_gz_interfaces/msg/entity.hpp>
+#include <ros_gz_interfaces/msg/entity_factory.hpp>
 #include <ros_gz_interfaces/msg/joint_wrench.hpp>
 #include <ros_gz_interfaces/msg/contact.hpp>
 #include <ros_gz_interfaces/msg/contacts.hpp>
@@ -89,12 +91,31 @@ convert_ros_to_gz(
   const ros_gz_interfaces::msg::Entity & ros_msg,
   gz::msgs::Entity & gz_msg);
 
+template<>
+void
+convert_ros_to_gz(
+  const ros_gz_interfaces::msg::Entity & ros_msg,
+  gz::msgs::Pose & gz_msg);
+
 template<>
 void
 convert_gz_to_ros(
   const gz::msgs::Entity & gz_msg,
   ros_gz_interfaces::msg::Entity & ros_msg);
 
+template<>
+void
+convert_ros_to_gz(
+  const ros_gz_interfaces::msg::EntityFactory & ros_msg,
+  gz::msgs::EntityFactory & gz_msg);
+
+
+template<>
+void
+convert_gz_to_ros(
+  const gz::msgs::EntityFactory & gz_msg,
+  ros_gz_interfaces::msg::EntityFactory & ros_msg);
+
 template<>
 void
 convert_ros_to_gz(

ros_gz_bridge/ros_gz_bridge/mappings.py

@@ -64,6 +64,7 @@
         Mapping('Contacts', 'Contacts'),
         Mapping('Dataframe', 'Dataframe'),
         Mapping('Entity', 'Entity'),
+        Mapping('EntityFactory', 'EntityFactory'),
         Mapping('Float32Array', 'Float_V'),
         Mapping('GuiCamera', 'GUICamera'),
         Mapping('JointWrench', 'JointWrench'),

ros_gz_bridge/src/convert/ros_gz_interfaces.cpp

@@ -109,6 +109,17 @@ convert_ros_to_gz(
   }
 }
 
+template<>
+void
+convert_ros_to_gz(
+  const ros_gz_interfaces::msg::Entity & ros_msg,
+  gz::msgs::Pose & gz_msg)
+{
+  gz_msg.set_id(ros_msg.id);
+  gz_msg.set_name(ros_msg.name);
+}
+
+
 template<>
 void
 convert_gz_to_ros(
@@ -139,6 +150,56 @@ convert_gz_to_ros(
   }
 }
 
+template<>
+void
+convert_ros_to_gz(
+  const ros_gz_interfaces::msg::EntityFactory & ros_msg,
+  gz::msgs::EntityFactory & gz_msg) {
+
+  gz_msg.set_name(ros_msg.name);
+  gz_msg.set_allow_renaming(ros_msg.allow_renaming);
+  gz_msg.set_relative_to(ros_msg.relative_to);
+
+  bool has_sdf = !ros_msg.sdf.empty();
+  bool has_sdf_filename = !ros_msg.sdf_filename.empty();
+  bool has_clone_name = !ros_msg.clone_name.empty();
+
+  bool provided_two = has_sdf ? (has_sdf_filename || has_clone_name) : (has_sdf_filename && has_clone_name);
+
+  if (provided_two)
+    std::cout << "Warning: You should only provide ONE of sdf, sdf_filname, or clone_name" << std::endl;
+
+  if(has_sdf) {
+    gz_msg.set_sdf(ros_msg.sdf);
+  } else if(has_sdf_filename) {
+    gz_msg.set_sdf_filename(ros_msg.sdf_filename);
+  } else if(has_clone_name) {
+    gz_msg.set_clone_name(ros_msg.clone_name);
+  } else {
+    std::cerr << "Must provide one of: sdf, sdf_filname, or clone_name" << std::endl;
+  }
+
+  convert_ros_to_gz(ros_msg.pose, *gz_msg.mutable_pose());
+}
+
+template<>
+void
+convert_gz_to_ros(
+  const gz::msgs::EntityFactory & gz_msg,
+  ros_gz_interfaces::msg::EntityFactory & ros_msg)
+{
+  ros_msg.name = gz_msg.name();
+  ros_msg.allow_renaming = gz_msg.allow_renaming();
+
+  ros_msg.sdf = gz_msg.sdf();
+  ros_msg.sdf_filename = gz_msg.sdf_filename();
+  ros_msg.clone_name = gz_msg.clone_name();
+
+  convert_gz_to_ros(gz_msg.pose(), ros_msg.pose);
+
+  ros_msg.relative_to = gz_msg.relative_to();
+}
+
 template<>
 void
 convert_ros_to_gz(

ros_gz_bridge/src/service_factories/ros_gz_interfaces.cpp

@@ -14,12 +14,18 @@
 
 #include <gz/msgs/boolean.pb.h>
 #include <gz/msgs/world_control.pb.h>
+#include <gz/msgs/entity.pb.h>
+#include <gz/msgs/entity_factory.pb.h>
+#include <gz/msgs/pose.pb.h>
 
 #include <memory>
 #include <string>
 
 #include "service_factories/ros_gz_interfaces.hpp"
 #include "ros_gz_interfaces/srv/control_world.hpp"
+#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_bridge/convert/ros_gz_interfaces.hpp"
 
 #include "service_factory.hpp"
@@ -46,6 +52,45 @@ get_service_factory__ros_gz_interfaces(
     >(ros_type_name, "gz.msgs.WorldControl", "gz.msgs.Boolean");
   }
 
+  if (
+    ros_type_name == "ros_gz_interfaces/srv/DeleteEntity" &&
+    (gz_req_type_name.empty() || gz_req_type_name == "gz.msgs.Entity") &&
+    (gz_rep_type_name.empty() || gz_rep_type_name == "gz.msgs.Boolean"))
+  {
+    return std::make_shared<
+      ServiceFactory<
+        ros_gz_interfaces::srv::DeleteEntity,
+        gz::msgs::Entity,
+        gz::msgs::Boolean>
+    >(ros_type_name, "gz.msgs.Entity", "gz.msgs.Boolean");
+  }
+
+  if (
+    ros_type_name == "ros_gz_interfaces/srv/SpawnEntity" &&
+    (gz_req_type_name.empty() || gz_req_type_name == "gz.msgs.EntityFactory") &&
+    (gz_rep_type_name.empty() || gz_rep_type_name == "gz.msgs.Boolean"))
+  {
+    return std::make_shared<
+      ServiceFactory<
+        ros_gz_interfaces::srv::SpawnEntity,
+        gz::msgs::EntityFactory,
+        gz::msgs::Boolean>
+    >(ros_type_name, "gz.msgs.EntityFactory", "gz.msgs.Boolean");
+  }
+
+  if (
+    ros_type_name == "ros_gz_interfaces/srv/SetEntityPose" &&
+    (gz_req_type_name.empty() || gz_req_type_name == "gz.msgs.Pose") &&
+    (gz_rep_type_name.empty() || gz_rep_type_name == "gz.msgs.Boolean"))
+  {
+    return std::make_shared<
+      ServiceFactory<
+        ros_gz_interfaces::srv::SetEntityPose,
+        gz::msgs::Pose,
+        gz::msgs::Boolean>
+    >(ros_type_name, "gz.msgs.Pose", "gz.msgs.Boolean");
+  }
+
   return nullptr;
 }
 
@@ -58,6 +103,35 @@ convert_ros_to_gz(
   convert_ros_to_gz(ros_req.world_control, gz_req);
 }
 
+template<>
+void
+convert_ros_to_gz(
+  const ros_gz_interfaces::srv::DeleteEntity::Request & ros_req,
+  gz::msgs::Entity & gz_req)
+{
+  convert_ros_to_gz(ros_req.entity, gz_req);
+}
+
+template<>
+void
+convert_ros_to_gz(
+  const ros_gz_interfaces::srv::SpawnEntity::Request & ros_req,
+  gz::msgs::EntityFactory & gz_req)
+{
+  convert_ros_to_gz(ros_req.entity_factory, gz_req);
+}
+
+template<>
+void
+convert_ros_to_gz(
+  const ros_gz_interfaces::srv::SetEntityPose::Request & ros_req,
+  gz::msgs::Pose & gz_req)
+{
+
+  convert_ros_to_gz(ros_req.entity, gz_req);
+  convert_ros_to_gz(ros_req.pose, gz_req);
+}
+
 template<>
 void
 convert_gz_to_ros(
@@ -67,6 +141,33 @@ convert_gz_to_ros(
   ros_res.success = gz_rep.data();
 }
 
+template<>
+void
+convert_gz_to_ros(
+  const gz::msgs::Boolean & gz_rep,
+  ros_gz_interfaces::srv::DeleteEntity::Response & ros_res)
+{
+  ros_res.success = gz_rep.data();
+}
+
+template<>
+void
+convert_gz_to_ros(
+  const gz::msgs::Boolean & gz_rep,
+  ros_gz_interfaces::srv::SpawnEntity::Response & ros_res)
+{
+  ros_res.success = gz_rep.data();
+}
+
+template<>
+void
+convert_gz_to_ros(
+  const gz::msgs::Boolean & gz_rep,
+  ros_gz_interfaces::srv::SetEntityPose::Response & ros_res)
+{
+  ros_res.success = gz_rep.data();
+}
+
 template<>
 bool
 send_response_on_error(ros_gz_interfaces::srv::ControlWorld::Response & ros_res)
@@ -76,4 +177,34 @@ send_response_on_error(ros_gz_interfaces::srv::ControlWorld::Response & ros_res)
   ros_res.success = false;
   return true;
 }
+
+template<>
+bool
+send_response_on_error(ros_gz_interfaces::srv::DeleteEntity::Response & ros_res)
+{
+  // TODO(now): Is it worth it to have a different field to encode Gazebo request errors?
+  //  Currently we're reusing the success field, which seems fine for this case.
+  ros_res.success = false;
+  return true;
+}
+
+template<>
+bool
+send_response_on_error(ros_gz_interfaces::srv::SpawnEntity::Response & ros_res)
+{
+  // TODO(now): Is it worth it to have a different field to encode Gazebo request errors?
+  //  Currently we're reusing the success field, which seems fine for this case.
+  ros_res.success = false;
+  return true;
+}
+
+template<>
+bool
+send_response_on_error(ros_gz_interfaces::srv::SetEntityPose::Response & ros_res)
+{
+  // TODO(now): Is it worth it to have a different field to encode Gazebo request errors?
+  //  Currently we're reusing the success field, which seems fine for this case.
+  ros_res.success = false;
+  return true;
+}
 }  // namespace ros_gz_bridge

ros_gz_bridge/test/subscribers/ros_subscriber/ros_gz_interfaces_subscriber.cpp

@@ -57,6 +57,17 @@ TEST(ROSSubscriberTest, Entity)
   EXPECT_TRUE(client.callbackExecuted);
 }
 
+TEST(ROSSubscriberTest, EntityFactory)
+{
+  MyTestClass<ros_gz_interfaces::msg::EntityFactory> client("entity_factory");
+
+  using namespace std::chrono_literals;
+  ros_gz_bridge::testing::waitUntilBoolVarAndSpin(
+    ros_subscriber::TestNode(), client.callbackExecuted, 10ms, 200);
+
+  EXPECT_TRUE(client.callbackExecuted);
+}
+
 /////////////////////////////////////////////////
 TEST(ROSSubscriberTest, Contact)
 {

ros_gz_bridge/test/utils/gz_test_msg.cpp

@@ -524,6 +524,18 @@ void compareTestMsg(const std::shared_ptr<gz::msgs::Entity> & _msg)
   EXPECT_EQ(expected_msg.type(), _msg->type());
 }
 
+void createTestMsg(gz::msgs::EntityFactory & _msg)
+{
+  _msg.set_name("entity");
+}
+
+void compareTestMsg(const std::shared_ptr<gz::msgs::EntityFactory> & _msg)
+{
+  gz::msgs::EntityFactory expected_msg;
+  createTestMsg(expected_msg);
+  EXPECT_EQ(expected_msg.name(), _msg->name());
+}
+
 void createTestMsg(gz::msgs::Contact & _msg)
 {
   gz::msgs::Entity collision1;

ros_gz_bridge/test/utils/gz_test_msg.hpp

@@ -29,6 +29,7 @@
 #include <gz/msgs/double.pb.h>
 #include <gz/msgs/empty.pb.h>
 #include <gz/msgs/entity.pb.h>
+#include <gz/msgs/entity_factory.pb.h>
 #include <gz/msgs/dataframe.pb.h>
 #include <gz/msgs/float.pb.h>
 #include <gz/msgs/fluid_pressure.pb.h>
@@ -292,6 +293,14 @@ void createTestMsg(gz::msgs::Entity & _msg);
 /// \param[in] _msg The message to compare.
 void compareTestMsg(const std::shared_ptr<gz::msgs::Entity> & _msg);
 
+/// \brief Create a message used for testing.
+/// \param[out] _msg The message populated.
+void createTestMsg(gz::msgs::EntityFactory & _msg);
+
+/// \brief Compare a message with the populated for testing.
+/// \param[in] _msg The message to compare.
+void compareTestMsg(const std::shared_ptr<gz::msgs::EntityFactory> & _msg);
+
 /// \brief Create a message used for testing.
 /// \param[out] _msg The message populated.
 void createTestMsg(gz::msgs::Contact & _msg);

ros_gz_bridge/test/utils/ros_test_msg.cpp

@@ -867,6 +867,19 @@ void compareTestMsg(const std::shared_ptr<ros_gz_interfaces::msg::Entity> & _msg
   EXPECT_EQ(expected_msg.type, _msg->type);
 }
 
+void createTestMsg(ros_gz_interfaces::msg::EntityFactory & _msg)
+{
+  _msg.name = "entity";
+}
+
+void compareTestMsg(const std::shared_ptr<ros_gz_interfaces::msg::EntityFactory> & _msg)
+{
+  ros_gz_interfaces::msg::EntityFactory expected_msg;
+  createTestMsg(expected_msg);
+
+  EXPECT_EQ(expected_msg.name, _msg->name);
+}
+
 void createTestMsg(ros_gz_interfaces::msg::Contact & _msg)
 {
   createTestMsg(_msg.collision1);