Multiple analog sensors nwc ros2

src/devices/CMakeLists.txt

@@ -18,4 +18,5 @@ add_subdirectory(frameTransformGet_nwc_ros2)
 add_subdirectory(ros2RGBDConversionUtils)
 add_subdirectory(rgbdSensor_nwc_ros2)
 add_subdirectory(multipleAnalogSensors_nws_ros2)
+add_subdirectory(multipleAnalogSensors_nwc_ros2)
 add_subdirectory(rangefinder2D_controlBoard_nws_ros2)

src/devices/multipleAnalogSensors_nwc_ros2/CMakeLists.txt

@@ -0,0 +1,56 @@
+# SPDX-FileCopyrightText: 2023 Istituto Italiano di Tecnologia (IIT)
+# SPDX-License-Identifier: BSD-3-Clause
+
+yarp_prepare_plugin(imu_nwc_ros2
+  CATEGORY device
+  TYPE Imu_nwc_ros2
+  INCLUDE Imu_nwc_ros2.h
+  DEPENDS "TARGET YARP::YARP_math"
+  DEFAULT ON
+)
+
+if(ENABLE_imu_nwc_ros2)
+  yarp_add_plugin(yarp_imu_nwc_ros2)
+
+  target_sources(yarp_imu_nwc_ros2
+    PRIVATE
+      Imu_nwc_ros2.cpp
+      Imu_nwc_ros2.h
+      GenericSensor_nwc_ros2.h
+  )
+
+  target_link_libraries(yarp_imu_nwc_ros2
+    PRIVATE
+      YARP::YARP_os
+      YARP::YARP_sig
+      YARP::YARP_dev
+      rclcpp::rclcpp
+      sensor_msgs::sensor_msgs__rosidl_typesupport_cpp
+      tf2::tf2
+      Ros2Utils
+  )
+
+  list(APPEND YARP_${YARP_PLUGIN_MASTER}_PRIVATE_DEPS
+    YARP_os
+    YARP_sig
+    YARP_dev
+  )
+
+  yarp_install(
+    TARGETS yarp_imu_nwc_ros2
+    EXPORT YARP_${YARP_PLUGIN_MASTER}
+    COMPONENT ${YARP_PLUGIN_MASTER}
+    LIBRARY DESTINATION ${YARP_DYNAMIC_PLUGINS_INSTALL_DIR}
+    ARCHIVE DESTINATION ${YARP_STATIC_PLUGINS_INSTALL_DIR}
+    YARP_INI DESTINATION ${YARP_PLUGIN_MANIFESTS_INSTALL_DIR}
+  )
+
+  set(YARP_${YARP_PLUGIN_MASTER}_PRIVATE_DEPS ${YARP_${YARP_PLUGIN_MASTER}_PRIVATE_DEPS} PARENT_SCOPE)
+
+  set_property(TARGET yarp_imu_nwc_ros2 PROPERTY FOLDER "Plugins/Device")
+
+  if(YARP_COMPILE_TESTS)
+    add_subdirectory(tests)
+  endif()
+
+endif()

src/devices/multipleAnalogSensors_nwc_ros2/GenericSensor_nwc_ros2.h

