[iKinGazeCtrl] clean up + fixed handling of torso velocity

see robotology/icub-tech-support#530

src/modules/iKinGazeCtrl/include/iCub/controller.h

@@ -127,6 +127,7 @@ class Controller : public GazeComponent, public RateThread
     Controller(PolyDriver *_drvTorso, PolyDriver *_drvHead, ExchangeData *_commData,
                const double _neckTime, const double _eyesTime,
                const double _min_abs_vel, const unsigned int _period);
+    virtual ~Controller();
 
     void   findMinimumAllowedVergence();
     void   minAllowedVergenceChanged();
@@ -136,9 +137,9 @@ class Controller : public GazeComponent, public RateThread
     void   stopControl();    
     void   printIter(Vector &xd, Vector &fp, Vector &qd, Vector &q, Vector &v, double printTime);
     bool   threadInit();
+    void   threadRelease();
     void   afterStart(bool s);
     void   run();
-    void   threadRelease();
     void   suspend();
     void   resume();
     double getTneck() const;

src/modules/iKinGazeCtrl/include/iCub/localizer.h

@@ -74,6 +74,7 @@ class Localizer : public GazeComponent, public RateThread
 
 public:
     Localizer(ExchangeData *_commData, const unsigned int _period);
+    virtual ~Localizer();
 
     double getDistFromVergence(const double ver);
     void   getPidOptions(Bottle &options);
@@ -89,9 +90,9 @@ class Localizer : public GazeComponent, public RateThread
     bool   getIntrinsicsMatrix(const string &type, Matrix &M, int &w, int &h);
     bool   setIntrinsicsMatrix(const string &type, const Matrix &M, const int w, const int h);
     bool   threadInit();
+    void   threadRelease();
     void   afterStart(bool s);
     void   run();
-    void   threadRelease();
 };
 
 

src/modules/iKinGazeCtrl/include/iCub/solver.h

@@ -27,6 +27,7 @@
 #include <yarp/math/Math.h>
 
 #include <iCub/ctrl/pids.h>
+#include <iCub/ctrl/adaptWinPolyEstimator.h>
 #include <iCub/gazeNlp.h>
 #include <iCub/utils.h>
 #include <iCub/localizer.h>
@@ -37,7 +38,7 @@
 #define SACCADES_INHIBITION_PERIOD          0.2     // [s]
 #define SACCADES_ACTIVATION_ANGLE           10.0    // [deg]
 #define NECKSOLVER_ACTIVATIONDELAY          0.25    // [s]
-#define NECKSOLVER_ACTIVATIONANGLE_JOINTS   1.0     // [deg]
+#define NECKSOLVER_ACTIVATIONANGLE_JOINTS   0.5     // [deg/s]
 #define NECKSOLVER_ACTIVATIONANGLE          2.5     // [deg]
 #define NECKSOLVER_RESTORINGANGLE           5.0     // [deg]
 
@@ -92,6 +93,7 @@ class EyePinvRefGen : public GazeComponent, public RateThread
 public:
     EyePinvRefGen(PolyDriver *_drvTorso, PolyDriver *_drvHead, ExchangeData *_commData,
                   Controller *_ctrl, const Vector &_counterRotGain, const unsigned int _period);
+    virtual ~EyePinvRefGen();
 
     void   enable()  { genOn=true;  }
     void   disable() { genOn=false; }
@@ -102,9 +104,9 @@ class EyePinvRefGen : public GazeComponent, public RateThread
     bool   clearEyes();
     void   manageBindEyes(const double ver);
     bool   threadInit();
+    void   threadRelease();
     void   afterStart(bool s);
     void   run();
-    void   threadRelease();
     void   suspend();
     void   resume();
 };
@@ -137,7 +139,8 @@ class Solver : public GazeComponent, public RateThread
     Vector fbHead;
     Vector neckPos;
     Vector gazePos;
-    Vector fbTorsoOld;
+
+    AWLinEstimator *torsoVel;
 
     double neckPitchMin;
     double neckPitchMax;
@@ -153,6 +156,7 @@ class Solver : public GazeComponent, public RateThread
     Solver(PolyDriver *_drvTorso, PolyDriver *_drvHead, ExchangeData *_commData,
            EyePinvRefGen *_eyesRefGen, Localizer *_loc, Controller *_ctrl,
            const unsigned int _period);
+    virtual ~Solver();
 
     // Returns a measure of neck angle required to reach the target
     double neckTargetRotAngle(const Vector &xd);
@@ -168,9 +172,9 @@ class Solver : public GazeComponent, public RateThread
     double getNeckAngleUserTolerance() const;
     void   setNeckAngleUserTolerance(const double angle);    
     bool   threadInit();
+    void   threadRelease();
     void   afterStart(bool s);
     void   run();
-    void   threadRelease();
     void   suspend();
     void   resume();
 };

