Merge pull request #35 from florent-lamiraux/devel

[RSSotController] Better handle subsampling.

Author: nim65s

src/roscontrol-sot-controller.cpp

@@ -74,13 +74,18 @@ RCSotController::RCSotController()
        // -> 124 Mo of data.
       type_name_("RCSotController"),
       simulation_mode_(false),
-      accumulated_time_(0.0),
-      jitter_(0.0),
-      verbosity_level_(0) {
+      subSampling_(0),
+      step_(0),
+      verbosity_level_(0),
+      io_service_(),
+      io_work_(io_service_),
+      sotComputing_(false) {
   RESETDEBUG4();
 }
 
 RCSotController::~RCSotController() {
+  io_service_.stop();
+  thread_pool_.join_all();
   std::string afilename("/tmp/sot.log");
 
   RcSotLog_.record(DataOneIter_);
@@ -617,12 +622,6 @@ bool RCSotController::readParamsControlMode(ros::NodeHandle &robot_nh) {
 }
 
 bool RCSotController::readParamsdt(ros::NodeHandle &robot_nh) {
-  /// Reading the jitter is optional but it is a very good idea.
-  if (robot_nh.hasParam("/sot_controller/jitter")) {
-    robot_nh.getParam("/sot_controller/jitter", jitter_);
-    if (verbosity_level_ > 0) ROS_INFO_STREAM("jitter: " << jitter_);
-  }
-
   /// Read /sot_controller/dt to know what is the control period
   if (robot_nh.hasParam("/sot_controller/dt")) {
     robot_nh.getParam("/sot_controller/dt", dt_);
@@ -940,13 +939,19 @@ void RCSotController::readControl(
       DataOneIter_.controls[i] = command_[i];
     }
 
-  } else
+  } else {
     ROS_INFO_STREAM("no control.");
+    localStandbyEffortControlMode(ros::Duration(dtRos_));
+    localStandbyVelocityControlMode(ros::Duration(dtRos_));
+    localStandbyPositionControlMode();
+  }
 }
 
 void RCSotController::one_iteration() {
+  sotComputing_ = true;
   // Chrono start
   RcSotLog_.start_it();
+
   /// Update the sensors.
   fillSensors();
 
@@ -961,21 +966,24 @@ void RCSotController::one_iteration() {
 
     throw e;
   }
-
   try {
-    sotController_->getControl(controlValues_);
+    sotController_->getControl(controlValues_copy_);
   } catch (std::exception &e) {
     std::cerr << "Failure happened during one_iteration(): "
               << "when calling getControl " << std::endl;
     std::cerr << __FILE__ << " " << __LINE__ << std::endl
               << e.what() << std::endl;
-    ;
     throw e;
   }
 
-  /// Read the control values
-  readControl(controlValues_);
-
+  // Wait until last subsampling step to write result in controlValues_
+  while (step_ != subSampling_ - 1) {
+    ros::Duration(.01 * dtRos_).sleep();
+  }
+  // mutex
+  mutex_.lock();
+  controlValues_ = controlValues_copy_;
+  mutex_.unlock();
   // Chrono stop.
   double it_duration = RcSotLog_.stop_it();
   if (it_duration > dt_) {
@@ -985,6 +993,7 @@ void RCSotController::one_iteration() {
 
   /// Store everything in Log.
   RcSotLog_.record(DataOneIter_);
+  sotComputing_ = false;
 }
 
 void RCSotController::localStandbyEffortControlMode(
@@ -1073,16 +1082,81 @@ void RCSotController::localStandbyPositionControlMode() {
   first_time = false;
 }
 
+void RCSotController::computeSubSampling(const ros::Duration &period) {
+  if (subSampling_ != 0) return;  // already computed
+  dtRos_ = period.toSec();
+  double ratio = dt_ / dtRos_;
+  if (fabs(ratio - std::round(ratio)) > 1e-8) {
+    std::ostringstream os;
+    os << "SoT period (" << dt_ << ") is not a multiple of roscontrol period ("
+       << dtRos_
+       << "). Modify rosparam /sot_controller/dt to a "
+          "multiple of roscontrol period.";
+    throw std::runtime_error(os.str().c_str());
+  }
+  subSampling_ = static_cast<std::size_t>(std::round(ratio));
+  // First time method update is called, step_ will turn to 1.
+  step_ = subSampling_ - 1;
+  ROS_INFO_STREAM("Subsampling SoT graph computation by ratio "
+                  << subSampling_);
+  if (subSampling_ != 1) {
+    // create thread in this method instead of in constructor to inherit the
+    // same priority level.
+    // If subsampling is equal to one, the control graph is evaluated in the
+    // same thread.
+    thread_pool_.create_thread(
+        boost::bind(&boost::asio::io_service::run, &io_service_));
+  }
+}
 void RCSotController::update(const ros::Time &, const ros::Duration &period) {
   // Do not send any control if the dynamic graph is not started
   if (!isDynamicGraphStopped()) {
+    // Increment step at beginning of this method since the end time may
+    // vary a lot.
+    ++step_;
+    if (step_ == subSampling_) step_ = 0;
     try {
-      double periodInSec = period.toSec();
-      if (periodInSec + accumulated_time_ > dt_ - jitter_) {
+      // If subsampling is equal to 1, i.e. no subsampling, evaluate
+      // control graph in this thread
+      if (subSampling_ == 1) {
         one_iteration();
-        accumulated_time_ = 0.0;
-      } else
-        accumulated_time_ += periodInSec;
+      } 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));
+          } else {
+            ROS_INFO_STREAM("Sot computation not finished yet.");
+          }
+          // protected read control
+          mutex_.lock();
+          /// Read the control values
+          readControl(controlValues_);
+          mutex_.unlock();
+        } else {
+          // protected read control
+          mutex_.lock();
+          // If the robot is controlled in position, update the reference
+          // posture with the latest velocity.
+          // Note that the entry "velocity" only exists if the robot is
+          // controlled in position.
+          if (controlValues_.find("velocity") != controlValues_.end()) {
+            std::vector<double> control = controlValues_["control"].getValues();
+            const std::vector<double> &velocity =
+                controlValues_["velocity"].getValues();
+            for (std::size_t i = 0; i < control.size(); ++i) {
+              control[i] += dtRos_ * velocity[i];
+            }
+            controlValues_["control"].setValues(control);
+          }
+          /// Read the control values
+          readControl(controlValues_);
+          mutex_.unlock();
+        }
+      }
+
     } catch (std::exception const &exc) {
       ROS_ERROR_STREAM("Failure happened during one_iteration evaluation: "
                        << exc.what()
@@ -1097,6 +1171,7 @@ void RCSotController::update(const ros::Time &, const ros::Duration &period) {
       throw;
     }
   } else {
+    computeSubSampling(period);
     /// Update the sensors.
     fillSensors();
     try {

src/roscontrol-sot-controller.hh

@@ -5,6 +5,9 @@
 #ifndef RC_SOT_CONTROLLER_H
 #define RC_SOT_CONTROLLER_H
 
+#include <atomic>
+#include <boost/asio/io_service.hpp>
+#include <boost/thread/thread.hpp>
 #include <map>
 #include <string>
 
@@ -75,6 +78,7 @@ class RCSotController : public lci::ControllerBase, SotLoaderBasic {
   rc_sot_system::DataToLog DataOneIter_;
 
  private:
+  void computeSubSampling(const ros::Duration &period);
   /// @{ \name Ros-control related fields
 
   /// \brief Vector of joint handles.
@@ -141,12 +145,12 @@ class RCSotController : public lci::ControllerBase, SotLoaderBasic {
   /// * torques
   std::map<std::string, std::string> mapFromRCToSotDevice_;
 
-  /// To be able to subsample control period.
-  double accumulated_time_;
-
-  /// Jitter for the subsampling.
-  double jitter_;
-
+  /// ratio between sot control period and roscontrol control period
+  std::size_t subSampling_;
+  /// iteration index of the subsampling. Ranges from 0 to subSampling_-1
+  std::atomic<std::size_t> step_;
+  /// double roscontrol sampling period
+  double dtRos_;
   /// \brief Verbosity level for ROS messages during initRequest/initialization
   /// phase. 0: no messages or error 1: info 2: debug
   int verbosity_level_;
@@ -157,6 +161,16 @@ class RCSotController : public lci::ControllerBase, SotLoaderBasic {
   /// Profile log
   rc_sot_system::ProfileLog profileLog_;
 
+  /// thread pool
+  boost::asio::io_service io_service_;
+  boost::asio::io_service::work io_work_;
+  boost::thread_group thread_pool_;
+  // mutex to protect access to controlValues_
+  std::mutex mutex_;
+  // Flag informing whether the graph computation is running to avoid starting
+  // a computation if the previous one has no finished.
+  bool sotComputing_;
+
  public:
   RCSotController();
 
@@ -274,6 +288,7 @@ class RCSotController : public lci::ControllerBase, SotLoaderBasic {
 
   /// Map of control values
   std::map<std::string, dgs::ControlValues> controlValues_;
+  std::map<std::string, dgs::ControlValues> controlValues_copy_;
 
   /// Control period
   double dt_;