@@ -0,0 +1,151 @@
+/*
+ * SPDX-FileCopyrightText: 2023 Istituto Italiano di Tecnologia (IIT)
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef YARP_DEV_GENERICSENSORS_NWC_ROS2_H
+#define YARP_DEV_GENERICSENSORS_NWC_ROS2_H
+
+#include <yarp/os/PeriodicThread.h>
+#include <yarp/dev/DeviceDriver.h>
+#include <yarp/dev/IMultipleWrapper.h>
+#include <yarp/dev/MultipleAnalogSensorsInterfaces.h>
+#include <yarp/os/Log.h>
+#include <yarp/os/LogComponent.h>
+#include <yarp/os/LogStream.h>
+#include <mutex>
+
+#include <rclcpp/rclcpp.hpp>
+#include <Ros2Utils.h>
+#include <Ros2Spinner.h>
+
+
+// The log component is defined in each device, with a specialized name
+YARP_DECLARE_LOG_COMPONENT(GENERICSENSOR_NWC_ROS2)
+
+
+/**
+ * @ingroup dev_impl_wrapper
+ *
+ * \brief This abstract template needs to be specialized in a ROS2 subscription, for a specific ROS2 message/sensor type.
+ *
+ * | YARP device name |
+ * |:-----------------:|
+ * | `genericSensor_nwc_ros2` |
+ *
+ * The parameters accepted by this device are:
+ * | Parameter name | SubParameter   | Type    | Units          | Default Value    | Required                    | Description                                                       | Notes                           |
+ * |:--------------:|:--------------:|:-------:|:--------------:|:----------------:|:--------------------------: |:-----------------------------------------------------------------:|:-------------------------------:|
+ * | topic_name     |      -         | string  | -              |   -              | Yes                         | The name of the ROS topic opened by this device.                  | MUST start with a '/' character |
+ * | node_name      |      -         | string  | -              |   -              | Yes                         | The name of the ROS node opened by this device                    | Autogenerated by default        |
+ * | sensor_name    |      -         | string  | -              |   -              | Yes                         | The name of the sensor the data are coming from                   |                                 |
+ */
+
+template <class ROS_MSG>
+class GenericSensor_nwc_ros2 :
+        public yarp::dev::DeviceDriver
+{
+protected:
+    double        m_periodInS{0.01};
+    double        m_timestamp;
+    std::string   m_subscriptionName;
+    std::string   m_rosNodeName;
+    std::string   m_framename;
+    std::string   m_sensorName;
+    Ros2Spinner*  m_spinner{nullptr};
+    const size_t  m_sens_index = 0;
+    mutable std::mutex      m_dataMutex;
+    yarp::dev::MAS_status   m_internalStatus;
+    rclcpp::Node::SharedPtr m_node;
+    typename rclcpp::Subscription<ROS_MSG>::SharedPtr m_subscription;
+
+    // Subscription callback. To be implemented for each derived device
+    virtual void subscription_callback(const std::shared_ptr<ROS_MSG> msg)=0;
+
+public:
+    GenericSensor_nwc_ros2();
+    virtual ~GenericSensor_nwc_ros2();
+
+    /* DevideDriver methods */
+    bool open(yarp::os::Searchable &params) override;
+    bool close() override;
+};
+
+template <class ROS_MSG>
+GenericSensor_nwc_ros2<ROS_MSG>::GenericSensor_nwc_ros2()
+{
+    m_node = nullptr;
+    m_subscription=nullptr;
+    m_timestamp=0;
+}
+
+template <class ROS_MSG>
+GenericSensor_nwc_ros2<ROS_MSG>::~GenericSensor_nwc_ros2() = default;
+
+template <class ROS_MSG>
+bool GenericSensor_nwc_ros2<ROS_MSG>::open(yarp::os::Searchable & config)
+{
+    if (!config.check("topic_name")) {
+        yCError(GENERICSENSOR_NWC_ROS2, "Missing topic_name parameter, exiting.");
+        return false;
+    }
+
+    if (!config.check("node_name"))
+    {
+        yCError(GENERICSENSOR_NWC_ROS2) << "Missing node_name parameter";
+        return false;
+    }
+
+    if (!config.check("sensor_name")) {
+        yCError(GENERICSENSOR_NWC_ROS2, "Missing sensor_name parameter, exiting.");
+        return false;
+    }
+
+    m_rosNodeName = config.find("node_name").asString();
+    if (m_rosNodeName.c_str()[0] == '/') {
+        yCError(GENERICSENSOR_NWC_ROS2) << "node name cannot begin with /";
+        return false;
+    }
+
+    m_subscriptionName = config.find("topic_name").asString();
+    if (m_subscriptionName.c_str()[0] != '/') {
+        yCError(GENERICSENSOR_NWC_ROS2) << "Missing '/' in topic_name parameter";
+        return false;
+    }
+
+    m_sensorName = config.find("sensor_name").asString();
+
+    m_node = NodeCreator::createNode(m_rosNodeName);
+    m_subscription = m_node->create_subscription<ROS_MSG>(m_subscriptionName, rclcpp::QoS(10),
+                                                          std::bind(&GenericSensor_nwc_ros2<ROS_MSG>::subscription_callback,
+                                                          this, std::placeholders::_1));
+
+    if (m_node == nullptr) {
+        yCError(GENERICSENSOR_NWC_ROS2) << "Opening " << m_rosNodeName << " Node creation failed, check your yarp-ROS network configuration\n";
+        return false;
+    }
+
+    if (m_subscription == nullptr) {
+        yCError(GENERICSENSOR_NWC_ROS2) << "Opening " << m_subscriptionName << " Topic failed, check your yarp-ROS network configuration\n";
+        return false;
+    }
+
+    m_internalStatus = yarp::dev::MAS_status::MAS_WAITING_FOR_FIRST_READ;
+    m_spinner = new Ros2Spinner(m_node);
+    m_spinner->start();
+
+    yCInfo(GENERICSENSOR_NWC_ROS2) << "Device opened";
+
+    return true;
+}
+
+template <class ROS_MSG>
+bool GenericSensor_nwc_ros2<ROS_MSG>::close()
+{
+    yCInfo(GENERICSENSOR_NWC_ROS2) << "Closing...";
+    delete m_spinner;
+    yCInfo(GENERICSENSOR_NWC_ROS2) << "Closed";
+    return true;
+}
+
+#endif