Fix set_simulation_state STOPPED timeout

ros_gz_sim/src/gz_simulation_interfaces/gazebo_proxy.cpp

@@ -218,11 +218,16 @@ bool GazeboProxy::AssertUpdatedWorldStats(simulation_interfaces::msg::Result & r
   return true;
 }
 
+void GazeboProxy::ArmResetDetection()
+{
+  std::lock_guard<std::mutex> lk(this->reset_detected_mutex_);
+  this->reset_detected_ = false;
+}
+
 bool GazeboProxy::WaitForResetDetected()
 {
   std::unique_lock lk(this->reset_detected_mutex_);
-  this->reset_detected_ = false;
-  if(!this->reset_detected_cv_.wait_for(
+  if (!this->reset_detected_cv_.wait_for(
     lk, std::chrono::milliseconds(kGzServiceTimeoutMs), [this] {return this->reset_detected_;}))
   {
     return false;
@@ -327,6 +332,9 @@ void GazeboProxy::InitializeCanonicalLinks(
   }
 
   gz::msgs::WorldControlState control_msg;
+  // Gazebo applies world_control even when this request is only synchronizing state. Preserve the
+  // current pause state so canonical-link synchronization doesn't implicitly unpause simulation.
+  control_msg.mutable_world_control()->set_pause(this->Paused());
   control_msg.mutable_state()->CopyFrom(this->ecm_.State(
     canonicalLinkEntities, {components::WorldPose::typeId, components::WorldLinearVelocity::typeId,
         components::WorldAngularVelocity::typeId}));

ros_gz_sim/src/gz_simulation_interfaces/gazebo_proxy.hpp

@@ -129,7 +129,10 @@ class GazeboProxy
   /// \return True if the world stats has been updated before a timeout occurred.
   bool AssertUpdatedWorldStats(simulation_interfaces::msg::Result & result);
 
-  /// \brief Wait until simulation reset is detected
+  /// \brief Arm reset detection before sending a reset request.
+  void ArmResetDetection();
+
+  /// \brief Wait until simulation reset is detected.
   /// \return True if reset was detected.
   bool WaitForResetDetected();
 

ros_gz_sim/src/gz_simulation_interfaces/services/reset_simulation.cpp

@@ -17,7 +17,6 @@
 #include <gz/msgs/boolean.pb.h>
 #include <gz/msgs/world_control.pb.h>
 
-#include <future>
 #include <memory>
 
 #include "../gazebo_proxy.hpp"
@@ -45,11 +44,6 @@ ResetSimulation::ResetSimulation(
   }
   this->services_handle_ = ros_node->create_service<ResetSimulationSrv>(
     "reset_simulation", [this, control_service](RequestPtr request, ResponsePtr response) {
-      auto reset_detected_future = std::async(std::launch::async, [this]
-      {
-        return this->gz_proxy_->WaitForResetDetected();
-      });
-
       using Result = simulation_interfaces::msg::Result;
       if (
         request->scope != ResetSimulationSrv::Request::SCOPE_DEFAULT &&
@@ -66,6 +60,7 @@ ResetSimulation::ResetSimulation(
       gz_request.mutable_reset()->set_all(true);
       // TODO(azeey) Resetting only the time, state or spawned models is not supported yet in Gazebo
 
+      this->gz_proxy_->ArmResetDetection();
       bool result;
       gz::msgs::Boolean reply;
       bool executed = this->gz_proxy_->GzNode()->Request(
@@ -83,7 +78,7 @@ ResetSimulation::ResetSimulation(
       // Since the "control" service is asynchronous, getting results from the service doesn't mean
       // the request has taken effect. Therefore, we wait here until the desired state is reached or
       // a timeout occurs.
-      if (!reset_detected_future.get()) {
+      if (!this->gz_proxy_->WaitForResetDetected()) {
         response->result.result = Result::RESULT_OPERATION_FAILED;
         response->result.error_message = "Timed out while trying to reset simulation";
       }

ros_gz_sim/src/gz_simulation_interfaces/services/set_simulation_state.cpp

@@ -52,8 +52,8 @@ SetSimulationState::SetSimulationState(
       gz::msgs::WorldControl gz_request;
       switch (request->state.state) {
         case SimulationState::STATE_STOPPED:
+          // STOPPED is a two-stage transition. Pause first, then reset while preserving pause.
           gz_request.set_pause(true);
-          gz_request.mutable_reset()->set_all(true);
           break;
         case SimulationState::STATE_PAUSED:
           gz_request.set_pause(true);
@@ -82,6 +82,67 @@ SetSimulationState::SetSimulationState(
         response->result.error_message = "Unknown error while trying to reset simulation";
       }
 
+      if (request->state.state == SimulationState::STATE_STOPPED) {
+        if (!executed || !result || !reply.data()) {
+          return;
+        }
+
+        bool state_reached = this->gz_proxy_->Paused();
+        auto t_init = std::chrono::steady_clock::now();
+        auto timeout = std::chrono::milliseconds(GazeboProxy::kGzStateUpdatedTimeoutMs);
+        while (
+          !state_reached && (std::chrono::steady_clock::now() - t_init) < timeout)
+        {
+          if (!this->gz_proxy_->AssertUpdatedWorldStats(response->result)) {
+            return;
+          }
+          state_reached = this->gz_proxy_->Paused();
+        }
+        if (!state_reached) {
+          response->result.result = Result::RESULT_OPERATION_FAILED;
+          response->result.error_message = "Timed out while trying to pause simulation";
+          return;
+        }
+
+        this->gz_proxy_->ArmResetDetection();
+        gz::msgs::WorldControl reset_request;
+        reset_request.set_pause(true);
+        reset_request.mutable_reset()->set_all(true);
+        executed = this->gz_proxy_->GzNode()->Request(
+          control_service, reset_request, GazeboProxy::kGzServiceTimeoutMs, reply, result);
+        if (!executed) {
+          response->result.result = Result::RESULT_OPERATION_FAILED;
+          response->result.error_message = "Timed out while trying to reset simulation";
+          return;
+        }
+        if (!result || !reply.data()) {
+          response->result.result = Result::RESULT_OPERATION_FAILED;
+          response->result.error_message = "Unknown error while trying to reset simulation";
+          return;
+        }
+        if (!this->gz_proxy_->WaitForResetDetected()) {
+          response->result.result = Result::RESULT_OPERATION_FAILED;
+          response->result.error_message = "Timed out while trying to reset simulation";
+          return;
+        }
+
+        state_reached = this->gz_proxy_->Paused() && (this->gz_proxy_->Iterations() == 0);
+        t_init = std::chrono::steady_clock::now();
+        while (
+          !state_reached && (std::chrono::steady_clock::now() - t_init) < timeout)
+        {
+          if (!this->gz_proxy_->AssertUpdatedWorldStats(response->result)) {
+            return;
+          }
+          state_reached = this->gz_proxy_->Paused() && (this->gz_proxy_->Iterations() == 0);
+        }
+        if (!state_reached) {
+          response->result.result = Result::RESULT_OPERATION_FAILED;
+          response->result.error_message = "Timed out while trying to stop simulation";
+        }
+        return;
+      }
+
       // Since the "control" service is asynchronous, getting results from the service doesn't mean
       // the request has taken effect. Therefore, we wait here until the desired state is reached or
       // a timeout occurs.
@@ -91,11 +152,6 @@ SetSimulationState::SetSimulationState(
       while (!state_reached && (std::chrono::steady_clock::now() - t_init) < timeout) {
         this->gz_proxy_->AssertUpdatedState(response->result);
         switch (request->state.state) {
-          case SimulationState::STATE_STOPPED:
-            if (this->gz_proxy_->Paused() && (this->gz_proxy_->Iterations() == 0)) {
-              state_reached = true;
-            }
-            break;
           case SimulationState::STATE_PAUSED:
             if (this->gz_proxy_->Paused()) {
               state_reached = true;

ros_gz_sim/test/test_gz_simulation_interfaces.launch.py

@@ -323,6 +323,21 @@ def test_toggle_simulation_state(self) -> None:
         restored_state = self.get_simulation_state().state.state
         self.assertEqual(restored_state, initial_state)
 
+    def test_stop_simulation_state(self) -> None:
+        # Regression test for #919: transitioning from PLAYING to STOPPED must report success.
+        self.set_simulation_state(SimulationState.STATE_PLAYING)
+        self.assertEqual(
+            self.get_simulation_state().state.state,
+            SimulationState.STATE_PLAYING)
+
+        self.set_simulation_state(SimulationState.STATE_STOPPED)
+        self.assertEqual(
+            self.get_simulation_state().state.state,
+            SimulationState.STATE_STOPPED)
+
+        # Avoid leaking STOPPED into existing state-transition tests.
+        self.set_simulation_state(SimulationState.STATE_PLAYING)
+
     def test_playing_when_already_playing(self) -> None:
         # Try to set it to the same state twice
         self.set_simulation_state(SimulationState.STATE_PLAYING)