Merge pull request leggedrobotics#182 from leggedrobotics/fix/image_p…

…ublisher Fixed copy of image publisher.
bxtbold · Oct 2, 2019 · 687d8f9 · 687d8f9
2 parents ac666ab + d09e2a8
commit 687d8f9
Show file tree

Hide file tree

Showing 3 changed files with 28 additions and 0 deletions.
diff --git a/darknet_ros/include/darknet_ros/image_interface.h b/darknet_ros/include/darknet_ros/image_interface.h
@@ -11,6 +11,8 @@
 
 #include "image.h"
 
+static float get_pixel(image m, int x, int y, int c);
 image **load_alphabet_with_file(char *datafile);
+void generate_image(image p, IplImage *disp);
 
 #endif
diff --git a/darknet_ros/src/YoloObjectDetector.cpp b/darknet_ros/src/YoloObjectDetector.cpp
@@ -541,6 +541,8 @@ void YoloObjectDetector::yolo()
       demoTime_ = what_time_is_it_now();
       if (viewImage_) {
         displayInThread(0);
+      } else {
+        generate_image(buff_[(buffIndex_ + 1)%3], ipl_);
       }
       publishInThread();
     } else {

diff --git a/darknet_ros/src/image_interface.c b/darknet_ros/src/image_interface.c
@@ -8,6 +8,12 @@
 
 #include "darknet_ros/image_interface.h"
 
+static float get_pixel(image m, int x, int y, int c)
+{
+    assert(x < m.w && y < m.h && c < m.c);
+    return m.data[c*m.h*m.w + y*m.w + x];
+}
+
 image **load_alphabet_with_file(char *datafile) {
   int i, j;
   const int nsize = 8;
@@ -26,3 +32,21 @@ image **load_alphabet_with_file(char *datafile) {
   }
   return alphabets;
 }
+
+#ifdef OPENCV
+void generate_image(image p, IplImage *disp)
+{
+    int x,y,k;
+    if(p.c == 3) rgbgr_image(p);
+    //normalize_image(copy);
+
+    int step = disp->widthStep;
+    for(y = 0; y < p.h; ++y){
+        for(x = 0; x < p.w; ++x){
+            for(k= 0; k < p.c; ++k){
+                disp->imageData[y*step + x*p.c + k] = (unsigned char)(get_pixel(p,x,y,k)*255);
+            }
+        }
+    }
+}
+#endif