[pre-commit.ci] auto fixes from pre-commit.com hooks

for more information, see https://pre-commit.ci

Author: pre-commit-ci[bot]

src/roscontrol-sot-controller.cpp

@@ -70,12 +70,16 @@ void ControlPDMotorControlData::read_from_xmlrpc_value(
 }
 
 RCSotController::RCSotController()
-    : // Store 32 DoFs for 5 minutes (1 Khz: 5*60*1000)
-      // -> 124 Mo of data.
-      type_name_("RCSotController"), simulation_mode_(false),
-      subSampling_(0), step_(0), verbosity_level_(0), io_service_(),
-      io_work_(io_service_), sotComputing_(false)
-{
+    :  // Store 32 DoFs for 5 minutes (1 Khz: 5*60*1000)
+       // -> 124 Mo of data.
+      type_name_("RCSotController"),
+      simulation_mode_(false),
+      subSampling_(0),
+      step_(0),
+      verbosity_level_(0),
+      io_service_(),
+      io_work_(io_service_),
+      sotComputing_(false) {
   RESETDEBUG4();
 }
 
@@ -935,7 +939,7 @@ void RCSotController::readControl(
       DataOneIter_.controls[i] = command_[i];
     }
 
-  } else{
+  } else {
     ROS_INFO_STREAM("no control.");
     localStandbyEffortControlMode(ros::Duration(dtRos_));
     localStandbyVelocityControlMode(ros::Duration(dtRos_));
@@ -973,8 +977,8 @@ void RCSotController::one_iteration() {
   }
 
   // Wait until last subsampling step to write result in controlValues_
-  while(step_ != subSampling_ - 1){
-    ros::Duration(.01*dtRos_).sleep();
+  while (step_ != subSampling_ - 1) {
+    ros::Duration(.01 * dtRos_).sleep();
   }
   // mutex
   mutex_.lock();
@@ -1078,79 +1082,79 @@ void RCSotController::localStandbyPositionControlMode() {
   first_time = false;
 }
 
-void RCSotController::computeSubSampling(const ros::Duration &period)
-{
-  if (subSampling_ != 0) return; // already computed
+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){
+  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.";
+       << 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;
+  step_ = subSampling_ - 1;
   ROS_INFO_STREAM("Subsampling SoT graph computation by ratio "
-		  << subSampling_);
-  if (subSampling_ != 1){
+                  << 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_));
+    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_;
+    ++step_;
     if (step_ == subSampling_) step_ = 0;
     try {
       // If subsampling is equal to 1, i.e. no subsampling, evaluate
       // control graph in this thread
-      if (subSampling_ == 1){
-	one_iteration();
+      if (subSampling_ == 1) {
+        one_iteration();
       } 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();
-	}
+        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) {

src/roscontrol-sot-controller.hh

@@ -5,12 +5,11 @@
 #ifndef RC_SOT_CONTROLLER_H
 #define RC_SOT_CONTROLLER_H
 
-#include <map>
-#include <string>
 #include <atomic>
-
-#include <boost/thread/thread.hpp>
 #include <boost/asio/io_service.hpp>
+#include <boost/thread/thread.hpp>
+#include <map>
+#include <string>
 
 #pragma GCC diagnostic push
 #pragma GCC system_header
@@ -78,8 +77,7 @@ class RCSotController : public lci::ControllerBase, SotLoaderBasic {
   /// Data log.
   rc_sot_system::DataToLog DataOneIter_;
 
-private:
-
+ private:
   void computeSubSampling(const ros::Duration &period);
   /// @{ \name Ros-control related fields
 
@@ -150,7 +148,7 @@ private:
   /// 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_;
+  std::atomic<std::size_t> step_;
   /// double roscontrol sampling period
   double dtRos_;
   /// \brief Verbosity level for ROS messages during initRequest/initialization
@@ -172,7 +170,8 @@ private:
   // Flag informing whether the graph computation is running to avoid starting
   // a computation if the previous one has no finished.
   bool sotComputing_;
-public:
+
+ public:
   RCSotController();
 
   virtual ~RCSotController();