[ros2] Port time commands (pause / reset)

.ros1_unported/gazebo_ros/include/gazebo_ros/gazebo_ros_api_plugin.h

@@ -165,18 +165,6 @@ class GazeboRosApiPlugin : public SystemPlugin
   /// \brief
   bool applyJointEffort(gazebo_msgs::ApplyJointEffort::Request &req,gazebo_msgs::ApplyJointEffort::Response &res);
 
-  /// \brief
-  bool resetSimulation(std_srvs::Empty::Request &req,std_srvs::Empty::Response &res);
-
-  /// \brief
-  bool resetWorld(std_srvs::Empty::Request &req,std_srvs::Empty::Response &res);
-
-  /// \brief
-  bool pausePhysics(std_srvs::Empty::Request &req,std_srvs::Empty::Response &res);
-
-  /// \brief
-  bool unpausePhysics(std_srvs::Empty::Request &req,std_srvs::Empty::Response &res);
-
   /// \brief
   bool clearJointForces(gazebo_msgs::JointRequest::Request &req,gazebo_msgs::JointRequest::Response &res);
   bool clearJointForces(std::string joint_name);
@@ -263,10 +251,6 @@ class GazeboRosApiPlugin : public SystemPlugin
   ros::ServiceServer set_joint_properties_service_;
   ros::ServiceServer apply_joint_effort_service_;
   ros::ServiceServer set_model_configuration_service_;
-  ros::ServiceServer reset_simulation_service_;
-  ros::ServiceServer reset_world_service_;
-  ros::ServiceServer pause_physics_service_;
-  ros::ServiceServer unpause_physics_service_;
   ros::ServiceServer clear_joint_forces_service_;
   ros::ServiceServer clear_body_wrenches_service_;
   ros::Publisher     pub_link_states_;

.ros1_unported/gazebo_ros/src/gazebo_ros_api_plugin.cpp

@@ -350,24 +350,6 @@ void GazeboRosApiPlugin::advertiseServices()
                                                                           ros::VoidPtr(), &gazebo_queue_);
   set_joint_properties_service_ = nh_->advertiseService(set_joint_properties_aso);
 
-  // Advertise more services on the custom queue
-  std::string pause_physics_service_name("pause_physics");
-  ros::AdvertiseServiceOptions pause_physics_aso =
-    ros::AdvertiseServiceOptions::create<std_srvs::Empty>(
-                                                          pause_physics_service_name,
-                                                          boost::bind(&GazeboRosApiPlugin::pausePhysics,this,_1,_2),
-                                                          ros::VoidPtr(), &gazebo_queue_);
-  pause_physics_service_ = nh_->advertiseService(pause_physics_aso);
-
-  // Advertise more services on the custom queue
-  std::string unpause_physics_service_name("unpause_physics");
-  ros::AdvertiseServiceOptions unpause_physics_aso =
-    ros::AdvertiseServiceOptions::create<std_srvs::Empty>(
-                                                          unpause_physics_service_name,
-                                                          boost::bind(&GazeboRosApiPlugin::unpausePhysics,this,_1,_2),
-                                                          ros::VoidPtr(), &gazebo_queue_);
-  unpause_physics_service_ = nh_->advertiseService(unpause_physics_aso);
-
   // Advertise more services on the custom queue
   std::string apply_body_wrench_service_name("apply_body_wrench");
   ros::AdvertiseServiceOptions apply_body_wrench_aso =
@@ -404,25 +386,6 @@ void GazeboRosApiPlugin::advertiseServices()
                                                                    ros::VoidPtr(), &gazebo_queue_);
   clear_body_wrenches_service_ = nh_->advertiseService(clear_body_wrenches_aso);
 
-  // Advertise more services on the custom queue
-  std::string reset_simulation_service_name("reset_simulation");
-  ros::AdvertiseServiceOptions reset_simulation_aso =
-    ros::AdvertiseServiceOptions::create<std_srvs::Empty>(
-                                                          reset_simulation_service_name,
-                                                          boost::bind(&GazeboRosApiPlugin::resetSimulation,this,_1,_2),
-                                                          ros::VoidPtr(), &gazebo_queue_);
-  reset_simulation_service_ = nh_->advertiseService(reset_simulation_aso);
-
-  // Advertise more services on the custom queue
-  std::string reset_world_service_name("reset_world");
-  ros::AdvertiseServiceOptions reset_world_aso =
-    ros::AdvertiseServiceOptions::create<std_srvs::Empty>(
-                                                          reset_world_service_name,
-                                                          boost::bind(&GazeboRosApiPlugin::resetWorld,this,_1,_2),
-                                                          ros::VoidPtr(), &gazebo_queue_);
-  reset_world_service_ = nh_->advertiseService(reset_world_aso);
-
-
   // set param for use_sim_time if not set by user already
   if(!(nh_->hasParam("/use_sim_time")))
     nh_->setParam("/use_sim_time", true);
@@ -956,30 +919,6 @@ bool GazeboRosApiPlugin::applyJointEffort(gazebo_msgs::ApplyJointEffort::Request
   return true;
 }
 
