Merge branch 'spnav'

roboticslab-uc3m · Jun 8, 2024 · a54f067 · a54f067
2 parents f11c1c5 + fbf5bb5
commit a54f067
Show file tree

Hide file tree

Showing 5 changed files with 54 additions and 93 deletions.
diff --git a/libraries/YarpPlugins/SpaceNavigator/CMakeLists.txt b/libraries/YarpPlugins/SpaceNavigator/CMakeLists.txt
@@ -15,8 +15,7 @@ yarp_prepare_plugin(SpaceNavigator
 
 if(NOT SKIP_SpaceNavigator)
 
-    yarp_add_plugin(SpaceNavigator SpaceNavigator.cpp
-                                   SpaceNavigator.hpp
+    yarp_add_plugin(SpaceNavigator SpaceNavigator.hpp
                                    DeviceDriverImpl.cpp
                                    IAnalogSensorImpl.cpp)
 

diff --git a/libraries/YarpPlugins/SpaceNavigator/DeviceDriverImpl.cpp b/libraries/YarpPlugins/SpaceNavigator/DeviceDriverImpl.cpp
@@ -21,6 +21,12 @@ bool SpaceNavigator::open(yarp::os::Searchable & config)
 {
     deadband = config.check("deadband", yarp::os::Value(DEFAULT_DEADBAND), "deadband [0,1]").asFloat64();
 
+    if (deadband < 0.0 || deadband > 1.0)
+    {
+        yCError(SPNAV) << "Invalid deadband value (must be in [0,1]):" << deadband;
+        return false;
+    }
+
     if (spnav_open() == -1)
     {
         yCError(SPNAV) << "Failed to connect to the space navigator daemon";

diff --git a/libraries/YarpPlugins/SpaceNavigator/IAnalogSensorImpl.cpp b/libraries/YarpPlugins/SpaceNavigator/IAnalogSensorImpl.cpp
@@ -2,6 +2,10 @@
 
 #include "SpaceNavigator.hpp"
 
+#include <cmath> // std::abs, std::copysign
+
+#include <algorithm> // std::clamp
+
 using namespace roboticslab;
 
 constexpr auto NUM_CHANNELS = 8;
@@ -13,6 +17,27 @@ constexpr auto FULL_SCALE_PITCH = 405.0;
 constexpr auto FULL_SCALE_YAW = 435.0;
 constexpr auto MAX_NO_DATA_ITERATIONS = 10;
 
+constexpr auto RANGE = 1.0;
+
+// -----------------------------------------------------------------------------
+
+namespace
+{
+    double normalize(double value, double deadband)
+    {
+        if (std::abs(value) <= deadband)
+        {
+            return 0.0;
+        }
+        else
+        {
+            const double slope = RANGE / (RANGE - deadband);
+            const double clamped = std::clamp(value, -RANGE, RANGE);
+            return slope * std::copysign(std::abs(clamped) - deadband, value);
+        }
+    }
+}
+
 // -----------------------------------------------------------------------------
 
 int SpaceNavigator::read(yarp::sig::Vector &out)
@@ -52,12 +77,12 @@ int SpaceNavigator::read(yarp::sig::Vector &out)
 
     out.resize(NUM_CHANNELS);
 
-    out[0] = enforceDeadband(enforceRange(dx / FULL_SCALE_X));
-    out[1] = enforceDeadband(enforceRange(dy / FULL_SCALE_Y));
-    out[2] = enforceDeadband(enforceRange(dz / FULL_SCALE_Z));
-    out[3] = enforceDeadband(enforceRange(droll / FULL_SCALE_ROLL));
-    out[4] = enforceDeadband(enforceRange(dpitch / FULL_SCALE_PITCH));
-    out[5] = enforceDeadband(enforceRange(dyaw / FULL_SCALE_YAW));
+    out[0] = normalize(dx / FULL_SCALE_X, deadband);
+    out[1] = normalize(dy / FULL_SCALE_Y, deadband);
+    out[2] = normalize(dz / FULL_SCALE_Z, deadband);
+    out[3] = normalize(droll / FULL_SCALE_ROLL, deadband);
+    out[4] = normalize(dpitch / FULL_SCALE_PITCH, deadband);
+    out[5] = normalize(dyaw / FULL_SCALE_YAW, deadband);
 
     out[6] = button1;
     out[7] = button2;

diff --git a/libraries/YarpPlugins/SpaceNavigator/SpaceNavigator.cpp b/libraries/YarpPlugins/SpaceNavigator/SpaceNavigator.cpp
diff --git a/libraries/YarpPlugins/SpaceNavigator/SpaceNavigator.hpp b/libraries/YarpPlugins/SpaceNavigator/SpaceNavigator.hpp
@@ -35,11 +35,11 @@ class SpaceNavigator : public yarp::dev::DeviceDriver,
 {
 public:
 
-    SpaceNavigator();
-    ~SpaceNavigator() override;
+    ~SpaceNavigator() override
+    { close(); }
 
     //  --------- DeviceDriver Declarations. Implementation in DeviceDriverImpl.cpp ---------
-    bool open(yarp::os::Searchable& config) override;
+    bool open(yarp::os::Searchable & config) override;
     bool close() override;
 
     //  --------- IAnalogSensor Declarations. Implementation in IAnalogSensorImpl.cpp ---------
@@ -48,7 +48,7 @@ class SpaceNavigator : public yarp::dev::DeviceDriver,
      * @param out a vector containing the sensor's last readings.
      * @return AS_OK or return code. AS_TIMEOUT if the sensor timed-out.
      */
-    int read(yarp::sig::Vector &out) override;
+    int read(yarp::sig::Vector & out) override;
 
     /**
      * Check the state value of a given channel.
@@ -74,7 +74,7 @@ class SpaceNavigator : public yarp::dev::DeviceDriver,
      * @param value a vector of calibration values.
      * @return status.
      */
-    int calibrateSensor(const yarp::sig::Vector& value) override;
+    int calibrateSensor(const yarp::sig::Vector & value) override;
 
     /**
      * Calibrates one single channel.
@@ -91,21 +91,21 @@ class SpaceNavigator : public yarp::dev::DeviceDriver,
      */
     int calibrateChannel(int ch, double value) override;
 
-protected:
+private:
 
-    //! @brief Enforce that a value is between -1 and 1
-    double enforceRange(double in);
+    double dx {0.0};
+    double dy {0.0};
+    double dz {0.0};
 
-    //! @brief Enforce the deadband (setting values within deadband to zero)
-    double enforceDeadband(double in);
+    double droll {0.0};
+    double dpitch {0.0};
+    double dyaw {0.0};
 
-private:
+    int button1 {0};
+    int button2 {0};
 
-    double dx, dy, dz;
-    double droll, dpitch, dyaw;
-    int button1, button2;
-    unsigned int noDataCounter;
-    double deadband;
+    unsigned int noDataCounter {0};
+    double deadband {0.0};
 };
 
 } // namespace roboticslab