Added Reset Simulation

plugins/clock/include/GazeboYarpPlugins/ClockServerImpl.h

@@ -23,7 +23,8 @@ class GazeboYarpPlugins::ClockServerImpl : public GazeboYarpPlugins::ClockServer
     virtual void continueSimulation();
     virtual void stepSimulation(const int32_t numberOfSteps = 1);
     virtual void stepSimulationAndWait(const int32_t numberOfSteps = 1);
-    void resetSimulationTime();
+    virtual void resetSimulationTime();
+    virtual void resetSimulation();
     virtual double getSimulationTime();
     virtual double getStepSize();
 

plugins/clock/include/gazebo/Clock.hh

@@ -100,6 +100,11 @@ public:
      * Reset the simulation time back to zero
      */
     void resetSimulationTime();
+
+    /**
+     * Reset the simulation time and state back to zero
+     */
+    void resetSimulation();    
 
     /**
      * Get the current step size in seconds.

plugins/clock/src/Clock.cc

@@ -177,6 +177,12 @@ namespace gazebo
         m_world->ResetTime();
     }
 
+    void GazeboYarpClock::resetSimulation()
+    {
+        //m_world->ResetEntities(gazebo::physics::Base::MODEL);
+        m_world->Reset();
+    }
+
     common::Time GazeboYarpClock::getSimulationTime()
     {
 #if GAZEBO_MAJOR_VERSION >= 8

plugins/clock/src/ClockServerImpl.cpp

@@ -37,6 +37,11 @@ namespace GazeboYarpPlugins {
     {
         m_clock.resetSimulationTime();
     }
+
+    void ClockServerImpl::resetSimulation()
+    {
+        m_clock.resetSimulation();
+    }
 
     double ClockServerImpl::getSimulationTime()
     {

thrift/clock/autogenerated/include/ClockServer.h

@@ -60,6 +60,10 @@ class GazeboYarpPlugins::ClockServer : public yarp::os::Wire {
    * @return the step size in seconds
    */
   virtual double getStepSize();
+  /**
+   * Reset the simulation state and time
+   */
+  virtual void resetSimulation();
   virtual bool read(yarp::os::ConnectionReader& connection) override;
   virtual std::vector<std::string> help(const std::string& functionName="--all");
 };

thrift/clock/autogenerated/src/ClockServer.cpp

@@ -68,6 +68,13 @@ class ClockServer_getStepSize : public yarp::os::Portable {
   virtual bool read(yarp::os::ConnectionReader& connection) override;
 };
 
+class ClockServer_resetSimulation : public yarp::os::Portable {
+public:
+  void init();
+  virtual bool write(yarp::os::ConnectionWriter& connection) const override;
+  virtual bool read(yarp::os::ConnectionReader& connection) override;
+};
+
 bool ClockServer_pauseSimulation::write(yarp::os::ConnectionWriter& connection) const {
   yarp::os::idl::WireWriter writer(connection);
   if (!writer.writeListHeader(1)) return false;
@@ -190,6 +197,21 @@ void ClockServer_getStepSize::init() {
   _return = (double)0;
 }
 
+bool ClockServer_resetSimulation::write(yarp::os::ConnectionWriter& connection) const {
+  yarp::os::idl::WireWriter writer(connection);
+  if (!writer.writeListHeader(1)) return false;
+  if (!writer.writeTag("resetSimulation",1,1)) return false;
+  return true;
+}
+
+bool ClockServer_resetSimulation::read(yarp::os::ConnectionReader& connection) {
+  YARP_UNUSED(connection);
+  return true;
+}
+
+void ClockServer_resetSimulation::init() {
+}
+
 ClockServer::ClockServer() {
   yarp().setOwner(*this);
 }
@@ -253,6 +275,14 @@ double ClockServer::getStepSize() {
   bool ok = yarp().write(helper,helper);
   return ok?helper._return:_return;
 }
+void ClockServer::resetSimulation() {
+  ClockServer_resetSimulation helper;
+  helper.init();
+  if (!yarp().canWrite()) {
+    yError("Missing server method '%s'?","void ClockServer::resetSimulation()");
+  }
+  yarp().write(helper);
+}
 
 bool ClockServer::read(yarp::os::ConnectionReader& connection) {
   yarp::os::idl::WireReader reader(connection);
@@ -362,6 +392,21 @@ bool ClockServer::read(yarp::os::ConnectionReader& connection) {
       reader.accept();
       return true;
     }
+    if (tag == "resetSimulation") {
+      if (!direct) {
+        ClockServer_resetSimulation helper;
+        helper.init();
+        yarp().callback(helper,*this,"__direct__");
+      } else {
+        resetSimulation();
+      }
+      yarp::os::idl::WireWriter writer(reader);
+      if (!writer.isNull()) {
+        if (!writer.writeOnewayResponse()) return false;
+      }
+      reader.accept();
+      return true;
+    }
     if (tag == "help") {
       std::string functionName;
       if (!reader.readString(functionName)) {
@@ -403,6 +448,7 @@ std::vector<std::string> ClockServer::help(const std::string& functionName) {
     helpString.push_back("resetSimulationTime");
     helpString.push_back("getSimulationTime");
     helpString.push_back("getStepSize");
+    helpString.push_back("resetSimulation");
     helpString.push_back("help");
   }
   else {
@@ -443,6 +489,10 @@ std::vector<std::string> ClockServer::help(const std::string& functionName) {
       helpString.push_back("Get the current step size in seconds. ");
       helpString.push_back("@return the step size in seconds ");
     }
+    if (functionName=="resetSimulation") {
+      helpString.push_back("void resetSimulation() ");
+      helpString.push_back("Reset the simulation state and time ");
+    }
     if (functionName=="help") {
       helpString.push_back("std::vector<std::string> help(const std::string& functionName=\"--all\")");
       helpString.push_back("Return list of available commands, or help message for a specific function");

thrift/clock/clock_rpc.thrift

@@ -41,5 +41,10 @@ service ClockServer {
      * @return the step size in seconds
      */
     double getStepSize();
+
+    /** Reset the simulation state and time
+     *
+     */
+    oneway void resetSimulation();
 
 }