src/modules/iKinGazeCtrl/include/iCub/utils.h

@@ -52,7 +52,7 @@ class xdPort : public BufferedPort<Bottle>,
                public Thread
 {
 protected:
-    void  *slv;
+    void *slv;
 
     Mutex  mutex_0;
     Mutex  mutex_1;

src/modules/iKinGazeCtrl/src/controller.cpp

@@ -154,6 +154,21 @@ Controller::Controller(PolyDriver *_drvTorso, PolyDriver *_drvHead, ExchangeData
 }
 
 
+/************************************************************************/
+Controller::~Controller()
+{
+    delete neck;
+    delete eyeL;
+    delete eyeR;
+    delete imu;
+    delete mjCtrlNeck;
+    delete mjCtrlEyes;
+    delete IntState;
+    delete IntPlan;
+    delete IntStabilizer;
+}
+
+
 /************************************************************************/
 void Controller::findMinimumAllowedVergence()
 {
@@ -360,6 +375,29 @@ bool Controller::threadInit()
 }
 
 
+/************************************************************************/
+void Controller::threadRelease()
+{
+    stopLimb();
+    notifyEvent("closing");
+
+    port_x.interrupt();
+    port_x.close();
+
+    port_q.interrupt();
+    port_q.close();
+
+    port_event.interrupt();
+    port_event.close();
+
+    if (commData->debugInfoEnabled)
+    {
+        port_debug.interrupt();
+        port_debug.close();
+    }
+}
+
+
 /************************************************************************/
 void Controller::afterStart(bool s)
 {
@@ -952,39 +990,6 @@ void Controller::run()
 }
 
 
-/************************************************************************/
-void Controller::threadRelease()
-{
-    stopLimb();
-    notifyEvent("closing");
-
-    port_x.interrupt();
-    port_x.close();
-
-    port_q.interrupt();
-    port_q.close();
-
-    port_event.interrupt();
-    port_event.close();
-
-    if (commData->debugInfoEnabled)
-    {
-        port_debug.interrupt();
-        port_debug.close();
-    }
-
-    delete neck;
-    delete eyeL;
-    delete eyeR;
-    delete imu;
-    delete mjCtrlNeck;
-    delete mjCtrlEyes;
-    delete IntState;
-    delete IntPlan;
-    delete IntStabilizer;
-}
-
-
 /************************************************************************/
 void Controller::suspend()
 {

src/modules/iKinGazeCtrl/src/localizer.cpp

@@ -122,6 +122,27 @@ Localizer::Localizer(ExchangeData *_commData, const unsigned int _period) :
 }
 
 
+/************************************************************************/
+Localizer::~Localizer()
+{
+    delete eyeL;
+    delete eyeR;
+    delete pid;
+
+    if (PrjL!=NULL)
+    {
+        delete PrjL;
+        delete invPrjL;
+    }
+
+    if (PrjR!=NULL)
+    {
+        delete PrjR;
+        delete invPrjR;
+    }
+}
+
+
 /************************************************************************/
 bool Localizer::threadInit()
 { 
@@ -134,6 +155,20 @@ bool Localizer::threadInit()
     return true;
 }
 
+/************************************************************************/
+void Localizer::threadRelease()
+{
+    port_mono.interrupt();
+    port_stereo.interrupt();
+    port_anglesIn.interrupt();
+    port_anglesOut.interrupt();
+
+    port_mono.close();
+    port_stereo.close();
+    port_anglesIn.close();
+    port_anglesOut.close();
+}
+
 
 /************************************************************************/
 void Localizer::afterStart(bool s)
@@ -698,35 +733,4 @@ void Localizer::run()
 }
 
 
-/************************************************************************/
-void Localizer::threadRelease()
-{
-    port_mono.interrupt();
-    port_stereo.interrupt();
-    port_anglesIn.interrupt();
-    port_anglesOut.interrupt();
-
-    port_mono.close();
-    port_stereo.close();
-    port_anglesIn.close();
-    port_anglesOut.close();
-
-    delete eyeL;
-    delete eyeR;
-    delete pid;
-
-    if (PrjL!=NULL)
-    {
-        delete PrjL;
-        delete invPrjL;
-    }
-
-    if (PrjR!=NULL)
-    {
-        delete PrjR;
-        delete invPrjR;
-    }
-}
-
-
 

src/modules/iKinGazeCtrl/src/main.cpp

@@ -2076,13 +2076,14 @@ class GazeModule: public RFModule
         if (rpcPort.asPort().isOpen())
             rpcPort.close();
 
+        // this switch-off order does matter !!
+        delete commData.port_xd;
         delete loc;
         delete eyesRefGen;
         delete slv;
         delete ctrl;
         delete drvTorso;
         delete drvHead;
-        delete commData.port_xd;
 
         contextMap.clear();
     }