From c25004d5de5b95ccb8d34ca4d562143f3846bb79 Mon Sep 17 00:00:00 2001 From: JiaShen Wang Date: Mon, 20 May 2019 01:30:45 +0900 Subject: [PATCH 1/3] Update Submodules --- darknet | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/darknet b/darknet index 508381b37..c4b1df3a0 160000 --- a/darknet +++ b/darknet @@ -1 +1 @@ -Subproject commit 508381b37fe75e0e1a01bcb2941cb0b31eb0e4c9 +Subproject commit c4b1df3a08f7a9314ca0263c4140c6fecd8af488 From fcb2a57ba44f4fee21740d0343b5b05942c15a32 Mon Sep 17 00:00:00 2001 From: Jason Wang Date: Sat, 9 Nov 2019 21:41:49 -0500 Subject: [PATCH 2/3] Made fixes --- darknet_ros/CMakeLists.txt | 2 +- .../darknet_ros/YoloObjectDetector.hpp | 18 ++---- darknet_ros/launch/darknet_ros.launch | 2 +- darknet_ros/src/YoloObjectDetector.cpp | 60 +++++++------------ 4 files changed, 28 insertions(+), 54 deletions(-) diff --git a/darknet_ros/CMakeLists.txt b/darknet_ros/CMakeLists.txt index f7032f41b..520aa3e9f 100644 --- a/darknet_ros/CMakeLists.txt +++ b/darknet_ros/CMakeLists.txt @@ -3,7 +3,7 @@ project(darknet_ros) # Set c++11 cmake flags set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}") -set(CMAKE_C_FLAGS "-Wall -Wno-unused-result -Wno-unknown-pragmas -Wno-unused-variable -Wfatal-errors -fPIC ${CMAKE_C_FLAGS}") +set(CMAKE_C_FLAGS "-Wall -Wno-unused-result -Wno-unknown-pragmas -Wno-unused-variable -Wfatal-errors -Wno-deprecated-declarations -fPIC ${CMAKE_C_FLAGS}") # Define path of darknet folder here. find_path(DARKNET_PATH diff --git a/darknet_ros/include/darknet_ros/YoloObjectDetector.hpp b/darknet_ros/include/darknet_ros/YoloObjectDetector.hpp index cce65049b..5288f0140 100644 --- a/darknet_ros/include/darknet_ros/YoloObjectDetector.hpp +++ b/darknet_ros/include/darknet_ros/YoloObjectDetector.hpp @@ -214,23 +214,17 @@ class YoloObjectDetector int actionId_; boost::shared_mutex mutexActionStatus_; - // double getWallTime(); - int sizeNetwork(network *net); void rememberNetwork(network *net); detection *avgPredictions(network *net, int *nboxes); - void *detectInThread(); - - void *fetchInThread(); - - void *displayInThread(void *ptr); + void detectInThread(); - void *displayLoop(void *ptr); + void fetchInThread(); - void *detectLoop(void *ptr); + void displayInThread(); void setupNetwork(char *cfgfile, char *weightfile, char *datafile, float thresh, char **names, int classes, @@ -241,11 +235,11 @@ class YoloObjectDetector IplImageWithHeader_ getIplImageWithHeader(); - bool getImageStatus(void); + bool getImageStatus(); - bool isNodeRunning(void); + bool isNodeRunning(); - void *publishInThread(); + void publishInThread(); }; } /* namespace darknet_ros*/ diff --git a/darknet_ros/launch/darknet_ros.launch b/darknet_ros/launch/darknet_ros.launch index 76bacfd91..98e43719f 100644 --- a/darknet_ros/launch/darknet_ros.launch +++ b/darknet_ros/launch/darknet_ros.launch @@ -17,7 +17,7 @@ - + diff --git a/darknet_ros/src/YoloObjectDetector.cpp b/darknet_ros/src/YoloObjectDetector.cpp index 4181e71a4..64c667bc9 100644 --- a/darknet_ros/src/YoloObjectDetector.cpp +++ b/darknet_ros/src/YoloObjectDetector.cpp @@ -205,7 +205,6 @@ void YoloObjectDetector::cameraCallback(const sensor_msgs::ImageConstPtr& msg) frameWidth_ = cam_image->image.size().width; frameHeight_ = cam_image->image.size().height; } - return; } void YoloObjectDetector::checkForObjectsActionGoalCB() @@ -241,7 +240,6 @@ void YoloObjectDetector::checkForObjectsActionGoalCB() frameWidth_ = cam_image->image.size().width; frameHeight_ = cam_image->image.size().height; } - return; } void YoloObjectDetector::checkForObjectsActionPreemptCB() @@ -270,15 +268,6 @@ bool YoloObjectDetector::publishDetectionImage(const cv::Mat& detectionImage) return true; } -// double YoloObjectDetector::getWallTime() -// { -// struct timeval time; -// if (gettimeofday(&time, NULL)) { -// return 0; -// } -// return (double) time.tv_sec + (double) time.tv_usec * .000001; -// } - int YoloObjectDetector::sizeNetwork(network *net) { int i; @@ -311,7 +300,7 @@ detection *YoloObjectDetector::avgPredictions(network *net, int *nboxes) int count = 0; fill_cpu(demoTotal_, 0, avg_, 1); for(j = 0; j < demoFrame_; ++j){ - axpy_cpu(demoTotal_, 1./demoFrame_, predictions_[j], 1, avg_, 1); + axpy_cpu(demoTotal_, 1.f/demoFrame_, predictions_[j], 1, avg_, 1); } for(i = 0; i < net->n; ++i){ layer l = net->layers[i]; @@ -324,7 +313,7 @@ detection *YoloObjectDetector::avgPredictions(network *net, int *nboxes) return dets; } -void *YoloObjectDetector::detectInThread() +void YoloObjectDetector::detectInThread() { running_ = 1; float nms = .4; @@ -401,10 +390,9 @@ void *YoloObjectDetector::detectInThread() free_detections(dets, nboxes); demoIndex_ = (demoIndex_ + 1) % demoFrame_; running_ = 0; - return 0; } -void *YoloObjectDetector::fetchInThread() +void YoloObjectDetector::fetchInThread() { IplImageWithHeader_ imageAndHeader = getIplImageWithHeader(); IplImage* ROS_img = imageAndHeader.image; @@ -416,17 +404,16 @@ void *YoloObjectDetector::fetchInThread() } rgbgr_image(buff_[buffIndex_]); letterbox_image_into(buff_[buffIndex_], net_->w, net_->h, buffLetter_[buffIndex_]); - return 0; } -void *YoloObjectDetector::displayInThread(void *ptr) +void YoloObjectDetector::displayInThread() { show_image_cv(buff_[(buffIndex_ + 1)%3], "YOLO V3", ipl_); int c = cvWaitKey(waitKeyDelay_); if (c != -1) c = c%256; if (c == 27) { demoDone_ = 1; - return 0; + return; } else if (c == 82) { demoThresh_ += .02; } else if (c == 84) { @@ -438,21 +425,6 @@ void *YoloObjectDetector::displayInThread(void *ptr) demoHier_ -= .02; if(demoHier_ <= .0) demoHier_ = .0; } - return 0; -} - -void *YoloObjectDetector::displayLoop(void *ptr) -{ - while (1) { - displayInThread(0); - } -} - -void *YoloObjectDetector::detectLoop(void *ptr) -{ - while (1) { - detectInThread(); - } } void YoloObjectDetector::setupNetwork(char *cfgfile, char *weightfile, char *datafile, float thresh, @@ -534,10 +506,20 @@ void YoloObjectDetector::yolo() fetch_thread = std::thread(&YoloObjectDetector::fetchInThread, this); detect_thread = std::thread(&YoloObjectDetector::detectInThread, this); if (!demoPrefix_) { - fps_ = 1./(what_time_is_it_now() - demoTime_); + fps_ = 1.f/(what_time_is_it_now() - demoTime_); demoTime_ = what_time_is_it_now(); if (viewImage_) { - displayInThread(0); + displayInThread(); + } else { + const auto& p = buff_[(buffIndex_ + 1)%3]; + int step = ipl_->widthStep; + for(int y = 0; y < p.h; ++y){ + for(int x = 0; x < p.w; ++x){ + for(int k= 0; k < p.c; ++k){ + ipl_->imageData[y*step + x*p.c + k] = (unsigned char)(p.data[k*p.h*p.w + y*p.w + x]*255); + } + } + } } publishInThread(); } else { @@ -563,19 +545,19 @@ IplImageWithHeader_ YoloObjectDetector::getIplImageWithHeader() return header; } -bool YoloObjectDetector::getImageStatus(void) +bool YoloObjectDetector::getImageStatus() { boost::shared_lock lock(mutexImageStatus_); return imageStatus_; } -bool YoloObjectDetector::isNodeRunning(void) +bool YoloObjectDetector::isNodeRunning() { boost::shared_lock lock(mutexNodeStatus_); return isNodeRunning_; } -void *YoloObjectDetector::publishInThread() +void YoloObjectDetector::publishInThread() { // Publish image. cv::Mat cvImage = cv::cvarrToMat(ipl_); @@ -640,8 +622,6 @@ void *YoloObjectDetector::publishInThread() rosBoxes_[i].clear(); rosBoxCounter_[i] = 0; } - - return 0; } From 9b6ea501c01c141a1ea2a0c997686943ddeb95e9 Mon Sep 17 00:00:00 2001 From: Jason Wang Date: Fri, 15 Nov 2019 16:43:57 -0500 Subject: [PATCH 3/3] Bad --- darknet | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/darknet b/darknet index c4b1df3a0..508381b37 160000 --- a/darknet +++ b/darknet @@ -1 +1 @@ -Subproject commit c4b1df3a08f7a9314ca0263c4140c6fecd8af488 +Subproject commit 508381b37fe75e0e1a01bcb2941cb0b31eb0e4c9