Pass time since last call to method one_iteration.

Author: florent-lamiraux

src/roscontrol-sot-controller.cpp

@@ -955,7 +955,7 @@ void RCSotController::readControl(
   }
 }
 
-void RCSotController::one_iteration() {
+void RCSotController::one_iteration(const ros::Duration &period) {
   sotComputing_ = true;
   // Chrono start
   RcSotLog_.start_it();
@@ -975,7 +975,7 @@ void RCSotController::one_iteration() {
     throw e;
   }
   try {
-    sotController_->getControl(controlValues_copy_);
+    sotController_->getControl(controlValues_copy_, period.toSec());
   } catch (std::exception &e) {
     std::cerr << "Failure happened during one_iteration(): "
               << "when calling getControl " << std::endl;
@@ -1115,15 +1115,15 @@ void RCSotController::update(const ros::Time &, const ros::Duration &period) {
       // If subsampling is equal to 1, i.e. no subsampling, evaluate
       // control graph in this thread
       if (subSampling_ == 1) {
-        one_iteration();
+        one_iteration(period);
         readControl(controlValues_);
       } else {
         if (step_ == 0) {
           // If previous graph evaluation is not finished, do not start a new
           // one.
           if (!sotComputing_) {
             io_service_.post(
-                boost::bind(&RCSotController::one_iteration, this));
+                boost::bind(&RCSotController::one_iteration, this, period));
           } else {
             ROS_INFO_STREAM("Sot computation not finished yet.");
           }

src/roscontrol-sot-controller.hh

@@ -279,7 +279,9 @@ class RCSotController : public lci::ControllerBase, SotLoaderBasic {
   void localStandbyPositionControlMode();
 
   ///@}
-  /// Extract control values to send to the simulator.
+  /// Extract control values to send to the motors.
+  /// \param controlValues map string to vector of double. The function will fill in
+  ///        controlValues["control"]
   void readControl(std::map<std::string, dgs::ControlValues> &controlValues);
 
   /// Map of sensor readings
@@ -298,7 +300,8 @@ class RCSotController : public lci::ControllerBase, SotLoaderBasic {
   std::vector<double> command_;
 
   /// One iteration: read sensor, compute the control law, apply control.
-  void one_iteration();
+  /// \param period time since last call. Useful when realtime is not enforced correctly.
+  void one_iteration(const ros::Duration &period);
 
   /// Read URDF model from /robot_description parameter.
   bool readUrdf(ros::NodeHandle &robot_nh);

tests/sot-test-controller.cpp

@@ -89,10 +89,10 @@ void SoTTestController::cleanupSetSensors(
 }
 
 void SoTTestController::getControl(
-    map<string, dgsot::ControlValues> &controlOut) {
+    map<string, dgsot::ControlValues> &controlOut, const double& period) {
   try {
     sotDEBUG(25) << __FILE__ << __FUNCTION__ << "(#" << __LINE__ << ")" << endl;
-    device_->getControl(controlOut);
+    device_->getControl(controlOut, period);
     sotDEBUG(25) << __FILE__ << __FUNCTION__ << "(#" << __LINE__ << ")" << endl;
   } catch (dynamicgraph::sot::ExceptionAbstract &err) {
     std::cout << __FILE__ << " " << __FUNCTION__ << " (" << __LINE__ << ") "

tests/sot-test-controller.hh

@@ -38,7 +38,8 @@ class SoTTestController : public dgsot::AbstractSotExternalInterface {
 
   void cleanupSetSensors(std::map<std::string, dgsot::SensorValues> &sensorsIn);
 
-  void getControl(std::map<std::string, dgsot::ControlValues> &anglesOut);
+  void getControl(std::map<std::string, dgsot::ControlValues> &anglesOut,
+                  const double& period);
 
   void setNoIntegration(void);
   void setSecondOrderIntegration(void);

tests/sot-test-device.cpp

@@ -296,14 +296,15 @@ void SoTTestDevice::cleanupSetSensors(
   setSensors(SensorsIn);
 }
 
-void SoTTestDevice::getControl(map<string, dgsot::ControlValues> &controlOut) {
+void SoTTestDevice::getControl(map<string, dgsot::ControlValues> &controlOut,
+                               const double& period) {
   ODEBUG5FULL("start");
   sotDEBUGIN(25);
   vector<double> anglesOut;
   anglesOut.resize(state_.size());
 
   // Integrate control
-  increment(timestep_);
+  increment(period);
   sotDEBUG(25) << "state = " << state_ << std::endl;
   sotDEBUG(25) << "diff  = "
                << ((previousState_.size() == state_.size())

tests/sot-test-device.hh

@@ -41,7 +41,8 @@ class SoTTestDevice : public dgsot::Device {
 
   void cleanupSetSensors(std::map<std::string, dgsot::SensorValues> &sensorsIn);
 
-  void getControl(std::map<std::string, dgsot::ControlValues> &anglesOut);
+  void getControl(std::map<std::string, dgsot::ControlValues> &anglesOut,
+                  const double& period);
 
   void timeStep(double ts) { timestep_ = ts; }