Define members in the class header, embrace std::bind

programs/keyboardController/KeyboardController.cpp

@@ -35,13 +35,13 @@ namespace
 {
     struct termios ots;
 
-    bool readKey(char * key)
+    inline bool readKey(char * key)
     {
         return read(STDIN_FILENO, key, 1) > 0;
     }
 
     // https://stackoverflow.com/a/23397700
-    std::ostream& operator<<(std::ostream& out, const std::vector<double>& v)
+    std::ostream & operator<<(std::ostream & out, const std::vector<double> & v)
     {
         if (!v.empty())
         {
@@ -53,13 +53,13 @@ namespace
         return out;
     }
 
-    std::vector<double> roundZeroes(const std::vector<double>& v_in)
+    std::vector<double> roundZeroes(const std::vector<double> & v_in)
     {
-        static const double precision = 1e-6;
+        static constexpr double precision = 1e-6;
 
-        std::vector<double> v_out(v_in);
+        auto v_out(v_in);
 
-        for (std::vector<double>::iterator it = v_out.begin(); it != v_out.end(); ++it)
+        for (auto it = v_out.begin(); it != v_out.end(); ++it)
         {
             if (std::abs(*it) < precision)
             {
@@ -71,7 +71,7 @@ namespace
     }
 
     // reset the TTY configurations that was changed in the ttyset function (UNIX)
-    void ttyreset(int signal)
+    inline void ttyreset(int signal)
     {
         tcsetattr(STDIN_FILENO, TCSANOW, &ots);
         tcsetattr(STDOUT_FILENO, TCSANOW, &ots);
@@ -154,9 +154,7 @@ bool KeyboardController::configure(yarp::os::ResourceFinder & rf)
             return false;
         }
 
-        iEncoders->getAxes(&axes);
-
-        if (axes > MAX_JOINTS)
+        if (!iEncoders->getAxes(&axes) || axes > MAX_JOINTS)
         {
             yCError(KC, "Number of joints (%d) exceeds supported limit (%d)", axes, MAX_JOINTS);
             return false;
@@ -247,12 +245,8 @@ bool KeyboardController::configure(yarp::os::ResourceFinder & rf)
         }
     }
 
-    currentActuatorCommand = VOCAB_CC_ACTUATOR_NONE;
-
     issueStop(); // just in case
-
     ttyset();
-
     printHelp();
 
     return true;

programs/keyboardController/KeyboardController.hpp

@@ -71,26 +71,26 @@ class KeyboardController : public yarp::os::RFModule
 
     void printHelp();
 
-    int axes;
-    int currentActuatorCommand;
+    int axes {0};
+    int currentActuatorCommand {VOCAB_CC_ACTUATOR_NONE};
 
-    ICartesianSolver::reference_frame cartFrame;
+    ICartesianSolver::reference_frame cartFrame {ICartesianSolver::BASE_FRAME};
     std::string angleRepr;
-    KinRepresentation::orientation_system orient;
-    control_modes controlMode;
+    KinRepresentation::orientation_system orient {KinRepresentation::orientation_system::AXIS_ANGLE};
+    control_modes controlMode {NOT_CONTROLLING};
 
-    bool usingThread;
-    LinearTrajectoryThread * linTrajThread;
+    bool usingThread {false};
+    LinearTrajectoryThread * linTrajThread {nullptr};
 
     yarp::dev::PolyDriver controlBoardDevice;
     yarp::dev::PolyDriver cartesianControlDevice;
 
-    yarp::dev::IEncoders * iEncoders;
-    yarp::dev::IControlMode * iControlMode;
-    yarp::dev::IControlLimits * iControlLimits;
-    yarp::dev::IVelocityControl * iVelocityControl;
+    yarp::dev::IEncoders * iEncoders {nullptr};
+    yarp::dev::IControlMode * iControlMode {nullptr};
+    yarp::dev::IControlLimits * iControlLimits {nullptr};
+    yarp::dev::IVelocityControl * iVelocityControl {nullptr};
 
-    roboticslab::ICartesianControl * iCartesianControl;
+    roboticslab::ICartesianControl * iCartesianControl {nullptr};
 
     std::vector<double> maxVelocityLimits;
     std::vector<double> currentJointVels;

programs/keyboardController/LinearTrajectoryThread.cpp

@@ -2,8 +2,8 @@
 
 #include "LinearTrajectoryThread.hpp"
 
-#include <algorithm>
-#include <functional>
+#include <algorithm> // std::transform
+#include <functional> // std::bind
 
 #include <yarp/os/LogStream.h>
 #include <yarp/os/Time.h>
@@ -21,11 +21,7 @@ using namespace roboticslab;
 LinearTrajectoryThread::LinearTrajectoryThread(int _period, ICartesianControl * _iCartesianControl)
     : yarp::os::PeriodicThread(_period * 0.001, yarp::os::PeriodicThreadClock::Absolute),
       period(_period),
-      iCartesianControl(_iCartesianControl),
-      trajectory(nullptr),
-      startTime(0.0),
-      usingStreamingCommandConfig(false),
-      usingTcpFrame(false)
+      iCartesianControl(_iCartesianControl)
 {}
 
 LinearTrajectoryThread::~LinearTrajectoryThread()
@@ -59,13 +55,12 @@ bool LinearTrajectoryThread::configure(const std::vector<double> & vels)
 
     if (usingTcpFrame)
     {
+        using namespace std::placeholders;
         std::vector<double> deltaX(vels.size());
-        std::transform(vels.begin(), vels.end(), deltaX.begin(), std::bind1st(std::multiplies<double>(), period / 1000.0));
+        std::transform(vels.begin(), vels.end(), deltaX.begin(), std::bind(std::multiplies<double>(), period / 1000.0, _1));
 
-        mtx.lock();
+        std::lock_guard lock(mtx);
         this->deltaX = deltaX;
-        mtx.unlock();
-
         return true;
     }
 
@@ -77,10 +72,10 @@ bool LinearTrajectoryThread::configure(const std::vector<double> & vels)
         return false;
     }
 
-    KDL::Frame H = KdlVectorConverter::vectorToFrame(x);
-    KDL::Twist tw = KdlVectorConverter::vectorToTwist(vels);
+    auto H = KdlVectorConverter::vectorToFrame(x);
+    auto tw = KdlVectorConverter::vectorToTwist(vels);
 
-    mtx.lock();
+    std::lock_guard lock(mtx);
 
     if (trajectory)
     {
@@ -89,14 +84,12 @@ bool LinearTrajectoryThread::configure(const std::vector<double> & vels)
         trajectory = nullptr;
     }
 
-    KDL::Path * path = new KDL::Path_Line(H, tw, new KDL::RotationalInterpolation_SingleAxis(), 1.0);
-    KDL::VelocityProfile * profile = new KDL::VelocityProfile_Rectangular(10.0);
+    auto * path = new KDL::Path_Line(H, tw, new KDL::RotationalInterpolation_SingleAxis(), 1.0);
+    auto * profile = new KDL::VelocityProfile_Rectangular(10.0);
     profile->SetProfileDuration(0.0, 10.0, 10.0 / path->PathLength());
     trajectory = new KDL::Trajectory_Segment(path, profile);
     startTime = yarp::os::Time::now();
 
-    mtx.unlock();
-
     return true;
 }
 
@@ -115,10 +108,10 @@ void LinearTrajectoryThread::run()
     else
     {
         mtx.lock();
-        KDL::Frame H = trajectory->Pos(yarp::os::Time::now() - startTime);
+        auto H = trajectory->Pos(yarp::os::Time::now() - startTime);
         mtx.unlock();
 
-        std::vector<double> position = KdlVectorConverter::frameToVector(H);
+        auto position = KdlVectorConverter::frameToVector(H);
         iCartesianControl->pose(position);
     }
 }

programs/keyboardController/LinearTrajectoryThread.hpp

@@ -24,7 +24,8 @@ class LinearTrajectoryThread : public yarp::os::PeriodicThread
 {
 public:
     LinearTrajectoryThread(int period, ICartesianControl * iCartesianControl);
-    ~LinearTrajectoryThread();
+    ~LinearTrajectoryThread() override;
+
     bool checkStreamingConfig();
     bool configure(const std::vector<double> & vels);
     void useTcpFrame(bool enableTcpFrame) { usingTcpFrame = enableTcpFrame; }
@@ -33,12 +34,12 @@ class LinearTrajectoryThread : public yarp::os::PeriodicThread
     void run() override;
 
 private:
-    double period;
-    ICartesianControl * iCartesianControl;
-    KDL::Trajectory * trajectory;
-    double startTime;
-    bool usingStreamingCommandConfig;
-    bool usingTcpFrame;
+    double period {0.0};
+    ICartesianControl * iCartesianControl {nullptr};
+    KDL::Trajectory * trajectory {nullptr};
+    double startTime {0.0};
+    bool usingStreamingCommandConfig {false};
+    bool usingTcpFrame {false};
     std::vector<double> deltaX;
     mutable std::mutex mtx;
 };