Port PR #153 (add support for onboard orientation estimation) to ROS2

phidgets_api/include/phidgets_api/spatial.hpp

@@ -43,13 +43,14 @@ class Spatial final
   public:
     PHIDGET22_NO_COPY_NO_MOVE_NO_ASSIGN(Spatial)
 
-    explicit Spatial(int32_t serial_number, int hub_port,
-                     bool is_hub_port_device,
-                     std::function<void(const double[3], const double[3],
-                                        const double[3], double)>
-                         data_handler,
-                     std::function<void()> attach_handler = nullptr,
-                     std::function<void()> detach_handler = nullptr);
+    explicit Spatial(
+        int32_t serial_number, int hub_port, bool is_hub_port_device,
+        std::function<void(const double[3], const double[3], const double[3],
+                           double)>
+            data_handler,
+        std::function<void(const double[4], double)> algorithm_data_handler,
+        std::function<void()> attach_handler = nullptr,
+        std::function<void()> detach_handler = nullptr);
 
     ~Spatial();
 
@@ -63,12 +64,27 @@ class Spatial final
                                         double cc_T3, double cc_T4,
                                         double cc_T5);
 
+    void setSpatialAlgorithm(const std::string algorithm);
+
+    void setAHRSParameters(double angularVelocityThreshold,
+                           double angularVelocityDeltaThreshold,
+                           double accelerationThreshold, double magTime,
+                           double accelTime, double biasTime);
+
+    void setAlgorithmMagnetometerGain(double magnetometer_gain);
+
+    void setHeatingEnabled(bool heating_enabled);
+
     void setDataInterval(uint32_t interval_ms) const;
 
     void zero() const;
 
     void dataHandler(const double acceleration[3], const double angular_rate[3],
                      const double magnetic_field[3], double timestamp) const;
+
+    void algorithmDataHandler(const double quaternion[4],
+                              double timestamp) const;
+
     virtual void attachHandler();
     virtual void detachHandler();
 
@@ -77,6 +93,10 @@ class Spatial final
     std::function<void(const double[3], const double[3], const double[3],
                        double)>
         data_handler_;
+
+    std::function<void(const double quaternion[4], double)>
+        algorithm_data_handler_;
+
     std::function<void()> attach_handler_;
     std::function<void()> detach_handler_;
 
@@ -86,6 +106,9 @@ class Spatial final
                             const double acceleration[3],
                             const double angular_rate[3],
                             const double magnetic_field[3], double timestamp);
+    static void AlgorithmDataHandler(PhidgetSpatialHandle input_handle,
+                                     void *ctx, const double quaternion[4],
+                                     double timestamp);
     static void AttachHandler(PhidgetHandle input_handle, void *ctx);
     static void DetachHandler(PhidgetHandle input_handle, void *ctx);
 };

phidgets_api/src/spatial.cpp

