gazebo_ros: Added set_model_configuration service to ROS2(Humble)

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 @@ class GazeboRosPropertiesPrivate
     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 @@ class GazeboRosPropertiesPrivate
   /// \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
+}  // namespace gazebo_ros

gazebo_ros/worlds/empty.world

@@ -9,5 +9,12 @@
     <include>
       <uri>model://ground_plane</uri>
     </include>
+
+    <plugin name="gazebo_ros_properties" filename="libgazebo_ros_properties.so">
+      <ros>
+        <namespace>/gazebo</namespace>
+      </ros>
+      <update_rate>1.0</update_rate>
+    </plugin>
   </world>
 </sdf>