[WIP] added simple publisher and subscriber device for test

src/devices/CMakeLists.txt

@@ -13,3 +13,5 @@ add_subdirectory(map2D_nws_ros2)
 add_subdirectory(frameGrabber_nws_ros2)
 add_subdirectory(rgbdToPointCloudSensor_nws_ros2)
 add_subdirectory(mobileBaseVelocityControl_nws_ros2)
+add_subdirectory(ros2SubscriptionTest)
+add_subdirectory(ros2PublishTest)

src/devices/ros2PublishTest/CMakeLists.txt

@@ -0,0 +1,40 @@
+# Copyright (C) 2006-2020 Istituto Italiano di Tecnologia (IIT)
+# All rights reserved.
+#
+# This software may be modified and distributed under the terms of the
+# BSD-3-Clause license. See the accompanying LICENSE file for details.
+
+yarp_prepare_plugin(ros2PublishTest
+  CATEGORY device
+  TYPE Ros2PublishTest
+  INCLUDE Ros2PublishTest.h
+  INTERNAL ON
+)
+
+yarp_add_plugin(yarp_ros2PublishTest)
+
+target_sources(yarp_ros2PublishTest
+  PRIVATE
+    Ros2PublishTest.cpp
+    Ros2PublishTest.h
+)
+
+target_link_libraries(yarp_ros2PublishTest
+  PRIVATE
+    YARP::YARP_os
+    YARP::YARP_sig
+    YARP::YARP_dev
+    rclcpp::rclcpp
+    std_msgs::std_msgs__rosidl_typesupport_cpp
+)
+
+yarp_install(
+  TARGETS yarp_ros2PublishTest
+  EXPORT yarp-device-ros2PublishTest
+  COMPONENT yarp-device-ros2PublishTest
+  LIBRARY DESTINATION ${YARP_DYNAMIC_PLUGINS_INSTALL_DIR}
+  ARCHIVE DESTINATION ${YARP_STATIC_PLUGINS_INSTALL_DIR}
+  YARP_INI DESTINATION ${YARP_PLUGIN_MANIFESTS_INSTALL_DIR}
+)
+
+set_property(TARGET yarp_ros2PublishTest PROPERTY FOLDER "Plugins/Device")

src/devices/ros2PublishTest/Ros2PublishTest.cpp

@@ -0,0 +1,81 @@
+/*
+ * Copyright (C) 2006-2020 Istituto Italiano di Tecnologia (IIT)
+ * All rights reserved.
+ *
+ * This software may be modified and distributed under the terms of the
+ * BSD-3-Clause license. See the accompanying LICENSE file for details.
+ */
+
+#include "Ros2PublishTest.h"
+
+#include <yarp/os/LogComponent.h>
+#include <yarp/os/LogStream.h>
+
+#include <chrono>
+#include <functional>
+#include <memory>
+#include <string>
+
+using namespace std::chrono_literals;
+
+
+YARP_LOG_COMPONENT(ROS2PUBLISHTEST, "yarp.ros2.ros2PublishTest", yarp::os::Log::TraceType);
+
+MinimalPublisher::MinimalPublisher(std::string name, std::string topic)
+: Node(name)
+{
+    publisher_ = this->create_publisher<std_msgs::msg::String>(topic, 10);
+}
+
+void MinimalPublisher::publish(std::string messageString) {
+    auto message = std_msgs::msg::String();
+    message.data = messageString;
+    publisher_->publish(message);
+}
+
+Ros2PublishTest::Ros2PublishTest():
+PeriodicThread(0.033)
+{
+}
+
+bool Ros2PublishTest::open(yarp::os::Searchable& config)
+{
+    yCTrace(ROS2PUBLISHTEST);
+    if (config.check("node_name"))
+    {
+        m_node_name = config.find("node_name").asString();
+    } else {
+        yCError(ROS2PUBLISHTEST) << "missing node_name parameter";
+        return false;
+    }
+
+    if (config.check("topic_name"))
+    {
+        m_topic = config.find("topic_name").asString();
+    } else {
+        yCError(ROS2PUBLISHTEST) << "missing topic_name parameter";
+        return false;
+    }
+
+
+    if(!rclcpp::ok())
+    {
+        rclcpp::init(/*argc*/ 0, /*argv*/ nullptr);
+    }
+    minimalPublisher = new MinimalPublisher(m_node_name, m_topic) ;
+
+    start();
+    return true;
+}
+
+bool Ros2PublishTest::close()
+{
+    yCInfo(ROS2PUBLISHTEST, "Ros2PublishTest::close");
+    rclcpp::shutdown();
+    return true;
+}
+
+void Ros2PublishTest::run()
+{
+    minimalPublisher->publish("Hello from " + m_topic + "! " + m_node_name);
+}

