publishes via ros

roboticslab-uc3m · Jun 18, 2024 · d1c6ad6 · d1c6ad6
1 parent a19c99f
commit d1c6ad6
Show file tree

Hide file tree

Showing 2 changed files with 26 additions and 0 deletions.
diff --git 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
@@ -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;
 };