-bool GazeboRosApiPlugin::resetSimulation(std_srvs::Empty::Request &req,std_srvs::Empty::Response &res)
-{
-  world_->Reset();
-  return true;
-}
-
-bool GazeboRosApiPlugin::resetWorld(std_srvs::Empty::Request &req,std_srvs::Empty::Response &res)
-{
-  world_->ResetEntities(gazebo::physics::Base::MODEL);
-  return true;
-}
-
-bool GazeboRosApiPlugin::pausePhysics(std_srvs::Empty::Request &req,std_srvs::Empty::Response &res)
-{
-  world_->SetPaused(true);
-  return true;
-}
-
-bool GazeboRosApiPlugin::unpausePhysics(std_srvs::Empty::Request &req,std_srvs::Empty::Response &res)
-{
-  world_->SetPaused(false);
-  return true;
-}
-
 bool GazeboRosApiPlugin::clearJointForces(gazebo_msgs::JointRequest::Request &req,
                                           gazebo_msgs::JointRequest::Response &res)
 {

gazebo_ros/CMakeLists.txt

@@ -18,6 +18,7 @@ find_package(rclcpp REQUIRED)
 find_package(gazebo_dev REQUIRED)
 find_package(gazebo_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
+find_package(std_srvs REQUIRED)
 find_package(tinyxml_vendor REQUIRED)
 
 include_directories(
@@ -54,6 +55,7 @@ ament_target_dependencies(gazebo_ros_init
   "builtin_interfaces"
   "gazebo_dev"
   "rclcpp"
+  "std_srvs"
 )
 target_link_libraries(gazebo_ros_init
   gazebo_ros_node

gazebo_ros/package.xml

@@ -25,6 +25,7 @@
   <depend>gazebo_dev</depend>
   <depend>gazebo_msgs</depend>
   <depend>rclcpp</depend>
+  <depend>std_srvs</depend>
   <depend>tinyxml_vendor</depend>
 
   <build_export_depend>geometry_msgs</build_export_depend>

gazebo_ros/src/gazebo_ros_init.cpp

@@ -15,27 +15,96 @@
 #include "gazebo_ros/gazebo_ros_init.hpp"
 
 #include <gazebo/common/Plugin.hh>
+#include <gazebo/physics/PhysicsIface.hh>
+#include <gazebo/physics/World.hh>
+
 #include <gazebo_ros/conversions/builtin_interfaces.hpp>
 #include <gazebo_ros/node.hpp>
 #include <gazebo_ros/utils.hpp>
+
 #include <rosgraph_msgs/msg/clock.hpp>
+#include <std_srvs/srv/empty.hpp>
 
 #include <memory>
+#include <string>
 
 namespace gazebo_ros
 {
 
 class GazeboRosInitPrivate
 {
 public:
+  /// Constructor
   GazeboRosInitPrivate();
+
+  /// Callback when a world is created.
+  /// \param[in] _world_name The world's name
+  void OnWorldCreated(const std::string & _world_name);
+
+  /// Publish simulation time.
+  /// \param[in] _info World update information.
+  void PublishSimTime(const gazebo::common::UpdateInfo & _info);
+
+  /// Callback from ROS service to reset simulation.
+  /// \param[in] req Empty request
+  /// \param[out] res Empty response
+  void OnResetSimulation(
+    std_srvs::srv::Empty::Request::SharedPtr req,
+    std_srvs::srv::Empty::Response::SharedPtr res);
+
+  /// Callback from ROS service to reset world.
+  /// \param[in] req Empty request
+  /// \param[out] res Empty response
+  void OnResetWorld(
+    std_srvs::srv::Empty::Request::SharedPtr req,
+    std_srvs::srv::Empty::Response::SharedPtr res);
+
+  /// Callback from ROS service to pause physics.
+  /// \param[in] req Empty request
+  /// \param[out] res Empty response
+  void OnPause(
+    std_srvs::srv::Empty::Request::SharedPtr req,
+    std_srvs::srv::Empty::Response::SharedPtr res);
+
+  /// Callback from ROS service to unpause (play) physics.
+  /// \param[in] req Empty request
+  /// \param[out] res Empty response
+  void OnUnpause(
+    std_srvs::srv::Empty::Request::SharedPtr req,
+    std_srvs::srv::Empty::Response::SharedPtr res);
+
+  /// \brief Keep a pointer to the world.
+  gazebo::physics::WorldPtr world_;
+
+  /// Gazebo-ROS node
   gazebo_ros::Node::SharedPtr ros_node_;
+
+  /// Publishes simulation time
   rclcpp::Publisher<rosgraph_msgs::msg::Clock>::SharedPtr clock_pub_;
+
+  /// ROS service to handle requests to reset simulation.
+  rclcpp::Service<std_srvs::srv::Empty>::SharedPtr reset_simulation_service_;
+
+  /// ROS service to handle requests to reset world.
+  rclcpp::Service<std_srvs::srv::Empty>::SharedPtr reset_world_service_;
+
+  /// ROS service to handle requests to pause physics.
+  rclcpp::Service<std_srvs::srv::Empty>::SharedPtr pause_service_;
+
+  /// ROS service to handle requests to unpause physics.
+  rclcpp::Service<std_srvs::srv::Empty>::SharedPtr unpause_service_;
+
+  /// Connection to world update event, called at every iteration
   gazebo::event::ConnectionPtr world_update_event_;
+
+  /// To be notified once the world is created.
+  gazebo::event::ConnectionPtr world_created_event_;
+
+  /// Throttler for clock publisher.
   gazebo_ros::Throttler throttler_;
-  static constexpr double DEFAULT_PUBLISH_FREQUENCY = 10.;
 
-  void PublishSimTime(const gazebo::common::UpdateInfo & _info);
+  /// Default frequency for clock publisher.
+  static constexpr double DEFAULT_PUBLISH_FREQUENCY = 10.;
 };
 
 GazeboRosInit::GazeboRosInit()
@@ -85,6 +154,36 @@ void GazeboRosInit::Load(int argc, char ** argv)
 
   impl_->world_update_event_ = gazebo::event::Events::ConnectWorldUpdateBegin(
     std::bind(&GazeboRosInitPrivate::PublishSimTime, impl_.get(), std::placeholders::_1));
+
+  // Get a callback when a world is created
+  impl_->world_created_event_ = gazebo::event::Events::ConnectWorldCreated(
+    std::bind(&GazeboRosInitPrivate::OnWorldCreated, impl_.get(), std::placeholders::_1));
+}
+
+void GazeboRosInitPrivate::OnWorldCreated(const std::string & _world_name)
+{
+  // Only support one world
+  world_created_event_.reset();
+
+  world_ = gazebo::physics::get_world(_world_name);
+
+  // Reset services
+  reset_simulation_service_ = ros_node_->create_service<std_srvs::srv::Empty>("reset_simulation",
+      std::bind(&GazeboRosInitPrivate::OnResetSimulation, this,
+      std::placeholders::_1, std::placeholders::_2));
+
+  reset_world_service_ = ros_node_->create_service<std_srvs::srv::Empty>("reset_world",
+      std::bind(&GazeboRosInitPrivate::OnResetWorld, this,
+      std::placeholders::_1, std::placeholders::_2));
+
+  // Pause services
+  pause_service_ = ros_node_->create_service<std_srvs::srv::Empty>("pause_physics",
+      std::bind(&GazeboRosInitPrivate::OnPause, this,
+      std::placeholders::_1, std::placeholders::_2));
+
+  unpause_service_ = ros_node_->create_service<std_srvs::srv::Empty>("unpause_physics",
+      std::bind(&GazeboRosInitPrivate::OnUnpause, this,
+      std::placeholders::_1, std::placeholders::_2));
 }
 
 GazeboRosInitPrivate::GazeboRosInitPrivate()
@@ -94,13 +193,43 @@ GazeboRosInitPrivate::GazeboRosInitPrivate()
 
 void GazeboRosInitPrivate::PublishSimTime(const gazebo::common::UpdateInfo & _info)
 {
-  if (!throttler_.IsReady(_info.realTime)) {return;}
+  if (!throttler_.IsReady(_info.realTime)) {
+    return;
+  }
 
   rosgraph_msgs::msg::Clock clock;
   clock.clock = gazebo_ros::Convert<builtin_interfaces::msg::Time>(_info.simTime);
   clock_pub_->publish(clock);
 }
 
+void GazeboRosInitPrivate::OnResetSimulation(
+  std_srvs::srv::Empty::Request::SharedPtr,
+  std_srvs::srv::Empty::Response::SharedPtr)
+{
+  world_->Reset();
+}
+
+void GazeboRosInitPrivate::OnResetWorld(
+  std_srvs::srv::Empty::Request::SharedPtr,
+  std_srvs::srv::Empty::Response::SharedPtr)
+{
+  world_->ResetEntities(gazebo::physics::Base::MODEL);
+}
+
+void GazeboRosInitPrivate::OnPause(
+  std_srvs::srv::Empty::Request::SharedPtr,
+  std_srvs::srv::Empty::Response::SharedPtr)
+{
+  world_->SetPaused(true);
+}
+
+void GazeboRosInitPrivate::OnUnpause(
+  std_srvs::srv::Empty::Request::SharedPtr,
+  std_srvs::srv::Empty::Response::SharedPtr)
+{
+  world_->SetPaused(false);
+}
+
 GZ_REGISTER_SYSTEM_PLUGIN(GazeboRosInit)
 
 }  // namespace gazebo_ros

gazebo_ros/test/CMakeLists.txt

@@ -4,6 +4,7 @@ find_package(gazebo_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(std_msgs REQUIRED)
+find_package(std_srvs REQUIRED)
 
 # Plugins
 set(plugins
@@ -31,6 +32,7 @@ file(COPY worlds DESTINATION .)
 set(tests
   test_conversions
   test_gazebo_ros_factory
+  test_gazebo_ros_init
   test_gazebo_ros_state
   test_node
   test_plugins
@@ -60,6 +62,7 @@ foreach(test ${tests})
     "rclcpp"
     "sensor_msgs"
     "std_msgs"
+    "std_srvs"
   )
 endforeach()