added magnetic field feature

robotology · Aug 22, 2023 · 6111710 · 6111710
1 parent d470251
commit 6111710
Show file tree

Hide file tree

Showing 3 changed files with 42 additions and 16 deletions.
diff --git a/src/devices/multipleAnalogSensors_nws_ros2/GenericSensor_nws_ros2.h b/src/devices/multipleAnalogSensors_nws_ros2/GenericSensor_nws_ros2.h
@@ -49,9 +49,9 @@ class GenericSensor_nws_ros2 :
     double            m_periodInS{0.01};
     std::string       m_publisherName;
     std::string       m_rosNodeName;
-    rclcpp::Node::SharedPtr m_node;
-    typename rclcpp::Publisher<ROS_MSG>::SharedPtr m_publisher;
-    yarp::dev::PolyDriver* m_poly;
+    rclcpp::Node::SharedPtr m_node=nullptr;
+    typename rclcpp::Publisher<ROS_MSG>::SharedPtr m_publisher=nullptr;
+    yarp::dev::PolyDriver* m_poly=nullptr;
     double                 m_timestamp;
     std::string            m_framename;
     const size_t           m_sens_index = 0;
@@ -81,9 +81,6 @@ template <class ROS_MSG>
 GenericSensor_nws_ros2<ROS_MSG>::GenericSensor_nws_ros2() :
     PeriodicThread(0.02)
 {
-    m_node = nullptr;
-    m_publisher=nullptr;
-    m_poly = nullptr;
     m_timestamp=0;
 }
 

diff --git a/src/devices/multipleAnalogSensors_nws_ros2/Imu_nws_ros2.cpp b/src/devices/multipleAnalogSensors_nws_ros2/Imu_nws_ros2.cpp
@@ -19,7 +19,7 @@ YARP_LOG_COMPONENT(GENERICSENSOR_NWS_ROS2, "yarp.device.Imu_nws_ros2")
 
 bool Imu_nws_ros2::viewInterfaces()
 {
-    // View all the interfaces
+    // View mandatory interfaces
     bool ok = true;
     ok = m_poly->view(m_iThreeAxisGyroscopes);
     if (!ok) {
@@ -31,17 +31,26 @@ bool Imu_nws_ros2::viewInterfaces()
         yCError(GENERICSENSOR_NWS_ROS2) << "IThreeAxisLinearAccelerometers interface is not available";
         return false;
     }
-    ok = m_poly->view(m_iThreeAxisMagnetometers);
-    if (!ok) {
-        yCError(GENERICSENSOR_NWS_ROS2) << "IThreeAxisMagnetometers interface is not available";
-        return false;
-    }
     ok = m_poly->view(m_iOrientationSensors);
     if (!ok) {
         yCError(GENERICSENSOR_NWS_ROS2) << "IOrientationSensors interface is not available";
         return false;
     }
 
+    // View optional interfaces
+    bool mag_ok = m_poly->view(m_iThreeAxisMagnetometers);
+    if (mag_ok)
+    {
+        std::string m_publisherMagName = m_publisherName + "_magnetic_field";
+        yCInfo(GENERICSENSOR_NWS_ROS2) << "m_iThreeAxisMagnetometers interface available. Opening topic:" << m_publisherMagName;
+        m_publisher_mag = m_node->create_publisher<sensor_msgs::msg::MagneticField>(m_publisherMagName,rclcpp::QoS(10));
+    }
+    else
+    {
+        yCWarning(GENERICSENSOR_NWS_ROS2) << "m_iThreeAxisMagnetometers interface is not available";
+    }
+
+    // Get frame Name
     ok = m_iThreeAxisGyroscopes->getThreeAxisGyroscopeFrameName(m_sens_index, m_framename);
     return ok;
 }
@@ -53,13 +62,16 @@ void Imu_nws_ros2::run()
         yarp::sig::Vector vecgyr(3);
         yarp::sig::Vector vecacc(3);
         yarp::sig::Vector vecrpy(3);
+
+        m_iThreeAxisGyroscopes->getThreeAxisGyroscopeMeasure(m_sens_index, vecgyr, m_timestamp);
+        m_iThreeAxisLinearAccelerometers->getThreeAxisLinearAccelerometerMeasure(m_sens_index, vecacc, m_timestamp);
+        m_iOrientationSensors->getOrientationSensorMeasureAsRollPitchYaw(m_sens_index, vecrpy, m_timestamp);
+
         tf2::Quaternion quat;
         quat.setRPY(vecrpy[0] * M_PI / 180.0, vecrpy[1] * M_PI / 180.0, vecrpy[2] * M_PI / 180.0);
         quat.normalize();
+
         sensor_msgs::msg::Imu imu_ros_data;
-        m_iThreeAxisGyroscopes->getThreeAxisGyroscopeMeasure(m_sens_index, vecgyr, m_timestamp);
-        m_iThreeAxisLinearAccelerometers->getThreeAxisLinearAccelerometerMeasure(m_sens_index, vecacc, m_timestamp);
-        m_iOrientationSensors->getOrientationSensorMeasureAsRollPitchYaw(m_sens_index, vecrpy, m_timestamp);
         imu_ros_data.header.frame_id = m_framename;
         imu_ros_data.header.stamp = ros2TimeFromYarp(m_timestamp);
         imu_ros_data.angular_velocity.x = vecgyr[0] * M_PI / 180.0;
@@ -71,7 +83,20 @@ void Imu_nws_ros2::run()
         imu_ros_data.orientation.x = quat.x();
         imu_ros_data.orientation.y = quat.y();
         imu_ros_data.orientation.z = quat.z();
-        imu_ros_data.orientation.z = quat.w();
+        imu_ros_data.orientation.w = quat.w();
         m_publisher->publish(imu_ros_data);
+
+        if (m_iThreeAxisMagnetometers)
+        {
+           yarp::sig::Vector vecmag(3);
+           m_iThreeAxisMagnetometers->getThreeAxisMagnetometerMeasure(m_sens_index, vecmag, m_timestamp);
+           sensor_msgs::msg::MagneticField mag_ros_data;
+           mag_ros_data.header.frame_id = m_framename;
+           mag_ros_data.header.stamp = ros2TimeFromYarp(m_timestamp);
+           mag_ros_data.magnetic_field.x=vecmag[0];
+           mag_ros_data.magnetic_field.y=vecmag[1];
+           mag_ros_data.magnetic_field.z=vecmag[2];
+           m_publisher_mag->publish(mag_ros_data);
+        }
     }
 }
diff --git a/src/devices/multipleAnalogSensors_nws_ros2/Imu_nws_ros2.h b/src/devices/multipleAnalogSensors_nws_ros2/Imu_nws_ros2.h
@@ -8,6 +8,7 @@
 
 #include "GenericSensor_nws_ros2.h"
 #include <sensor_msgs/msg/imu.hpp>
+#include <sensor_msgs/msg/magnetic_field.hpp>
 
 /**
  * @ingroup dev_impl_wrapper
@@ -46,6 +47,9 @@ class Imu_nws_ros2 : public GenericSensor_nws_ros2<sensor_msgs::msg::Imu>
 
 protected:
     bool viewInterfaces() override;
+
+private:
+    rclcpp::Publisher<sensor_msgs::msg::MagneticField>::SharedPtr m_publisher_mag=nullptr;
 };
 
 #endif // YARP_DEV_IMU_NWS_ROS2_H