src/devices/ros2PublishTest/Ros2PublishTest.h

@@ -0,0 +1,56 @@
+/*
+ * Copyright (C) 2006-2020 Istituto Italiano di Tecnologia (IIT)
+ * All rights reserved.
+ *
+ * This software may be modified and distributed under the terms of the
+ * BSD-3-Clause license. See the accompanying LICENSE file for details.
+ */
+
+#ifndef YARP_ROS2_ROS2PUBLISHTEST_H
+#define YARP_ROS2_ROS2PUBLISHTEST_H
+
+#include <yarp/dev/DeviceDriver.h>
+#include <yarp/os/PeriodicThread.h>
+
+#include <rclcpp/rclcpp.hpp>
+#include <std_msgs/msg/string.hpp>
+
+#include <mutex>
+
+class MinimalPublisher : public rclcpp::Node
+{
+public:
+    MinimalPublisher(std::string name, std::string topic);
+    void publish(std::string messageString);
+
+private:
+    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
+};
+
+class Ros2PublishTest :
+        public yarp::dev::DeviceDriver,
+        public yarp::os::PeriodicThread
+{
+public:
+    Ros2PublishTest();
+    Ros2PublishTest(const Ros2PublishTest&) = delete;
+    Ros2PublishTest(Ros2PublishTest&&) noexcept = delete;
+    Ros2PublishTest& operator=(const Ros2PublishTest&) = delete;
+    Ros2PublishTest& operator=(Ros2PublishTest&&) noexcept = delete;
+    ~Ros2PublishTest() override = default;
+
+    // DeviceDriver
+    bool open(yarp::os::Searchable& config) override;
+    bool close() override;
+
+    // PeriodicThread
+    void run() override;
+
+private:
+
+    MinimalPublisher* minimalPublisher;
+    std::string m_node_name;
+    std::string m_topic;
+};
+
+#endif // YARP_ROS2_ROS2PUBLISHTEST_H

src/devices/ros2PublishTest/aaa.xml

@@ -0,0 +1,31 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<!DOCTYPE robot PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">
+<robot name="CER-SIM-DEPTH" prefix="CER-SIM-DEPTH" xmlns:xi="http://www.w3.org/2001/XInclude">
+   <devices>
+
+       <device xmlns:xi="http://www.w3.org/2001/XInclude"  name="subscriber1" type="ros2SubscriptionTest">
+           <param name="node_name">node1</param>
+           <param name="topic_name">topic_node1</param>
+
+       </device>
+
+       <device xmlns:xi="http://www.w3.org/2001/XInclude"  name="subscriber21" type="ros2SubscriptionTest">
+           <param name="node_name">node2</param>
+           <param name="topic_name">topic_node2</param>
+
+       </device>
+       <device xmlns:xi="http://www.w3.org/2001/XInclude"  name="publisher1" type="ros2PublishTest">
+           <param name="node_name">node3</param>
+           <param name="topic_name">topic_node1</param>
+
+       </device>
+       <device xmlns:xi="http://www.w3.org/2001/XInclude"  name="publisher1" type="ros2PublishTest">
+           <param name="node_name">node4</param>
+           <param name="topic_name">topic_node2</param>
+
+       </device>
+
+
+   </devices>
+</robot>
+

src/devices/ros2SubscriptionTest/CMakeLists.txt

