From 8af7e43d4871da0503d7b5ae8fec73273108d1e0 Mon Sep 17 00:00:00 2001 From: retinfai Date: Thu, 23 Feb 2023 17:53:25 +1300 Subject: [PATCH 1/4] feat: add spawn, delete, set pose service and entity_factory to bridge --- .../convert/ros_gz_interfaces.hpp | 21 +++ ros_gz_bridge/ros_gz_bridge/mappings.py | 1 + .../src/convert/ros_gz_interfaces.cpp | 58 ++++++++ .../service_factories/ros_gz_interfaces.cpp | 131 ++++++++++++++++++ .../ros_gz_interfaces_subscriber.cpp | 11 ++ ros_gz_bridge/test/utils/gz_test_msg.cpp | 12 ++ ros_gz_bridge/test/utils/gz_test_msg.hpp | 9 ++ ros_gz_bridge/test/utils/ros_test_msg.cpp | 13 ++ ros_gz_bridge/test/utils/ros_test_msg.hpp | 9 ++ 9 files changed, 265 insertions(+) diff --git a/ros_gz_bridge/include/ros_gz_bridge/convert/ros_gz_interfaces.hpp b/ros_gz_bridge/include/ros_gz_bridge/convert/ros_gz_interfaces.hpp index 193bdee9..4ce39f22 100644 --- a/ros_gz_bridge/include/ros_gz_bridge/convert/ros_gz_interfaces.hpp +++ b/ros_gz_bridge/include/ros_gz_bridge/convert/ros_gz_interfaces.hpp @@ -17,6 +17,7 @@ // Gazebo Msgs #include +#include #include #include #include @@ -33,6 +34,7 @@ // ROS 2 messages #include +#include #include #include #include @@ -71,12 +73,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( diff --git a/ros_gz_bridge/ros_gz_bridge/mappings.py b/ros_gz_bridge/ros_gz_bridge/mappings.py index 3597b09b..3e909a23 100644 --- a/ros_gz_bridge/ros_gz_bridge/mappings.py +++ b/ros_gz_bridge/ros_gz_bridge/mappings.py @@ -56,6 +56,7 @@ Mapping('Contacts', 'Contacts'), Mapping('Dataframe', 'Dataframe'), Mapping('Entity', 'Entity'), + Mapping('EntityFactory', 'EntityFactory'), Mapping('Float32Array', 'Float_V'), Mapping('GuiCamera', 'GUICamera'), Mapping('JointWrench', 'JointWrench'), diff --git a/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp b/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp index 51e21ab6..f0b848fc 100644 --- a/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp +++ b/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp @@ -85,6 +85,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( @@ -115,6 +126,53 @@ 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); + + + if(ros_msg.sdf.length()>0) { + gz_msg.set_sdf(ros_msg.sdf); + } else if(ros_msg.sdf_filename.length()>0) { + gz_msg.set_sdf_filename(ros_msg.sdf_filename); + } else if(ros_msg.clone_name.length()>0) { + 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()); + + gz_msg.set_relative_to(ros_msg.relative_to); + +} + +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( diff --git a/ros_gz_bridge/src/service_factories/ros_gz_interfaces.cpp b/ros_gz_bridge/src/service_factories/ros_gz_interfaces.cpp index a3c25a82..925bdd5a 100644 --- a/ros_gz_bridge/src/service_factories/ros_gz_interfaces.cpp +++ b/ros_gz_bridge/src/service_factories/ros_gz_interfaces.cpp @@ -14,12 +14,18 @@ #include #include +#include +#include +#include #include #include #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 diff --git a/ros_gz_bridge/test/subscribers/ros_subscriber/ros_gz_interfaces_subscriber.cpp b/ros_gz_bridge/test/subscribers/ros_subscriber/ros_gz_interfaces_subscriber.cpp index a4e885e9..eadeeae7 100644 --- a/ros_gz_bridge/test/subscribers/ros_subscriber/ros_gz_interfaces_subscriber.cpp +++ b/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 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) { diff --git a/ros_gz_bridge/test/utils/gz_test_msg.cpp b/ros_gz_bridge/test/utils/gz_test_msg.cpp index 981e9684..da869a43 100644 --- a/ros_gz_bridge/test/utils/gz_test_msg.cpp +++ b/ros_gz_bridge/test/utils/gz_test_msg.cpp @@ -466,6 +466,18 @@ void compareTestMsg(const std::shared_ptr & _msg) EXPECT_EQ(expected_msg.type(), _msg->type()); } +void createTestMsg(gz::msgs::EntityFactory & _msg) +{ + _msg.set_name("entity"); +} + +void compareTestMsg(const std::shared_ptr & _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; diff --git a/ros_gz_bridge/test/utils/gz_test_msg.hpp b/ros_gz_bridge/test/utils/gz_test_msg.hpp index 9d08c9fe..0cfaece0 100644 --- a/ros_gz_bridge/test/utils/gz_test_msg.hpp +++ b/ros_gz_bridge/test/utils/gz_test_msg.hpp @@ -28,6 +28,7 @@ #include #include #include +#include #include #include #include @@ -271,6 +272,14 @@ void createTestMsg(gz::msgs::Entity & _msg); /// \param[in] _msg The message to compare. void compareTestMsg(const std::shared_ptr & _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 & _msg); + /// \brief Create a message used for testing. /// \param[out] _msg The message populated. void createTestMsg(gz::msgs::Contact & _msg); diff --git a/ros_gz_bridge/test/utils/ros_test_msg.cpp b/ros_gz_bridge/test/utils/ros_test_msg.cpp index 7a534749..dad247a2 100644 --- a/ros_gz_bridge/test/utils/ros_test_msg.cpp +++ b/ros_gz_bridge/test/utils/ros_test_msg.cpp @@ -704,6 +704,19 @@ void compareTestMsg(const std::shared_ptr & _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 & _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); diff --git a/ros_gz_bridge/test/utils/ros_test_msg.hpp b/ros_gz_bridge/test/utils/ros_test_msg.hpp index ec543bb3..95c4e373 100644 --- a/ros_gz_bridge/test/utils/ros_test_msg.hpp +++ b/ros_gz_bridge/test/utils/ros_test_msg.hpp @@ -44,6 +44,7 @@ #include #include #include +#include #include #include #include @@ -353,6 +354,14 @@ void createTestMsg(ros_gz_interfaces::msg::Entity & _msg); /// \param[in] _msg The message to compare. void compareTestMsg(const std::shared_ptr & _msg); +/// \brief Create a message used for testing. +/// \param[out] _msg The message populated. +void createTestMsg(ros_gz_interfaces::msg::EntityFactory & _msg); + +/// \brief Compare a message with the populated for testing. +/// \param[in] _msg The message to compare. +void compareTestMsg(const std::shared_ptr & _msg); + /// \brief Create a message used for testing. /// \param[out] _msg The message populated. void createTestMsg(ros_gz_interfaces::msg::Contact & _msg); From 76deda9cd102e3941deb1b2f13f65d67dd23ba85 Mon Sep 17 00:00:00 2001 From: Afereti Pama Date: Fri, 7 Apr 2023 21:04:25 +0000 Subject: [PATCH 2/4] Add warning to EntityFactory conversion --- ros_gz_bridge/src/convert/ros_gz_interfaces.cpp | 17 ++++++++++++----- 1 file changed, 12 insertions(+), 5 deletions(-) diff --git a/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp b/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp index f0b848fc..2839eced 100644 --- a/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp +++ b/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp @@ -134,13 +134,22 @@ convert_ros_to_gz( 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(ros_msg.sdf.length()>0) { + 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(ros_msg.sdf_filename.length()>0) { + } else if(has_sdf_filename) { gz_msg.set_sdf_filename(ros_msg.sdf_filename); - } else if(ros_msg.clone_name.length()>0) { + } 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; @@ -149,8 +158,6 @@ convert_ros_to_gz( convert_ros_to_gz(ros_msg.pose, *gz_msg.mutable_pose()); - gz_msg.set_relative_to(ros_msg.relative_to); - } template<> From 4828b0136fd90579f370aca282d6d7601414b088 Mon Sep 17 00:00:00 2001 From: Afereti Pama <79831813+retinfai@users.noreply.github.com> Date: Fri, 5 Jan 2024 18:50:48 +1300 Subject: [PATCH 3/4] Update ros_gz_bridge/src/convert/ros_gz_interfaces.cpp MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Alejandro Hernández Cordero Signed-off-by: Afereti Pama <79831813+retinfai@users.noreply.github.com> --- ros_gz_bridge/src/convert/ros_gz_interfaces.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp b/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp index 2839eced..e233f052 100644 --- a/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp +++ b/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp @@ -178,8 +178,6 @@ convert_gz_to_ros( ros_msg.relative_to = gz_msg.relative_to(); } - - template<> void convert_ros_to_gz( From 0035048d223687c084a7e70671421afc2c4e9b75 Mon Sep 17 00:00:00 2001 From: Afereti Pama <79831813+retinfai@users.noreply.github.com> Date: Fri, 5 Jan 2024 18:50:58 +1300 Subject: [PATCH 4/4] Update ros_gz_bridge/src/convert/ros_gz_interfaces.cpp MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Alejandro Hernández Cordero Signed-off-by: Afereti Pama <79831813+retinfai@users.noreply.github.com> --- ros_gz_bridge/src/convert/ros_gz_interfaces.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp b/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp index e233f052..4f4c4e76 100644 --- a/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp +++ b/ros_gz_bridge/src/convert/ros_gz_interfaces.cpp @@ -155,9 +155,7 @@ convert_ros_to_gz( 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<>