Add resetSimulationState and regenerate thrift-generated files

plugins/clock/include/GazeboYarpPlugins/ClockServerImpl.h

@@ -27,6 +27,7 @@ class GazeboYarpPlugins::ClockServerImpl : public GazeboYarpPlugins::ClockServer
     virtual void resetSimulation();
     virtual double getSimulationTime();
     virtual double getStepSize();
+    virtual void resetSimulationState();
 
 private:
 

plugins/clock/include/gazebo/Clock.hh

@@ -101,6 +101,11 @@ public:
      */
     void resetSimulationTime();
 
+    /**
+     * Reset the simulation state back to initial state
+     */
+    void resetSimulationState();
+
     /**
      * Reset the simulation time and state back to zero
      */

plugins/clock/src/Clock.cc

@@ -12,6 +12,7 @@
 #include <gazebo/common/Events.hh>
 #include <gazebo/physics/PhysicsEngine.hh>
 #include <gazebo/physics/World.hh>
+#include <gazebo/physics/physics.hh>
 #include <gazebo/physics/PhysicsIface.hh>
 #include <yarp/os/Network.h>
 #include <yarp/os/Property.h>
@@ -30,7 +31,6 @@ namespace gazebo
     , m_rpcPort(0)
     , m_clockServer(0)
     {
-
     }
 
     GazeboYarpClock::~GazeboYarpClock()
@@ -182,6 +182,12 @@ namespace gazebo
         m_world->Reset();
     }
 
+    void GazeboYarpClock::resetSimulationState()
+    {
+        m_world->ResetEntities(gazebo::physics::Base::BASE);
+        event::Events::worldReset();
+    }    
+
     common::Time GazeboYarpClock::getSimulationTime()
     {
 #if GAZEBO_MAJOR_VERSION >= 8

plugins/clock/src/ClockServerImpl.cpp

@@ -52,4 +52,9 @@ namespace GazeboYarpPlugins {
     {
         return m_clock.getStepSize();
     }
+
+    void ClockServerImpl::resetSimulationState()
+    {
+        m_clock.resetSimulationState();
+    }
 }

thrift/clock/autogenerated/clock_rpc_thrift.cmake

@@ -1,4 +1,4 @@
-# Copyright (C) 2006-2018 Istituto Italiano di Tecnologia (IIT)
+# Copyright (C) 2006-2019 Istituto Italiano di Tecnologia (IIT)
 # All rights reserved.
 #
 # This software may be modified and distributed under the terms of the

thrift/clock/autogenerated/include/ClockServer.h

@@ -1,71 +1,92 @@
 /*
- * Copyright (C) 2006-2018 Istituto Italiano di Tecnologia (IIT)
+ * Copyright (C) 2006-2019 Istituto Italiano di Tecnologia (IIT)
  * All rights reserved.
  *
  * This software may be modified and distributed under the terms of the
  * BSD-3-Clause license. See the accompanying LICENSE file for details.
  */
 
+// Autogenerated by Thrift Compiler (0.12.0-yarped)
+//
 // This is an automatically generated file.
 // It could get re-generated if the ALLOW_IDL_GENERATION flag is on.
 
-#ifndef YARP_THRIFT_GENERATOR_ClockServer
-#define YARP_THRIFT_GENERATOR_ClockServer
+#ifndef YARP_THRIFT_GENERATOR_SERVICE_CLOCKSERVER_H
+#define YARP_THRIFT_GENERATOR_SERVICE_CLOCKSERVER_H
 
 #include <yarp/os/Wire.h>
 #include <yarp/os/idl/WireTypes.h>
 
 namespace GazeboYarpPlugins {
-  class ClockServer;
-}
 
-
-class GazeboYarpPlugins::ClockServer : public yarp::os::Wire {
+class ClockServer :
+        public yarp::os::Wire
+{
 public:
-  ClockServer();
-  /**
-   * Pause the simulation if it was running
-   */
-  virtual void pauseSimulation();
-  /**
-   * Resume the simulation if it was paused
-   */
-  virtual void continueSimulation();
-  /**
-   * Steps the simulation for the provided number of steps.
-   * The input paramter is the number of steps, not the time (Usually 1 step = 1ms but this is not guaranteed)
-   * @note: this function (will be) not blocking, i.e. it will return immediately. Currently calling this function
-   * twice before the previous call actually ends its computation gives and undefined behavior.
-   * @param numberOfSteps number of steps to simulate
-   */
-  virtual void stepSimulation(const std::int32_t numberOfSteps = 1);
-  /**
-   * Steps the simulation for the provided number of steps.
-   * The input paramter is the number of steps, not the time (Usually 1 step = 1ms but this is not guaranteed)
-   * @note: this function is blocking
-   * @param numberOfSteps number of steps to simulate
-   */
-  virtual void stepSimulationAndWait(const std::int32_t numberOfSteps = 1);
-  /**
-   * Reset the simulation time back to zero
-   */
-  virtual void resetSimulationTime();
-  /**
-   * Get the current simulation time
-   * @return the simulation time.
-   */
-  virtual double getSimulationTime();
-  /**
-   * Get the current step size in seconds.
-   * @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");
+    // Constructor
+    ClockServer();
+
+    /**
+     * Pause the simulation if it was running
+     */
+    virtual void pauseSimulation();
+
+    /**
+     * Resume the simulation if it was paused
+     */
+    virtual void continueSimulation();
+
+    /**
+     * Steps the simulation for the provided number of steps.
+     * The input paramter is the number of steps, not the time (Usually 1 step = 1ms but this is not guaranteed)
+     * @note: this function (will be) not blocking, i.e. it will return immediately. Currently calling this function
+     * twice before the previous call actually ends its computation gives and undefined behavior.
+     * @param numberOfSteps number of steps to simulate
+     */
+    virtual void stepSimulation(const std::int32_t numberOfSteps = 1);
+
+    /**
+     * Steps the simulation for the provided number of steps.
+     * The input paramter is the number of steps, not the time (Usually 1 step = 1ms but this is not guaranteed)
+     * @note: this function is blocking
+     * @param numberOfSteps number of steps to simulate
+     */
+    virtual void stepSimulationAndWait(const std::int32_t numberOfSteps = 1);
+
+    /**
+     * Reset the simulation time back to zero
+     */
+    virtual void resetSimulationTime();
+
+    /**
+     * Get the current simulation time
+     * @return the simulation time.
+     */
+    virtual double getSimulationTime();
+
+    /**
+     * Get the current step size in seconds.
+     * @return the step size in seconds
+     */
+    virtual double getStepSize();
+
+    /**
+     * Reset the simulation state and time
+     */
+    virtual void resetSimulation();
+
+    /**
+     * Reset the simulation state back to initial pose
+     */
+    virtual void resetSimulationState();
+
+    // help method
+    virtual std::vector<std::string> help(const std::string& functionName = "--all");
+
+    // read from ConnectionReader
+    bool read(yarp::os::ConnectionReader& connection) override;
 };
 
-#endif
+} // namespace GazeboYarpPlugins
+
+#endif // YARP_THRIFT_GENERATOR_SERVICE_CLOCKSERVER_H