@@ -0,0 +1,40 @@
+# Copyright (C) 2006-2020 Istituto Italiano di Tecnologia (IIT)
+# All rights reserved.
+#
+# This software may be modified and distributed under the terms of the
+# BSD-3-Clause license. See the accompanying LICENSE file for details.
+
+yarp_prepare_plugin(ros2SubscriptionTest
+  CATEGORY device
+  TYPE Ros2SubscriptionTest
+  INCLUDE Ros2SubscriptionTest.h
+  INTERNAL ON
+)
+
+yarp_add_plugin(yarp_ros2SubscriptionTest)
+
+target_sources(yarp_ros2SubscriptionTest
+  PRIVATE
+        Ros2SubscriptionTest.cpp
+        Ros2SubscriptionTest.h
+)
+
+target_link_libraries(yarp_ros2SubscriptionTest
+  PRIVATE
+    YARP::YARP_os
+    YARP::YARP_sig
+    YARP::YARP_dev
+    rclcpp::rclcpp
+    std_msgs::std_msgs__rosidl_typesupport_cpp
+)
+
+yarp_install(
+  TARGETS yarp_ros2SubscriptionTest
+  EXPORT yarp-device-ros2SubscriptionTest
+  COMPONENT yarp-device-ros2SubscriptionTest
+  LIBRARY DESTINATION ${YARP_DYNAMIC_PLUGINS_INSTALL_DIR}
+  ARCHIVE DESTINATION ${YARP_STATIC_PLUGINS_INSTALL_DIR}
+  YARP_INI DESTINATION ${YARP_PLUGIN_MANIFESTS_INSTALL_DIR}
+)
+
+set_property(TARGET yarp_ros2SubscriptionTest PROPERTY FOLDER "Plugins/Device")

src/devices/ros2SubscriptionTest/Ros2SubscriptionTest.cpp

@@ -0,0 +1,106 @@
+/*
+ * Copyright (C) 2006-2020 Istituto Italiano di Tecnologia (IIT)
+ * All rights reserved.
+ *
+ * This software may be modified and distributed under the terms of the
+ * BSD-3-Clause license. See the accompanying LICENSE file for details.
+ */
+
+#include "Ros2SubscriptionTest.h"
+
+#include <yarp/os/LogComponent.h>
+#include <yarp/os/LogStream.h>
+
+#include <chrono>
+#include <functional>
+#include <memory>
+#include <string>
+
+using namespace std::chrono_literals;
+using std::placeholders::_1;
+
+YARP_LOG_COMPONENT(ROS2SUBSCRIPTIONTEST, "yarp.ros2.ros2SubscriptionTest", yarp::os::Log::TraceType);
+
+class MinimalSubscriber : public rclcpp::Node
+{
+public:
+    MinimalSubscriber(std::string name, Ros2SubscriptionTest *subscriptionTest, std::string topic)
+    : Node(name)
+    {
+        subscription_ = this->create_subscription<std_msgs::msg::String>(
+            topic, 10, std::bind(&MinimalSubscriber::topic_callback, this, _1));
+        m_subscriptionTest = subscriptionTest;
+    }
+
+private:
+    void topic_callback(const std_msgs::msg::String::SharedPtr msg) const
+    {
+        m_subscriptionTest->local_callback(msg);
+    }
+    Ros2SubscriptionTest *m_subscriptionTest;
+    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscription_;
+};
+
+
+
+
+
+//Ros2Init::Ros2Init()
+//{
+//    rclcpp::init(/*argc*/ 0, /*argv*/ nullptr);
+//    node = std::make_shared<rclcpp::Node>("yarprobotinterface_node");
+//}
+//
+//Ros2Init& Ros2Init::get()
+//{
+//    static Ros2Init instance;
+//    return instance;
+//}
+
+
+Ros2SubscriptionTest::Ros2SubscriptionTest()
+{
+}
+
+bool Ros2SubscriptionTest::open(yarp::os::Searchable& config)
+{
+    if (config.check("node_name"))
+    {
+        m_node_name = config.find("node_name").asString();
+    } else {
+        yCError(ROS2SUBSCRIPTIONTEST) << "missing node_name parameter";
+        return false;
+    }
+
+    if (config.check("topic_name"))
+    {
+        m_topic = config.find("topic_name").asString();
+    } else {
+        yCError(ROS2SUBSCRIPTIONTEST) << "missing topic_name parameter";
+        return false;
+    }
+    start();
+    return true;
+}
+
+bool Ros2SubscriptionTest::close()
+{
+    yCTrace(ROS2SUBSCRIPTIONTEST);
+    yCInfo(ROS2SUBSCRIPTIONTEST, "Ros2SubscriptionTest::close");
+    return true;
+}
+
+void Ros2SubscriptionTest::local_callback(const std_msgs::msg::String::SharedPtr msg) const
+{
+    yInfo() << "hello, I'm " + m_node_name + m_topic + ", I heard: '%s'" << msg->data.c_str();
+}
+
+void Ros2SubscriptionTest::run()
+{
+    if(!rclcpp::ok())
+    {
+        rclcpp::init(/*argc*/ 0, /*argv*/ nullptr);
+    }
+    rclcpp::spin(std::make_shared<MinimalSubscriber>(m_node_name, this, m_topic));
+    rclcpp::shutdown();
+}