From 9c63b62a4b9f7f97c114a46007fcd78c1ec05457 Mon Sep 17 00:00:00 2001 From: Stefano Bernagozzi Date: Thu, 9 Sep 2021 15:48:10 +0200 Subject: [PATCH] added simple publisher and subscriber device for test --- src/devices/CMakeLists.txt | 3 + src/devices/ros2PublishTest/CMakeLists.txt | 40 +++++++ .../ros2PublishTest/Ros2PublishTest.cpp | 83 ++++++++++++++ src/devices/ros2PublishTest/Ros2PublishTest.h | 56 +++++++++ src/devices/ros2PublishTest/aaa.xml | 31 +++++ .../ros2SubscriptionTest/CMakeLists.txt | 40 +++++++ .../Ros2SubscriptionTest.cpp | 108 ++++++++++++++++++ .../Ros2SubscriptionTest.h | 76 ++++++++++++ 8 files changed, 437 insertions(+) create mode 100644 src/devices/ros2PublishTest/CMakeLists.txt create mode 100644 src/devices/ros2PublishTest/Ros2PublishTest.cpp create mode 100644 src/devices/ros2PublishTest/Ros2PublishTest.h create mode 100644 src/devices/ros2PublishTest/aaa.xml create mode 100644 src/devices/ros2SubscriptionTest/CMakeLists.txt create mode 100644 src/devices/ros2SubscriptionTest/Ros2SubscriptionTest.cpp create mode 100644 src/devices/ros2SubscriptionTest/Ros2SubscriptionTest.h diff --git a/src/devices/CMakeLists.txt b/src/devices/CMakeLists.txt index 0254e2e..7b7e9e7 100644 --- a/src/devices/CMakeLists.txt +++ b/src/devices/CMakeLists.txt @@ -13,3 +13,6 @@ add_subdirectory(map2D_nws_ros2) add_subdirectory(frameGrabber_nws_ros2) add_subdirectory(rgbdToPointCloudSensor_nws_ros2) add_subdirectory(mobileBaseVelocityControl_nws_ros2) +add_subdirectory(Ros2ConversionUtils) +add_subdirectory(ros2SubscriptionTest) +add_subdirectory(ros2PublishTest) diff --git a/src/devices/ros2PublishTest/CMakeLists.txt b/src/devices/ros2PublishTest/CMakeLists.txt new file mode 100644 index 0000000..6fdda27 --- /dev/null +++ b/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") diff --git a/src/devices/ros2PublishTest/Ros2PublishTest.cpp b/src/devices/ros2PublishTest/Ros2PublishTest.cpp new file mode 100644 index 0000000..97597a8 --- /dev/null +++ b/src/devices/ros2PublishTest/Ros2PublishTest.cpp @@ -0,0 +1,83 @@ +/* + * 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 +#include + +#include +#include +#include +#include + +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(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; + } + + + try{ + rclcpp::init(/*argc*/ 0, /*argv*/ nullptr); + }catch (const std::exception& e) + { + yCInfo(ROS2PUBLISHTEST) << "context already initialized, continuing"; + } + 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); +} diff --git a/src/devices/ros2PublishTest/Ros2PublishTest.h b/src/devices/ros2PublishTest/Ros2PublishTest.h new file mode 100644 index 0000000..a7d785d --- /dev/null +++ b/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 +#include + +#include +#include + +#include + +class MinimalPublisher : public rclcpp::Node +{ +public: + MinimalPublisher(std::string name, std::string topic); + void publish(std::string messageString); + +private: + rclcpp::Publisher::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 diff --git a/src/devices/ros2PublishTest/aaa.xml b/src/devices/ros2PublishTest/aaa.xml new file mode 100644 index 0000000..31c3444 --- /dev/null +++ b/src/devices/ros2PublishTest/aaa.xml @@ -0,0 +1,31 @@ + + + + + + + node1 + topic_node1 + + + + + node2 + topic_node2 + + + + node3 + topic_node1 + + + + node4 + topic_node2 + + + + + + + diff --git a/src/devices/ros2SubscriptionTest/CMakeLists.txt b/src/devices/ros2SubscriptionTest/CMakeLists.txt new file mode 100644 index 0000000..3f5af6a --- /dev/null +++ b/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") diff --git a/src/devices/ros2SubscriptionTest/Ros2SubscriptionTest.cpp b/src/devices/ros2SubscriptionTest/Ros2SubscriptionTest.cpp new file mode 100644 index 0000000..fa1e896 --- /dev/null +++ b/src/devices/ros2SubscriptionTest/Ros2SubscriptionTest.cpp @@ -0,0 +1,108 @@ +/* + * 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 +#include + +#include +#include +#include +#include + +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( + 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::SharedPtr subscription_; +}; + + + + + +//Ros2Init::Ros2Init() +//{ +// rclcpp::init(/*argc*/ 0, /*argv*/ nullptr); +// node = std::make_shared("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() +{ + try{ + rclcpp::init(/*argc*/ 0, /*argv*/ nullptr); + }catch (const std::exception& e) + { + yCInfo(ROS2SUBSCRIPTIONTEST) << "context already initialized, continuing"; + } + rclcpp::spin(std::make_shared(m_node_name, this, m_topic)); + rclcpp::shutdown(); +} diff --git a/src/devices/ros2SubscriptionTest/Ros2SubscriptionTest.h b/src/devices/ros2SubscriptionTest/Ros2SubscriptionTest.h new file mode 100644 index 0000000..5aa3a06 --- /dev/null +++ b/src/devices/ros2SubscriptionTest/Ros2SubscriptionTest.h @@ -0,0 +1,76 @@ +/* + * 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_ROS2SUBSCRIPTIONTEST_H +#define YARP_ROS2_ROS2SUBSCRIPTIONTEST_H + +#include +#include + +#include +#include + +#include + +//class Ros2Init +//{ +//public: +// Ros2Init(); +// +// std::shared_ptr node; +// +// static Ros2Init& get(); +//}; + +/***** + * in ros2 non puoi inizializzare i nomi dei nodi con lo / + * e non possono contenere caratteri alfanumerici diversi da _ + * + * puoi chiamare i nodi con lo stesso nome ma per ros2 sono due nodi diversi + * try catch, purtroppo non esiste un modo per controllare se e' gia' inizializzato.... + * esisteva ma loo hanno deprecato. si puo' controllare se ci sono alternative + */ + +class MinimalPublisher +{ +public: + MinimalPublisher(const std::string& topicname); +}; + + +class Ros2SubscriptionTest : + public yarp::dev::DeviceDriver, + public yarp::os::Thread +{ +public: + Ros2SubscriptionTest(); + Ros2SubscriptionTest(const Ros2SubscriptionTest&) = delete; + Ros2SubscriptionTest(Ros2SubscriptionTest&&) noexcept = delete; + Ros2SubscriptionTest& operator=(const Ros2SubscriptionTest&) = delete; + Ros2SubscriptionTest& operator=(Ros2SubscriptionTest&&) noexcept = delete; + ~Ros2SubscriptionTest() override = default; + + // DeviceDriver + bool open(yarp::os::Searchable& config) override; + bool close() override; + + // PeriodicThread + void run() override; + void local_callback(const std_msgs::msg::String::SharedPtr msg) const; + +private: + rclcpp::Publisher::SharedPtr m_publisher; + rclcpp::Subscription::SharedPtr m_subscription; + + std::string m_topic; + std::string m_node_name; + size_t m_count {0}; + +}; + +#endif // YARP_ROS2_ROS2SUBSCRIPTIONTEST_H