Step by step simulation c++ with gazebo

Issue #2385 open
Brice Clement
created an issue

HI,

We are trying to use gazebo in a step by step way, because we have a controller running at some frequency, and we want to avoid asynchronous issue.

So we looked at the example of your Custom main, and we found that your are using :

gazebo::runWorld(world, iterations);

Where iteration is the number of iteration to perform. But this function can't be used like this :

 while (!stop) {
    gazebo::runWorld(world, 1);
    //  do some stuff
  }

Because it seems like it's reseting everythings from scratch.

So we would like to use the world->Step() function which is private, to run step by step the simulation.

Best regards,

Comments (7)

  1. Shane Loretz

    I think you are looking for this version of World::Step()

          /// \brief Step the world forward in time.
          /// \param[in] _steps The number of steps the World should take.
          public: void Step(const unsigned int _steps);
    

    https://bitbucket.org/osrf/gazebo/src/7da89efd6c8f5d6fb9e0989684c893736e004988/gazebo/physics/World.hh?at=gazebo8&fileviewer=file-view-default#World.hh-449

    The integration tests have examples of using it.

    https://bitbucket.org/osrf/gazebo/src/7da89efd6c8f5d6fb9e0989684c893736e004988/test/integration/plugin.cc?at=gazebo8&fileviewer=file-view-default#plugin.cc-107

    I will close this issue since I think that answers the question. Feel free to reopen if there is more you would like to discuss.

  2. Brice Clement reporter

    Hi,

    This function does nothing, here are the implementation :

    void World::Step(const unsigned int _steps)
    {
      if (!this->IsPaused())
      {
        gzwarn << "Calling World::Step(steps) while world is not paused\n";
        this->SetPaused(true);
      }
    
      {
        std::lock_guard<std::recursive_mutex> lock(this->dataPtr->worldUpdateMutex);
        this->dataPtr->stepInc = _steps;
      }
    
      // block on completion
      bool wait = true;
      while (wait)
      {
        common::Time::NSleep(1);
        std::lock_guard<std::recursive_mutex> lock(this->dataPtr->worldUpdateMutex);
        if (this->dataPtr->stepInc == 0 || this->dataPtr->stop)
          wait = false;
      }
    }
    

    And this is the Step() :

    void World::Step()
    {
      DIAG_TIMER_START("World::Step");
    
      /// need this because ODE does not call dxReallocateWorldProcessContext()
      /// until dWorld.*Step
      /// Plugins that manipulate joints (and probably other properties) require
      /// one iteration of the physics engine. Do not remove this.
      if (!this->dataPtr->pluginsLoaded && this->SensorsInitialized())
      {
        this->LoadPlugins();
        this->dataPtr->pluginsLoaded = true;
      }
    
      DIAG_TIMER_LAP("World::Step", "loadPlugins");
    
      // Send statistics about the world simulation
      this->PublishWorldStats();
    
      DIAG_TIMER_LAP("World::Step", "publishWorldStats");
    
      double updatePeriod = this->dataPtr->physicsEngine->GetUpdatePeriod();
      // sleep here to get the correct update rate
      common::Time tmpTime = common::Time::GetWallTime();
      common::Time sleepTime = this->dataPtr->prevStepWallTime +
        common::Time(updatePeriod) - tmpTime - this->dataPtr->sleepOffset;
    
      common::Time actualSleep;
      if (sleepTime > 0)
      {
        common::Time::Sleep(sleepTime);
        actualSleep = common::Time::GetWallTime() - tmpTime;
      }
      else
        sleepTime = 0;
    
      // exponentially avg out
      this->dataPtr->sleepOffset = (actualSleep - sleepTime) * 0.01 +
                          this->dataPtr->sleepOffset * 0.99;
    
      DIAG_TIMER_LAP("World::Step", "sleepOffset");
    
      // throttling update rate, with sleepOffset as tolerance
      // the tolerance is needed as the sleep time is not exact
      if (common::Time::GetWallTime() - this->dataPtr->prevStepWallTime +
          this->dataPtr->sleepOffset >= common::Time(updatePeriod))
      {
        std::lock_guard<std::recursive_mutex> lock(this->dataPtr->worldUpdateMutex);
    
        DIAG_TIMER_LAP("World::Step", "worldUpdateMutex");
    
        this->dataPtr->prevStepWallTime = common::Time::GetWallTime();
    
        double stepTime = this->dataPtr->physicsEngine->GetMaxStepSize();
    
        if (!this->IsPaused() || this->dataPtr->stepInc > 0
            || this->dataPtr->needsReset)
        {
          // query timestep to allow dynamic time step size updates
          this->dataPtr->simTime += stepTime;
          this->dataPtr->iterations++;
          this->Update();
    
          DIAG_TIMER_LAP("World::Step", "update");
    
          if (this->IsPaused() && this->dataPtr->stepInc > 0)
            this->dataPtr->stepInc--;
        }
        else
        {
          // Flush the log record buffer, if there is data in it.
          if (util::LogRecord::Instance()->BufferSize() > 0)
            util::LogRecord::Instance()->Notify();
          this->dataPtr->pauseTime += stepTime;
        }
      }
    
      gazebo::util::IntrospectionManager::Instance()->NotifyUpdates();
    
      this->ProcessMessages();
    
      DIAG_TIMER_STOP("World::Step");
    
      if (g_clearModels)
        this->ClearModels();
    }
    

    Best regards,

  3. Shane Loretz

    It assumes the world is updating in another thread. The ServerFixture used by the integration tests shows how to set that up.

    https://bitbucket.org/osrf/gazebo/src/7da89efd6c8f5d6fb9e0989684c893736e004988/gazebo/test/ServerFixture.cc?at=gazebo8&fileviewer=file-view-default#ServerFixture.cc-200

    It sounds like you might be interested in fewer API calls to get to that point. Feel free to update this issue with a proposal for what you would that API to look like.

  4. Brice Clement reporter

    Hi,

    I'm not sure if i understood what you wanted to show me about the ServerFixture, but we would like to avoid to have the world in a thread. Just something like your Step() function (the one implemented without parameters), but if it's possible to make it public instead of private. So we can access it.

    Best regards

  5. Log in to comment