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>