From d1c6ad6a8d0262ef8b991ddb974d8b27b038e906 Mon Sep 17 00:00:00 2001
From: anacg1620 <anacg1620@gmail.com>
Date: Tue, 18 Jun 2024 12:54:28 +0200
Subject: [PATCH] publishes via ros

---
 programs/rgbdDetection/RgbdDetection.cpp | 19 +++++++++++++++++++
 programs/rgbdDetection/RgbdDetection.hpp |  7 +++++++
 2 files changed, 26 insertions(+)

diff --git a/programs/rgbdDetection/RgbdDetection.cpp b/programs/rgbdDetection/RgbdDetection.cpp
index 665dc834..872f7cce 100644
--- a/programs/rgbdDetection/RgbdDetection.cpp
+++ b/programs/rgbdDetection/RgbdDetection.cpp
@@ -14,6 +14,9 @@
 #include <yarp/sig/ImageDraw.h>
 #include <yarp/sig/ImageUtils.h>
 
+//#include <yarp/os/Nodes.h>
+//#include <yarp/os/impl/NameClient.h>
+
 constexpr auto DEFAULT_SENSOR_DEVICE = "RGBDSensorClient";
 constexpr auto DEFAULT_SENSOR_REMOTE = "/rgbd";
 constexpr auto DEFAULT_LOCAL_PREFIX = "/rgbdDetection";
@@ -130,6 +133,15 @@ bool RgbdDetection::configure(yarp::os::ResourceFinder &rf)
     cropPort.setReadOnly();
     cropPort.useCallback(cropCallback);
 
+    node = new yarp::os::Node("/yarp/test_publisher");
+
+    if (!publisher.topic("/chatter")) {
+        yCError(RGBD) << "Failed to create publisher to /chatter";
+        return false;
+    }
+
+    yCDebug(RGBD) << "ROS publisher is ready";
+
     return true;
 }
 
@@ -251,6 +263,13 @@ bool RgbdDetection::updateModule()
 
     imagePort.prepare() = rgbImage;
     imagePort.write();
+
+    // publish ros msgs
+    yarp::rosmsg::std_msgs::String data;
+    data.data = "Hello from YARP";
+
+    publisher.write(data);
+
     return true;
 }
 
diff --git a/programs/rgbdDetection/RgbdDetection.hpp b/programs/rgbdDetection/RgbdDetection.hpp
index 204aa3b5..e4a092de 100644
--- a/programs/rgbdDetection/RgbdDetection.hpp
+++ b/programs/rgbdDetection/RgbdDetection.hpp
@@ -13,6 +13,10 @@
 #include <yarp/sig/Image.h>
 #include <yarp/sig/IntrinsicParams.h>
 
+#include <yarp/os/Node.h>
+#include <yarp/os/Publisher.h>
+#include <yarp/rosmsg/std_msgs/String.h>
+
 #include "IDetector.hpp"
 #include "YarpCropCallback.hpp"
 
@@ -48,6 +52,9 @@ class RgbdDetection : public yarp::os::RFModule
     yarp::os::BufferedPort<yarp::os::Bottle> cropPort;
     YarpCropCallback cropCallback;
 
+    yarp::os::Node * node {nullptr};
+    yarp::os::Publisher<yarp::rosmsg::std_msgs::String> publisher;
+
     double period;
 };