publishes 3d point msg

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

Hide file tree

Showing 2 changed files with 12 additions and 16 deletions.
diff --git a/programs/rgbdDetection/RgbdDetection.cpp b/programs/rgbdDetection/RgbdDetection.cpp
@@ -14,9 +14,6 @@
 #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";
@@ -133,15 +130,13 @@ bool RgbdDetection::configure(yarp::os::ResourceFinder &rf)
     cropPort.setReadOnly();
     cropPort.useCallback(cropCallback);
 
-    node = new yarp::os::Node("/yarp/test_publisher");
+    node = new yarp::os::Node("/yarp/rgbd_detection_publisher");
 
-    if (!publisher.topic("/chatter")) {
-        yCError(RGBD) << "Failed to create publisher to /chatter";
+    if (!publisher.topic("/rgbd_detection_coords")) {
+        yCError(RGBD) << "Failed to create publisher to /rgbd_detection_coords";
         return false;
     }
 
-    yCDebug(RGBD) << "ROS publisher is ready";
-
     return true;
 }
 
@@ -252,6 +247,13 @@ bool RgbdDetection::updateModule()
 
                 statePort.prepare() = {yarp::os::Value(x), yarp::os::Value(y), yarp::os::Value(z)};
                 statePort.write();
+
+                yarp::rosmsg::geometry_msgs::Point data;
+                data.x = x;
+                data.y = y;
+                data.z = z;
+
+                publisher.write(data);
             }
 
             if (imagePort.getOutputCount() > 0)
@@ -264,12 +266,6 @@ 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
@@ -15,7 +15,7 @@
 
 #include <yarp/os/Node.h>
 #include <yarp/os/Publisher.h>
-#include <yarp/rosmsg/std_msgs/String.h>
+#include <yarp/rosmsg/geometry_msgs/Point.h>
 
 #include "IDetector.hpp"
 #include "YarpCropCallback.hpp"
@@ -53,7 +53,7 @@ class RgbdDetection : public yarp::os::RFModule
     YarpCropCallback cropCallback;
 
     yarp::os::Node * node {nullptr};
-    yarp::os::Publisher<yarp::rosmsg::std_msgs::String> publisher;
+    yarp::os::Publisher<yarp::rosmsg::geometry_msgs::Point> publisher;
 
     double period;
 };