Merge pull request #257 from fmrico/opencv4

Support for Opencv4 in ROS2 Foxy [WIP]
leggedrobotics · Aug 21, 2020 · 9e76f3b · 9e76f3b
2 parents fec923b + 8afe311
commit 9e76f3b
Show file tree

Hide file tree

Showing 5 changed files with 148 additions and 40 deletions.
diff --git a/README.md b/README.md
@@ -4,7 +4,7 @@
 
 This is a ROS package developed for object detection in camera images. You only look once (YOLO) is a state-of-the-art, real-time object detection system. In the following ROS package you are able to use YOLO (V3) on GPU and CPU. The pre-trained model of the convolutional neural network is able to detect pre-trained classes including the data set from VOC and COCO, or you can also create a network with your own detection objects. For more information about YOLO, Darknet, available training data and training YOLO see the following link: [YOLO: Real-Time Object Detection](http://pjreddie.com/darknet/yolo/).
 
-The YOLO packages have been tested under ROS Melodic and Ubuntu 18.04. This is research code, expect that it changes often and any fitness for a particular purpose is disclaimed.
+The YOLO packages have been tested under ROS Foxy, Ubuntu 20.04 and OpenCV 4.2.0. This is research code, expect that it changes often and any fitness for a particular purpose is disclaimed.
 
 **Author: [Marko Bjelonic](https://www.markobjelonic.com), [email protected]**
 
@@ -52,7 +52,7 @@ URL: https://github.com/leggedrobotics/darknet_ros, 2018.
 
 ### Dependencies
 
-This software is built on the Robotic Operating System ([ROS]), which needs to be [installed](http://wiki.ros.org) first. Additionally, YOLO for ROS depends on following software:
+This software is built on the Robotic Operating System version([ROS]), which needs to be [installed](http://wiki.ros.org) first. Additionally, YOLO for ROS depends on following software:
 
 - [OpenCV](http://opencv.org/) (computer vision library),
 - [boost](http://www.boost.org/) (c++ library),
@@ -63,17 +63,14 @@ This software is built on the Robotic Operating System ([ROS]), which needs to b
 
 In order to install darknet_ros, clone the latest version using SSH (see [how to set up an SSH key](https://confluence.atlassian.com/bitbucket/set-up-an-ssh-key-728138079.html)) from this repository into your catkin workspace and compile the package using ROS.
 
-    cd catkin_workspace/src
+    cd colcon_workspace/src
     git clone --recursive [email protected]:leggedrobotics/darknet_ros.git
     cd ../
 
 To maximize performance, make sure to build in *Release* mode. You can specify the build type by setting
 
-    catkin_make -DCMAKE_BUILD_TYPE=Release
+    colcon build -DCMAKE_BUILD_TYPE=Release
 
-or using the [Catkin Command Line Tools](http://catkin-tools.readthedocs.io/en/latest/index.html#)
-
-    catkin build darknet_ros -DCMAKE_BUILD_TYPE=Release
 
 Darknet on the CPU is fast (approximately 1.5 seconds on an Intel Core i7-6700HQ CPU @ 2.60GHz × 8) but it's like 500 times faster on GPU! You'll have to have an Nvidia GPU and you'll have to install CUDA. The CMakeLists.txt file automatically detects if you have CUDA installed or not. CUDA is a parallel computing platform and application programming interface (API) model created by Nvidia. If you do not have CUDA on your System the build process will switch to the CPU version of YOLO. If you are compiling with CUDA, you might receive the following build error:
 
@@ -87,7 +84,7 @@ This means that you need to check the compute capability (version) of your GPU.
 
 The yolo-voc.weights and tiny-yolo-voc.weights are downloaded automatically in the CMakeLists.txt file. If you need to download them again, go into the weights folder and download the two pre-trained weights from the COCO data set:
 
-    cd catkin_workspace/src/darknet_ros/darknet_ros/yolo_network_config/weights/
+    cd colcon_workspace/src/darknet_ros/darknet_ros/yolo_network_config/weights/
     wget http://pjreddie.com/media/files/yolov2.weights
     wget http://pjreddie.com/media/files/yolov2-tiny.weights
 
@@ -107,24 +104,12 @@ There are more pre-trained weights from different data sets reported [here](http
 
 In order to use your own detection objects you need to provide your weights and your cfg file inside the directories:
 
-    catkin_workspace/src/darknet_ros/darknet_ros/yolo_network_config/weights/
-    catkin_workspace/src/darknet_ros/darknet_ros/yolo_network_config/cfg/
-
-In addition, you need to create your config file for ROS where you define the names of the detection objects. You need to include it inside:
-
-    catkin_workspace/src/darknet_ros/darknet_ros/config/
-
-Then in the launch file you have to point to your new config file in the line:
-
-    <rosparam command="load" ns="darknet_ros" file="$(find darknet_ros)/config/your_config_file.yaml"/>
+    colcon_workspace/src/darknet_ros/darknet_ros/yolo_network_config/weights/
+    colcon_workspace/src/darknet_ros/darknet_ros/yolo_network_config/cfg/
 
 ### Unit Tests
 
-Run the unit tests using the [Catkin Command Line Tools](http://catkin-tools.readthedocs.io/en/latest/index.html#)
-
-    catkin build darknet_ros --no-deps --verbose --catkin-make-args run_tests
-
-You will see the image above popping up.
+** Currently disabled in ROS2 **
 
 ## Basic Usage
 
@@ -142,27 +127,27 @@ You can change the names and other parameters of the publishers, subscribers and
 
 #### Subscribed Topics
 
-* **`/camera_reading`** ([sensor_msgs/Image])
+* **`/camera_reading`** ([sensor_msgs/msg/Image])
 
     The camera measurements.
 
 #### Published Topics
 
-* **`object_detector`** ([std_msgs::Int8])
+* **`object_detector`** ([std_msgs/msg/Int8])
 
     Publishes the number of detected objects.
 
-* **`bounding_boxes`** ([darknet_ros_msgs::BoundingBoxes])
+* **`bounding_boxes`** ([darknet_ros_msgs/msg/BoundingBoxes])
 
     Publishes an array of bounding boxes that gives information of the position and size of the bounding box in pixel coordinates.
 
-* **`detection_image`** ([sensor_msgs::Image])
+* **`detection_image`** ([sensor_msgs/msg/Image])
 
     Publishes an image of the detection image including the bounding boxes.
 
 #### Actions
 
-* **`camera_reading`** ([sensor_msgs::Image])
+* **`camera_reading`** ([sensor_msgs/msg/Image])
 
     Sends an action with an image and the result is an array of bounding boxes.
 

diff --git a/darknet_ros/CMakeLists.txt b/darknet_ros/CMakeLists.txt
@@ -3,7 +3,6 @@ project(darknet_ros)
 
 set(CMAKE_CXX_STANDARD 17)
 
-
 # Define path of darknet folder here.
 find_path(DARKNET_PATH
   NAMES "README.md"
@@ -71,7 +70,7 @@ set(dependencies
 )
 
 # Enable OPENCV in darknet
-add_definitions(-DOPENCV)
+# add_definitions(-DOPENCV)
 add_definitions(-O4 -g)
 
 include_directories(
@@ -128,6 +127,8 @@ set(DARKNET_CUDA_FILES
     ${DARKNET_PATH}/src/convolutional_kernels.cu  ${DARKNET_PATH}/src/maxpool_layer_kernels.cu
 )
 
+set_source_files_properties(${PROJECT_LIB_FILES} PROPERTIES LANGUAGE CXX)
+
 if (CUDA_FOUND)
 
   link_directories(
@@ -156,10 +157,15 @@ if (CUDA_FOUND)
 
 else()
 
+  add_library(${PROJECT_NAME}_core_lib
+    ${DARKNET_CORE_FILES}
+  )
+
   add_library(${PROJECT_NAME}_lib
-    ${PROJECT_LIB_FILES} ${DARKNET_CORE_FILES}
+    ${PROJECT_LIB_FILES}
   )
   ament_target_dependencies(${PROJECT_NAME}_lib ${dependencies})
+  target_compile_definitions(${PROJECT_NAME}_lib PRIVATE -DOPENCV)
 
   add_executable(${PROJECT_NAME}
     src/yolo_object_detector_node.cpp
@@ -176,11 +182,13 @@ target_link_libraries(${PROJECT_NAME}_lib
   ${catkin_LIBRARIES}
   ${OpenCV_LIBS}
 )
+target_compile_definitions(${PROJECT_NAME}_lib PRIVATE -DOPENCV)
 
 target_link_libraries(${PROJECT_NAME}
   ${PROJECT_NAME}_lib
+  ${PROJECT_NAME}_core_lib
 )
-
+target_compile_definitions(${PROJECT_NAME} PRIVATE -DOPENCV)
 
 install(TARGETS ${PROJECT_NAME}_lib ${PROJECT_NAME}
   ARCHIVE DESTINATION lib

diff --git a/darknet_ros/include/darknet_ros/image_interface.h b/darknet_ros/include/darknet_ros/image_interface.h
@@ -9,6 +9,10 @@
 #ifndef IMAGE_INTERFACE_H
 #define IMAGE_INTERFACE_H
 
+#include "opencv2/highgui/highgui_c.h"
+#include "opencv2/imgproc/imgproc_c.h"
+#include "opencv2/core/version.hpp"
+
 #include "image.h"
 
 static float get_pixel(image m, int x, int y, int c);

diff --git a/darknet_ros/src/YoloObjectDetector.cpp b/darknet_ros/src/YoloObjectDetector.cpp
@@ -455,13 +455,32 @@ void *YoloObjectDetector::detectInThread()
   return 0;
 }
 
+
+void ipl_into_image_cp(IplImage* src, image im)
+{
+    unsigned char *data = (unsigned char *)src->imageData;
+    int h = src->height;
+    int w = src->width;
+    int c = src->nChannels;
+    int step = src->widthStep;
+    int i, j, k;
+
+    for(i = 0; i < h; ++i){
+        for(k= 0; k < c; ++k){
+            for(j = 0; j < w; ++j){
+                im.data[k*w*h + i*w + j] = data[i*step + j*c + k]/255.;
+            }
+        }
+    }
+}
+
 void *YoloObjectDetector::fetchInThread()
 {
   {
     std::shared_lock<std::shared_mutex> lock(mutexImageCallback_);
     IplImageWithHeader_ imageAndHeader = getIplImageWithHeader();
     IplImage* ROS_img = imageAndHeader.image;
-    ipl_into_image(ROS_img, buff_[buffIndex_]);
+    ipl_into_image_cp(ROS_img, buff_[buffIndex_]);
     headerBuff_[buffIndex_] = imageAndHeader.header;
     buffId_[buffIndex_] = actionId_;
   }
@@ -470,9 +489,54 @@ void *YoloObjectDetector::fetchInThread()
   return 0;
 }
 
+
+float get_pixel_cp(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];
+}
+
+int windows = 0;
+
+void show_image_cv_cp(image p, const char *name, IplImage *disp)
+{
+    int x,y,k;
+    if(p.c == 3) rgbgr_image(p);
+    //normalize_image(copy);
+
+    char buff[256];
+    //sprintf(buff, "%s (%d)", name, windows);
+    sprintf(buff, "%s", name);
+
+    int step = disp->widthStep;
+    cvNamedWindow(buff, CV_WINDOW_NORMAL); 
+    //cvMoveWindow(buff, 100*(windows%10) + 200*(windows/10), 100*(windows%10));
+    ++windows;
+    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_cp(p,x,y,k)*255);
+            }
+        }
+    }
+    if(0){
+        int w = 448;
+        int h = w*p.h/p.w;
+        if(h > 1000){
+            h = 1000;
+            w = h*p.w/p.h;
+        }
+        IplImage *buffer = disp;
+        disp = cvCreateImage(cvSize(w, h), buffer->depth, buffer->nChannels);
+        cvResize(buffer, disp, CV_INTER_LINEAR);
+        cvReleaseImage(&buffer);
+    }
+    cvShowImage(buff, disp);
+}
+
 void *YoloObjectDetector::displayInThread(void *ptr)
 {
-  show_image_cv(buff_[(buffIndex_ + 1)%3], "YOLO V3", ipl_);
+  show_image_cv_cp(buff_[(buffIndex_ + 1)%3], "YOLO V3", ipl_);
   int c = cv::waitKey(waitKeyDelay_);
   if (c != -1) c = c%256;
   if (c == 27) {
@@ -506,6 +570,26 @@ void *YoloObjectDetector::detectLoop(void *ptr)
   }
 }
 
+
+image **load_alphabet_with_file_cp(char *datafile) {
+  int i, j;
+  const int nsize = 8;
+  image **alphabets = (image**)calloc(nsize, sizeof(image));
+  char* labels = "/labels/%d_%d.png";
+  char * files = (char *) malloc(1 + strlen(datafile)+ strlen(labels) );
+  strcpy(files, datafile);
+  strcat(files, labels);
+  for(j = 0; j < nsize; ++j){
+    alphabets[j] = (image*)calloc(128, sizeof(image));
+    for(i = 32; i < 127; ++i){
+      char buff[256];
+      sprintf(buff, files, i, j);
+      alphabets[j][i] = load_image_color(buff, 0, 0);
+    }
+  }
+  return alphabets;
+}
+
 void YoloObjectDetector::setupNetwork(char *cfgfile, char *weightfile, char *datafile, float thresh,
                                       char **names, int classes,
                                       int delay, char *prefix, int avg_frames, float hier, int w, int h,
@@ -514,7 +598,7 @@ void YoloObjectDetector::setupNetwork(char *cfgfile, char *weightfile, char *dat
   demoPrefix_ = prefix;
   demoDelay_ = delay;
   demoFrame_ = avg_frames;
-  image **alphabet = load_alphabet_with_file(datafile);
+  image **alphabet = load_alphabet_with_file_cp(datafile);
   demoNames_ = names;
   demoAlphabet_ = alphabet;
   demoClasses_ = classes;
@@ -526,6 +610,32 @@ void YoloObjectDetector::setupNetwork(char *cfgfile, char *weightfile, char *dat
   set_batch_network(net_, 1);
 }
 
+void generate_image_cp(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_cp(p,x,y,k)*255);
+            }
+        }
+    }
+}
+
+image ipl_to_image_cp(IplImage* src)
+{
+    int h = src->height;
+    int w = src->width;
+    int c = src->nChannels;
+    image out = make_image(w, h, c);
+    ipl_into_image_cp(src, out);
+    return out;
+}
+
 void YoloObjectDetector::yolo()
 {
   const auto wait_duration = std::chrono::milliseconds(2000);
@@ -557,7 +667,7 @@ void YoloObjectDetector::yolo()
     std::shared_lock<std::shared_mutex> lock(mutexImageCallback_);
     IplImageWithHeader_ imageAndHeader = getIplImageWithHeader();
     IplImage* ROS_img = imageAndHeader.image;
-    buff_[0] = ipl_to_image(ROS_img);
+    buff_[0] = ipl_to_image_cp(ROS_img);
     headerBuff_[0] = imageAndHeader.header;
   }
   buff_[1] = copy_image(buff_[0]);
@@ -593,7 +703,7 @@ void YoloObjectDetector::yolo()
       if (viewImage_) {
         displayInThread(0);
       } else {
-        generate_image(buff_[(buffIndex_ + 1)%3], ipl_);
+        generate_image_cp(buff_[(buffIndex_ + 1)%3], ipl_);
       }
       publishInThread();
     } else {
@@ -613,7 +723,8 @@ void YoloObjectDetector::yolo()
 
 IplImageWithHeader_ YoloObjectDetector::getIplImageWithHeader()
 {
-  IplImage* ROS_img = new IplImage(camImageCopy_);
+  IplImage* ROS_img = new IplImage();
+  *ROS_img = cvIplImage(camImageCopy_);
   IplImageWithHeader_ header = {.image = ROS_img, .header = imageHeader_};
   return header;
 }

diff --git a/darknet_ros/src/image_interface.c b/darknet_ros/src/image_interface.c
@@ -17,13 +17,13 @@ static float get_pixel(image m, int x, int y, int c)
 image **load_alphabet_with_file(char *datafile) {
   int i, j;
   const int nsize = 8;
-  image **alphabets = calloc(nsize, sizeof(image));
+  image **alphabets = (image**)calloc(nsize, sizeof(image));
   char* labels = "/labels/%d_%d.png";
   char * files = (char *) malloc(1 + strlen(datafile)+ strlen(labels) );
   strcpy(files, datafile);
   strcat(files, labels);
   for(j = 0; j < nsize; ++j){
-    alphabets[j] = calloc(128, sizeof(image));
+    alphabets[j] = (image*)calloc(128, sizeof(image));
     for(i = 32; i < 127; ++i){
       char buff[256];
       sprintf(buff, files, i, j);