@@ -37,14 +37,16 @@
 
 namespace phidgets {
 
-Spatial::Spatial(int32_t serial_number, int hub_port, bool is_hub_port_device,
-                 std::function<void(const double[3], const double[3],
-                                    const double[3], double)>
-                     data_handler,
-                 std::function<void()> attach_handler,
-                 std::function<void()> detach_handler)
+Spatial::Spatial(
+    int32_t serial_number, int hub_port, bool is_hub_port_device,
+    std::function<void(const double[3], const double[3], const double[3],
+                       double)>
+        data_handler,
+    std::function<void(const double[4], double)> algorithm_data_handler,
+    std::function<void()> attach_handler, std::function<void()> detach_handler)
     : serial_number_(serial_number),
       data_handler_(std::move(data_handler)),
+      algorithm_data_handler_(std::move(algorithm_data_handler)),
       attach_handler_(std::move(attach_handler)),
       detach_handler_(std::move(detach_handler))
 {
@@ -65,6 +67,17 @@ Spatial::Spatial(int32_t serial_number, int hub_port, bool is_hub_port_device,
         throw Phidget22Error("Failed to set change handler for Spatial", ret);
     }
 
+    if (algorithm_data_handler_ != nullptr)
+    {
+        ret = PhidgetSpatial_setOnAlgorithmDataHandler(
+            spatial_handle_, AlgorithmDataHandler, this);
+        if (ret != EPHIDGET_OK)
+        {
+            throw Phidget22Error("Failed to set algorithm handler for Spatial",
+                                 ret);
+        }
+    }
+
     if (attach_handler_ != nullptr)
     {
         ret = Phidget_setOnAttachHandler(
@@ -137,6 +150,67 @@ void Spatial::setCompassCorrectionParameters(
     }
 }
 
+void Spatial::setSpatialAlgorithm(const std::string algorithm_name)
+{
+    Phidget_SpatialAlgorithm algorithm;
+
+    if (algorithm_name.compare("none") == 0)
+    {
+        algorithm = SPATIAL_ALGORITHM_NONE;
+    } else if (algorithm_name.compare("ahrs") == 0)
+    {
+        algorithm = SPATIAL_ALGORITHM_AHRS;
+    } else if (algorithm_name.compare("imu") == 0)
+    {
+        algorithm = SPATIAL_ALGORITHM_IMU;
+    } else
+    {
+        throw std::invalid_argument("Unknown spatial algorithm name");
+    }
+
+    PhidgetReturnCode ret =
+        PhidgetSpatial_setAlgorithm(spatial_handle_, algorithm);
+    if (ret != EPHIDGET_OK)
+    {
+        throw Phidget22Error("Failed to set spatial algorithm", ret);
+    }
+}
+
+void Spatial::setAHRSParameters(double angularVelocityThreshold,
+                                double angularVelocityDeltaThreshold,
+                                double accelerationThreshold, double magTime,
+                                double accelTime, double biasTime)
+{
+    PhidgetReturnCode ret = PhidgetSpatial_setAHRSParameters(
+        spatial_handle_, angularVelocityThreshold,
+        angularVelocityDeltaThreshold, accelerationThreshold, magTime,
+        accelTime, biasTime);
+    if (ret != EPHIDGET_OK)
+    {
+        throw Phidget22Error("Failed to set AHRS parameters", ret);
+    }
+}
+
+void Spatial::setAlgorithmMagnetometerGain(double magnetometer_gain)
+{
+    PhidgetReturnCode ret = PhidgetSpatial_setAlgorithmMagnetometerGain(
+        spatial_handle_, magnetometer_gain);
+    if (ret != EPHIDGET_OK)
+    {
+        throw Phidget22Error("Failed to set algorithm magnetometer gain", ret);
+    }
+}
+
+void Spatial::setHeatingEnabled(bool heating_enabled)
+{
+    PhidgetReturnCode ret =
+        PhidgetSpatial_setHeatingEnabled(spatial_handle_, heating_enabled);
+    if (ret != EPHIDGET_OK)
+    {
+        throw Phidget22Error("Failed to set heating flag", ret);
+    }
+}
+
 void Spatial::setDataInterval(uint32_t interval_ms) const
 {
     PhidgetReturnCode ret =
@@ -155,6 +229,12 @@ void Spatial::dataHandler(const double acceleration[3],
     data_handler_(acceleration, angular_rate, magnetic_field, timestamp);
 }
 
+void Spatial::algorithmDataHandler(const double quaternion[4],
+                                   double timestamp) const
+{
+    algorithm_data_handler_(quaternion, timestamp);
+}
+
 void Spatial::attachHandler()
 {
     attach_handler_();
@@ -174,6 +254,13 @@ void Spatial::DataHandler(PhidgetSpatialHandle /* input_handle */, void *ctx,
         ->dataHandler(acceleration, angular_rate, magnetic_field, timestamp);
 }
 
+void Spatial::AlgorithmDataHandler(PhidgetSpatialHandle /* input_handle */,
+                                   void *ctx, const double quaternion[4],
+                                   double timestamp)
+{
+    ((Spatial *)ctx)->algorithmDataHandler(quaternion, timestamp);
+}
+
 void Spatial::AttachHandler(PhidgetHandle /* input_handle */, void *ctx)
 {
     ((Spatial *)ctx)->attachHandler();

phidgets_spatial/README.md

@@ -28,6 +28,16 @@ Parameters
 * `serial` (int) - The serial number of the phidgets spatial to connect to.  If -1 (the default), connects to any spatial phidget that can be found.
 * `hub_port` (int) - The phidgets VINT hub port to connect to.  Only used if the spatial phidget is connected to a VINT hub.  Defaults to 0.
 * `frame_id` (string) - The header frame ID to use when publishing the message.  Defaults to [REP-0145](http://www.ros.org/reps/rep-0145.html) compliant `imu_link`.
+* `use_orientation` (bool) - Use the phidget spatials onboard orientation estimation; Just available on MOT0109 onwards. Set to false for older versions. Defaults to false.
+* `spatial_algorithm` (string) - Name of the spatial algorithm used for orientation estimation (one of "none", "ahrs", "imu"); Just used if `use_orientation` is set to true. Defaults to `ahrs`.
+* `ahrs_angular_velocity_threshold` (double) - Parameter for AHRS orientation estimation; Just used if `use_orientation` is set to true.
+* `ahrs_angular_velocity_delta_threshold` (double) - Parameter for AHRS orientation estimation; Just used if `use_orientation` is set to true.
+* `ahrs_acceleration_threshold` (double) - Parameter for AHRS orientation estimation; Just used if `use_orientation` is set to true.
+* `ahrs_mag_time` (double) - Parameter for AHRS orientation estimation; Just used if `use_orientation` is set to true.
+* `ahrs_accel_time` (double) - Parameter for AHRS orientation estimation; Just used if `use_orientation` is set to true.
+* `ahrs_bias_time` (double) - Parameter for AHRS orientation estimation; Just used if `use_orientation` is set to true.
+* `algorithm_magnetometer_gain` (double) - Gain of magnetometer in orientation estimation algorithm; Just used if `use_orientation` is set to true. Defaults to 0.005
+* `heating_enabled` (bool) - Use the internal heating element; Just available on MOT0109 onwards. Do not set this parameter for older versions.
 * `linear_acceleration_stdev` (double) - The standard deviation to use for the linear acceleration when publishing the message.  Defaults to 280 ug.
 * `angular_velocity_stdev` (double) - The standard deviation to use for the angular velocity when publishing the message.  Defaults to 0.095 deg/s.
 * `magnetic_field_stdev` (double) - The standard deviation to use for the magnetic field when publishing the message.  Defaults to 1.1 milligauss.

phidgets_spatial/include/phidgets_spatial/spatial_ros_i.hpp

@@ -100,11 +100,19 @@ class SpatialRosI final : public rclcpp::Node
     double last_mag_y_{0.0};
     double last_mag_z_{0.0};
 
+    // Onboard orientation estimation results
+    double last_quat_w_;
+    double last_quat_x_;
+    double last_quat_y_;
+    double last_quat_z_;
+
     void publishLatest();
 
     void spatialDataCallback(const double acceleration[3],
                              const double angular_rate[3],
                              const double magnetic_field[3], double timestamp);
+    void spatialAlgorithmDataCallback(const double quaternion[4],
+                                      double timestamp);
     void attachCallback();
     void detachCallback();
 };

phidgets_spatial/launch/spatial-launch.py

@@ -21,6 +21,29 @@
 
 def generate_launch_description():
     """Generate launch description with multiple components."""
+
+    params = {
+        # optional param use_orientation, default is false
+        'use_orientation': False,
+
+        # optional param spatial_algorithm, default is 'ahrs'
+        'spatial_algorithm': 'ahrs',
+
+        # optional ahrs parameters
+        'ahrs_angular_velocity_threshold': 1.0,
+        'ahrs_angular_velocity_delta_threshold': 0.1,
+        'ahrs_acceleration_threshold': 0.1,
+        'ahrs_mag_time': 10.0,
+        'ahrs_accel_time': 10.0,
+        'ahrs_bias_time': 1.25,
+
+        # optional param algorithm_magnetometer_gain, default is 0.005
+        'algorithm_magnetometer_gain': 0.005,
+
+        # optional param heating_enabled, not modified by default
+        'heating_enabled': False,
+    }
+
     container = ComposableNodeContainer(
             name='phidget_container',
             namespace='',
@@ -30,7 +53,8 @@ def generate_launch_description():
                 ComposableNode(
                     package='phidgets_spatial',
                     plugin='phidgets::SpatialRosI',
-                    name='phidgets_spatial'),
+                    name='phidgets_spatial',
+                    parameters=[params]),
             ],
             output='both',
     )