[ROS2][gazebo_ros] gazebo_ros_properties services not working

[Blebot0]

Hey,
I'm working with Gazebo 11 with ROS2 Humble.

I have been trying to migrate the set model configuration service for ROS2 but I noticed that none of the services in gazebo_ros_properties are showing up.

I made the following changes to the ros2 branch hoping that this would be enough to launch gazebo_ros_properties with the desired service.

diff --git a/gazebo_ros/launch/gzserver.launch.py b/gazebo_ros/launch/gzserver.launch.py
index 23f87cb..09ec3e1 100644
--- a/gazebo_ros/launch/gzserver.launch.py
+++ b/gazebo_ros/launch/gzserver.launch.py
@@ -55,6 +55,7 @@ def generate_launch_description():
         _plugin_command('init'),
         _plugin_command('factory'),
         _plugin_command('force_system'),
+        _plugin_command('properties'),
         # Wait for (https://github.com/ros-simulation/gazebo_ros_pkgs/pull/941)
         # _plugin_command('force_system'), ' ',
         _arg_command('profile'),
@@ -195,6 +196,10 @@ def generate_launch_description():
             'force_system', default_value='true',
             description='Set "false" not to load "libgazebo_ros_force_system.so"'
         ),
+        DeclareLaunchArgument(
+            'properties', default_value='true',
+            description='Set "false" to not load "libgazebo_ros_properties.so"'
+        ),
         DeclareLaunchArgument(
             'server_required', default_value='false',
             description='Set "true" to shut down launch script when server is terminated'

diff --git a/gazebo_ros/src/gazebo_ros_properties.cpp b/gazebo_ros/src/gazebo_ros_properties.cpp
index 127fd5e..83c0765 100644
--- a/gazebo_ros/src/gazebo_ros_properties.cpp
+++ b/gazebo_ros/src/gazebo_ros_properties.cpp
@@ -32,7 +32,7 @@
 #include <gazebo_msgs/srv/set_physics_properties.hpp>
 #include <gazebo_ros/conversions/geometry_msgs.hpp>
 #include <gazebo_ros/node.hpp>
-
+#include <gazebo_msgs/srv/set_model_configuration.hpp>
 #include <memory>
 
 #include "gazebo_ros/gazebo_ros_properties.hpp"
@@ -92,6 +92,11 @@ public:
     gazebo_msgs::srv::SetLightProperties::Request::SharedPtr _req,
     gazebo_msgs::srv::SetLightProperties::Response::SharedPtr _res);
 
+  void SetModelConfiguration(
+    gazebo_msgs::srv::SetModelConfiguration::Request::SharedPtr _req,
+    gazebo_msgs::srv::SetModelConfiguration::Response::SharedPtr _res
+  );
+  
   /// \brief World pointer from Gazebo.
   gazebo::physics::WorldPtr world_;
 
@@ -119,6 +124,9 @@ public:
   /// \brief ROS service to handle requests to set light properties.
   rclcpp::Service<gazebo_msgs::srv::SetLightProperties>::SharedPtr set_light_properties_service_;
 
+  /// \brief ROS service to handle requests for set model configuration.
+  rclcpp::Service<gazebo_msgs::srv::SetModelConfiguration>::SharedPtr set_model_configuration_service_;
+
   /// Gazebo node for communication.
   gazebo::transport::NodePtr gz_node_;
 
@@ -183,6 +191,12 @@ void GazeboRosProperties::Load(gazebo::physics::WorldPtr _world, sdf::ElementPtr
       &GazeboRosPropertiesPrivate::SetLightProperties, impl_.get(),
       std::placeholders::_1, std::placeholders::_2));
 
+  impl_->set_model_configuration_service_ = 
+    impl_->ros_node_->create_service<gazebo_msgs::srv::SetModelConfiguration>(
+      "set_model_configuration", std::bind(
+        &GazeboRosPropertiesPrivate::SetModelConfiguration, impl_.get(),
+        std::placeholders::_1, std::placeholders::_2));
+
   // Gazebo transport
   impl_->gz_node_ = gazebo::transport::NodePtr(new gazebo::transport::Node());
   impl_->gz_node_->Init(_world->Name());
@@ -464,6 +478,51 @@ void GazeboRosPropertiesPrivate::SetLightProperties(
   }
 }
 
+void GazeboRosPropertiesPrivate::SetModelConfiguration(
+    gazebo_msgs::srv::SetModelConfiguration::Request::SharedPtr _req,
+    gazebo_msgs::srv::SetModelConfiguration::Response::SharedPtr _res) 
+{
+  gazebo::physics::ModelPtr gazebo_model = world_->ModelByName(_req->model_name);
+
+  if(!gazebo_model)
+  {
+    RCLCPP_ERROR(
+      ros_node_->get_logger(), "SetModelConfiguration: model [%s] does not exist",
+      _req->model_name.c_str());
+    _res->success = false;
+    _res->status_message = "SetModelConfiguration: model does not exist";
+    return;
+  }    
+  if (_req->joint_names.size() == _req->joint_positions.size())
+  {
+    std::map<std::string, double> joint_position_map;
+
+    for (unsigned int i = 0; i < _req->joint_names.size(); i++)
+    {
+      joint_position_map[_req->joint_names[i]] = _req->joint_positions[i];
+    }
+
+    // make the service call to pause gazebo
+    bool is_paused = world_->IsPaused();
+    if (!is_paused) world_->SetPaused(true);
+
+    gazebo_model->SetJointPositions(joint_position_map);
+
+    // resume paused state before this call
+    world_->SetPaused(is_paused);
+
+    _res->success = true;
+    _res->status_message = "SetModelConfiguration: success";
+    return;
+  }
+  else
+  {
+    _res->success = false;
+    _res->status_message = "SetModelConfiguration: joint name and position list have different lengths";
+    return;
+  }
+}
+
 GZ_REGISTER_WORLD_PLUGIN(GazeboRosProperties)
 
 }  // namespace gazebo_ros

But after making these changes and launching gazebo, I realised that the services still do not exist. Services that show up are:

/apply_joint_effort
/apply_link_wrench
/clear_joint_efforts
/clear_link_wrenches
/delete_entity
/gazebo/describe_parameters
/gazebo/get_parameter_types
/gazebo/get_parameters
/gazebo/list_parameters
/gazebo/set_parameters
/gazebo/set_parameters_atomically
/get_model_list
/pause_physics
/reset_simulation
/reset_world
/rviz/describe_parameters
/rviz/get_parameter_types
/rviz/get_parameters
/rviz/list_parameters
/rviz/set_parameters
/rviz/set_parameters_atomically
/spawn_entity
/unpause_physics

Link to the branch that I'm working on: https://github.com/Blebot0/gazebo_ros_pkgs/tree/ros2_set_model_configuration

Can any of the contributors help me with this?

[prakharg01]

#1503

I have created the pull request, the service works now in gazebo11 and ros2 humble.
@Blebot0 I had to load the plugins in the world file, and the service works!