From 7cb0659c6a1577cd31be265bc79fa67595dc46a8 Mon Sep 17 00:00:00 2001 From: scirocco Date: Tue, 28 Jan 2020 14:13:11 +0800 Subject: [PATCH 01/12] Init Binocular --- src/binocular/src/Binocular.cpp | 61 +++++++++++++++++++++++++++++++++ 1 file changed, 61 insertions(+) create mode 100644 src/binocular/src/Binocular.cpp diff --git a/src/binocular/src/Binocular.cpp b/src/binocular/src/Binocular.cpp new file mode 100644 index 0000000..c963f0d --- /dev/null +++ b/src/binocular/src/Binocular.cpp @@ -0,0 +1,61 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace seuimage; +using namespace common; + +bool MallocMemory(); +void NetworkInit(std::string cfg, std::string wts); +bool BinocularInit(); +bool CameraInit(); +void Run(const ros::TimerEvent &event); + +void ImagePublish(const cv::Mat &bgr); +bool SendTypeService(SetInt::Request &req, SetInt::Response &res); + +const int width = 640; // 仅发布左眼 +const int height = 400; +CudaMatC srcMat; +CudaMatC rgbMat; +CudaMatC dstMat; +CudaMatC hsvMat; +CudaMatC relMat; +CudaMatC netMat; +CudaMatF netfMat; +CudaMatF yoloMat; +network yolo; + +ObjectFilter ballFilter, postFilter; +std::atomic_int send_type(0); +std::shared_ptr camera; +ros::Publisher imagePublisher; +ros::Publisher resultPublisher; + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "binocular"); + ros::NodeHandle node; + if (!CameraInit()) + { + ROS_ERROR("failed in opening binocular camera"); + return 1; + } + BinocularInit(); + imagePublisher = node.advertise("/result/binocular/compressed", 1); + resultPublisher = node.advertise("/result/binocular/imgproc", 1); + ros::ServiceServer typeServer = node.advertiseService("/setting/sendtype", SendTypeService); + //camera->Run(); + ros::Timer timer = node.createTimer(ros::Duration(0.05), Run); + ros::spin(); + //camera->Close(); + return 0; +} -- Gitee From 150ade5ae2cc4890998b0ef2f682aa75b79bd4bd Mon Sep 17 00:00:00 2001 From: scirocco Date: Sat, 1 Feb 2020 12:36:41 +0800 Subject: [PATCH 02/12] copy from vision --- .gitignore | 5 +- src/binocular/src/Binocular.cpp | 319 +++++++++++++++++++++++++++----- src/binocular/src/Binocular.hpp | 42 +++++ 3 files changed, 322 insertions(+), 44 deletions(-) create mode 100644 src/binocular/src/Binocular.hpp diff --git a/.gitignore b/.gitignore index 31ffce5..11d4f8e 100644 --- a/.gitignore +++ b/.gitignore @@ -1,4 +1,7 @@ build devel .catkin_workspace -src/CMakeLists.txt \ No newline at end of file +src/CMakeLists.txt +src/common/include/common/ImageResult.h +src/common/include/common/PlayerInfo.h +src/common/include/common/SetInt.h diff --git a/src/binocular/src/Binocular.cpp b/src/binocular/src/Binocular.cpp index c963f0d..f267b1c 100644 --- a/src/binocular/src/Binocular.cpp +++ b/src/binocular/src/Binocular.cpp @@ -1,44 +1,4 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -using namespace seuimage; -using namespace common; - -bool MallocMemory(); -void NetworkInit(std::string cfg, std::string wts); -bool BinocularInit(); -bool CameraInit(); -void Run(const ros::TimerEvent &event); - -void ImagePublish(const cv::Mat &bgr); -bool SendTypeService(SetInt::Request &req, SetInt::Response &res); - -const int width = 640; // 仅发布左眼 -const int height = 400; -CudaMatC srcMat; -CudaMatC rgbMat; -CudaMatC dstMat; -CudaMatC hsvMat; -CudaMatC relMat; -CudaMatC netMat; -CudaMatF netfMat; -CudaMatF yoloMat; -network yolo; - -ObjectFilter ballFilter, postFilter; -std::atomic_int send_type(0); -std::shared_ptr camera; -ros::Publisher imagePublisher; -ros::Publisher resultPublisher; +#include "Binocular.hpp" int main(int argc, char **argv) { @@ -53,9 +13,282 @@ int main(int argc, char **argv) imagePublisher = node.advertise("/result/binocular/compressed", 1); resultPublisher = node.advertise("/result/binocular/imgproc", 1); ros::ServiceServer typeServer = node.advertiseService("/setting/sendtype", SendTypeService); - //camera->Run(); + camera->Run(); ros::Timer timer = node.createTimer(ros::Duration(0.05), Run); ros::spin(); - //camera->Close(); + camera->Close(); return 0; } + +void Run(const ros::TimerEvent &event) +{ + if (!ros::ok()) + { + return; + } + bool ret; + uint32_t image_time; + ret = srcMat.upload(camera->GetData(image_time)); + + if (!ret) + { + ROS_ERROR("upload error"); + return; + } + + ret = YUV422ToRGB(srcMat, rgbMat); + ret = Resize(rgbMat, dstMat); + ret = dstMat.copyTo(relMat); + ret = RGBToHSV(relMat, netMat); + + if (!ret) + { + ROS_ERROR("Resize to net error"); + return; + } + + ret = RGB8uTo32fNorm(netMat, netfMat); + if (!ret) + { + ROS_ERROR("RGB8uTo32fNorm error"); + return; + } + + ret = PackedToPlanar(netfMat, yoloMat); + if (!ret) + { + ROS_ERROR("PackedToPlanar error"); + return; + } + + layer l = yolo.layers[yolo.n - 1]; + network_predict1(yolo, yoloMat.data()); + int nboxes = 0; + float nms = 0.45; + detection *dets = get_network_boxes(&yolo, width, height, + 0.5, 0.5, 0, 1, &nboxes, 0); + + if (nms) + { + do_nms_sort(dets, nboxes, l.classes, nms); + } + + std::vector ball_dets, post_dets; + ball_dets.clear(); + post_dets.clear(); + + for (int i = 0; i < nboxes; i++) + { + int id = 0; + for (int j = 0; j < l.classes; j++) + { + if (dets[i].prob[j] > dets[i].prob[id]) + id = j; + } + if (id == 0) + { + if (dets[i].prob[id] >= ballFilter.min_prob) + { + int bx = (dets[i].bbox.x - dets[i].bbox.w / 2.0) * width; + int by = (dets[i].bbox.y - dets[i].bbox.h / 2.0) * height; + int bw = dets[i].bbox.w * width, bh = dets[i].bbox.h * height + 1; + ball_dets.push_back(DetObject(0, dets[i].prob[id], bx, by, bw, bh)); + } + } + else if (id == 1) + { + if (dets[i].prob[id] >= postFilter.min_prob) + { + int px = (dets[i].bbox.x - dets[i].bbox.w / 2.0) * width; + int py = (dets[i].bbox.y - dets[i].bbox.h / 2.0) * height; + int pw = dets[i].bbox.w * width, ph = dets[i].bbox.h * height; + post_dets.push_back(DetObject(1, dets[i].prob[id], px, py, pw, ph)); + } + } + } + std::sort(ball_dets.rbegin(), ball_dets.rend()); + std::sort(post_dets.rbegin(), post_dets.rend()); + free_detections(dets, nboxes); + + common::ImageResult imgResult; + imgResult.stamp = image_time; + if (!ball_dets.empty()) + { + imgResult.has_ball = true; + imgResult.ball.x = ball_dets[0].x + ball_dets[0].w / 2; + imgResult.ball.y = ball_dets[0].y + ball_dets[0].h; + } + for (uint32_t i = 0; i < post_dets.size() && i < 2; i++) + { + imgResult.has_post = true; + ObjInfo tmp; + tmp.x = ball_dets[i].x + ball_dets[i].w / 2; + tmp.y = ball_dets[i].y + ball_dets[i].h / 3 * 2; + imgResult.posts.push_back(tmp); + } + resultPublisher.publish(imgResult); + + if (send_type > 0) + { + cv::Mat rgb(height, width, CV_8UC3); + if (!dstMat.download(rgb)) + { + ROS_ERROR("download error"); + return; + } + cv::Mat bgr; + cv::cvtColor(rgb, bgr, CV_RGB2BGR); + + if (send_type > 1) + { + if (!ball_dets.empty()) + { + cv::rectangle(bgr, cv::Point(ball_dets[0].x, ball_dets[0].y), cv::Point(ball_dets[0].x + ball_dets[0].w, ball_dets[0].y + ball_dets[0].h), cv::Scalar(255, 0, 0), 2); + } + + for (uint32_t i = 0; i < post_dets.size(); i++) + { + if (i >= 2) + { + break; + } + + cv::rectangle(bgr, cv::Point(post_dets[i].x, post_dets[i].y), cv::Point(post_dets[i].x + post_dets[i].w, post_dets[i].y + post_dets[i].h), cv::Scalar(0, 0, 255), 2); + } + } + ImagePublish(bgr); + } +} + +bool CameraInit() +{ + std::string cfgfile; + try + { + ros::param::get("camera_config_file", cfgfile); + } + catch (ros::InvalidNameException &e) + { + ROS_ERROR("%s", e.what()); + return false; + } + camera = std::make_shared(cfgfile); + camera->Open(); + if (camera->Available()) + return true; + camera = std::make_shared(0); + camera->Open(); + if (camera->Available()) + return true; + return false; +} + +bool BinocularInit() +{ + std::string visionfile; + try + { + ros::param::get("vision_file", visionfile); + } + catch (ros::InvalidNameException &e) + { + ROS_ERROR("%s", e.what()); + return false; + } + + std::string vision_root = visionfile.substr(0, visionfile.find_last_of("/") + 1); + std::string yolo_cfg, weights; + if (visionfile.empty()) + return false; + common::bpt::ptree pt; + if (!common::get_tree_from_file(visionfile, pt)) + { + ROS_ERROR("parse file: [%s] failed", visionfile.c_str()); + return false; + } + yolo_cfg = vision_root + pt.get("cfg_file"); + weights = vision_root + pt.get("weights_file"); + ballFilter.min_prob = pt.get("dets.prob.ball"); + ballFilter.r_range = get_config_vector(pt, "dets.boxes.ball.ratio"); + ballFilter.w_range = get_config_vector(pt, "dets.boxes.ball.width"); + ballFilter.h_range = get_config_vector(pt, "dets.boxes.ball.height"); + postFilter.min_prob = pt.get("dets.prob.post"); + postFilter.r_range = get_config_vector(pt, "dets.boxes.post.ratio"); + postFilter.w_range = get_config_vector(pt, "dets.boxes.post.width"); + postFilter.h_range = get_config_vector(pt, "dets.boxes.post.height"); + + yolo.gpu_index = 0; + yolo = parse_network_cfg_custom((char *)(yolo_cfg.c_str()), 1, 0); + load_weights(&yolo, const_cast((char *)(weights.c_str()))); + set_batch_network(&yolo, 1); + fuse_conv_batchnorm(yolo); + calculate_binary_weights(yolo); + srand(2222222); + cudaSetDevice(0); + + if (!MallocMemory()) + { + ROS_ERROR("MallocMemory failed"); + return false; + } + return true; +} + +void ImagePublish(const cv::Mat &bgr) +{ + sensor_msgs::CompressedImage image; + std::vector buf; + cv::imencode(".jpg", bgr, buf); + image.header.stamp = ros::Time::now(); + image.header.frame_id = "vision"; + image.format = "jpeg"; + image.data.resize(buf.size()); + memcpy(&image.data[0], &(buf[0]), buf.size()); + imagePublisher.publish(image); +} + +bool MallocMemory() +{ + if (camera.get() == nullptr) + return false; + bool ret; + ret = srcMat.create(camera->Height(), camera->Width(), GET_CHANNELS(camera->Type())); + if (!ret) + return false; + + ret = rgbMat.create(camera->Height(), camera->Width(), 3); + if (!ret) + return false; + + ret = dstMat.create(height, width, 3); + if (!ret) + return false; + + ret = relMat.create(height, width, 3); + if (!ret) + return false; + + ret = hsvMat.create(height, width, 3); + if (!ret) + return false; + + ret = netMat.create(yolo.h, yolo.w, 3); + if (!ret) + return false; + + ret = netfMat.create(yolo.h, yolo.w, 3); + if (!ret) + return false; + + ret = yoloMat.create(yolo.h, yolo.w, 3); + if (!ret) + return false; + + return true; +} + +bool SendTypeService(SetInt::Request &req, SetInt::Response &res) +{ + send_type = req.number; + return true; +} \ No newline at end of file diff --git a/src/binocular/src/Binocular.hpp b/src/binocular/src/Binocular.hpp new file mode 100644 index 0000000..870105b --- /dev/null +++ b/src/binocular/src/Binocular.hpp @@ -0,0 +1,42 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace seuimage; +using namespace common; + +bool MallocMemory(); +void NetworkInit(std::string cfg, std::string wts); +bool BinocularInit(); +bool CameraInit(); +void Run(const ros::TimerEvent &event); + +void ImagePublish(const cv::Mat &bgr); +bool SendTypeService(SetInt::Request &req, SetInt::Response &res); + +const int width = 640; // 仅发布左眼 +const int height = 400; +CudaMatC srcMat; +CudaMatC rgbMat; +CudaMatC dstMat; +CudaMatC hsvMat; +CudaMatC relMat; +CudaMatC netMat; +CudaMatF netfMat; +CudaMatF yoloMat; +network yolo; + +ObjectFilter ballFilter, postFilter; +std::atomic_int send_type(0); +std::shared_ptr camera; +ros::Publisher imagePublisher; +ros::Publisher resultPublisher; + -- Gitee From 46fcdc22141e59f19c01bc60912c7bb005deeeb3 Mon Sep 17 00:00:00 2001 From: scirocco Date: Fri, 7 Feb 2020 22:28:23 +0800 Subject: [PATCH 03/12] test binocular --- .gitignore | 3 - src/binocular/src/Binocular.cpp | 96 ++++++++++++++++--------- src/binocular/src/Binocular.hpp | 4 +- src/binocular/src/MYNTeye.cpp | 91 +++++++++++++++++++++++ src/binocular/src/MYNTeye.hpp | 41 +++++++++++ src/common/include/common/ImageResult.h | 0 src/common/include/common/PlayerInfo.h | 0 src/common/include/common/SetInt.h | 0 8 files changed, 197 insertions(+), 38 deletions(-) create mode 100644 src/binocular/src/MYNTeye.cpp create mode 100644 src/binocular/src/MYNTeye.hpp create mode 100644 src/common/include/common/ImageResult.h create mode 100644 src/common/include/common/PlayerInfo.h create mode 100644 src/common/include/common/SetInt.h diff --git a/.gitignore b/.gitignore index 11d4f8e..47b63fc 100644 --- a/.gitignore +++ b/.gitignore @@ -2,6 +2,3 @@ build devel .catkin_workspace src/CMakeLists.txt -src/common/include/common/ImageResult.h -src/common/include/common/PlayerInfo.h -src/common/include/common/SetInt.h diff --git a/src/binocular/src/Binocular.cpp b/src/binocular/src/Binocular.cpp index f267b1c..e2b8b52 100644 --- a/src/binocular/src/Binocular.cpp +++ b/src/binocular/src/Binocular.cpp @@ -7,7 +7,6 @@ int main(int argc, char **argv) if (!CameraInit()) { ROS_ERROR("failed in opening binocular camera"); - return 1; } BinocularInit(); imagePublisher = node.advertise("/result/binocular/compressed", 1); @@ -28,17 +27,23 @@ void Run(const ros::TimerEvent &event) } bool ret; uint32_t image_time; - ret = srcMat.upload(camera->GetData(image_time)); + ret = srcMat.upload(camera->GetData(image_time).color); if (!ret) { ROS_ERROR("upload error"); return; } - - ret = YUV422ToRGB(srcMat, rgbMat); - ret = Resize(rgbMat, dstMat); - ret = dstMat.copyTo(relMat); + if (GET_CHANNELS(camera->Type()) == 2) // YUYV + { + ret = YUV422ToRGB(srcMat, rgbMat); + ret = Resize(rgbMat, dstMat); + ret = dstMat.copyTo(relMat); + } + else + { + ROS_ERROR("channel error"); + } ret = RGBToHSV(relMat, netMat); if (!ret) @@ -143,42 +148,29 @@ void Run(const ros::TimerEvent &event) { if (!ball_dets.empty()) { - cv::rectangle(bgr, cv::Point(ball_dets[0].x, ball_dets[0].y), cv::Point(ball_dets[0].x + ball_dets[0].w, ball_dets[0].y + ball_dets[0].h), cv::Scalar(255, 0, 0), 2); + cv::Rect ball_roi(ball_dets[0].x, ball_dets[0].y, ball_dets[0].w, ball_dets[0].h); + cv::rectangle(bgr, ball_roi, cv::Scalar(255, 0, 0), 2); + int ball_dist = CountDistance(camera->GetData(image_time).depth, ball_roi); + ROS_INFO("ball_dist=%d", ball_dist); // TODO: 接入接口而不是什么INFO！ } - for (uint32_t i = 0; i < post_dets.size(); i++) + for (uint32_t i = 0; i < post_dets.size() && i < 2; i++) { - if (i >= 2) - { - break; - } - - cv::rectangle(bgr, cv::Point(post_dets[i].x, post_dets[i].y), cv::Point(post_dets[i].x + post_dets[i].w, post_dets[i].y + post_dets[i].h), cv::Scalar(0, 0, 255), 2); + cv::Rect post_roi(post_dets[0].x, post_dets[0].y, post_dets[0].w, post_dets[0].h); + cv::rectangle(bgr, post_roi, cv::Scalar(0, 0, 255), 2); + int post_dist = CountDistance(camera->GetData(image_time).depth, post_roi); + ROS_INFO("post_dist=%d", post_dist); // TODO: 接入接口而不是什么INFO！ } } + cv::imshow("binocular", bgr); ImagePublish(bgr); } } bool CameraInit() { - std::string cfgfile; - try - { - ros::param::get("camera_config_file", cfgfile); - } - catch (ros::InvalidNameException &e) - { - ROS_ERROR("%s", e.what()); - return false; - } - camera = std::make_shared(cfgfile); - camera->Open(); - if (camera->Available()) - return true; - camera = std::make_shared(0); - camera->Open(); - if (camera->Available()) + camera = std::make_shared(); + if (camera->Open()) return true; return false; } @@ -234,13 +226,49 @@ bool BinocularInit() return true; } +int CountDistance(cv::Mat &mat, cv::Rect roi) +{ + cv::Mat depth_cop = mat(roi).clone(); + + int count[2] = {0, 0}; + + std::vector depth_vector; + depth_vector.clear(); + + ushort *p = depth_cop.ptr(); + for (size_t i = 0; i < depth_cop.total(); i++) + { + if (*p != 0) + depth_vector.push_back((float)*p); + p++; + } + + cv::Mat labels; + std::vector centers; + if (depth_vector.size() < 2) // N < K 时 + { + return 0; + } + else + { + // kmeans 只接收CV_32F + cv::kmeans(depth_vector, 2, labels, cv::TermCriteria(cv::TermCriteria::EPS + cv::TermCriteria::COUNT, 5, 1.0), 3, cv::KMEANS_PP_CENTERS, centers); + + for (size_t i = 0; i < depth_vector.size(); i++) + { + count[labels.data[i]]++; + } + return centers[0] > centers[1] ? centers[0] : centers[1]; + } +} + void ImagePublish(const cv::Mat &bgr) { sensor_msgs::CompressedImage image; std::vector buf; cv::imencode(".jpg", bgr, buf); image.header.stamp = ros::Time::now(); - image.header.frame_id = "vision"; + image.header.frame_id = "binocular"; image.format = "jpeg"; image.data.resize(buf.size()); memcpy(&image.data[0], &(buf[0]), buf.size()); @@ -252,11 +280,11 @@ bool MallocMemory() if (camera.get() == nullptr) return false; bool ret; - ret = srcMat.create(camera->Height(), camera->Width(), GET_CHANNELS(camera->Type())); + ret = srcMat.create(camera->height, camera->width, GET_CHANNELS(camera->Type())); if (!ret) return false; - ret = rgbMat.create(camera->Height(), camera->Width(), 3); + ret = rgbMat.create(camera->height, camera->width, 3); if (!ret) return false; diff --git a/src/binocular/src/Binocular.hpp b/src/binocular/src/Binocular.hpp index 870105b..3d4def4 100644 --- a/src/binocular/src/Binocular.hpp +++ b/src/binocular/src/Binocular.hpp @@ -9,6 +9,7 @@ #include #include #include +#include "MYNTeye.hpp" using namespace seuimage; using namespace common; @@ -18,6 +19,7 @@ void NetworkInit(std::string cfg, std::string wts); bool BinocularInit(); bool CameraInit(); void Run(const ros::TimerEvent &event); +int CountDistance(cv::Mat &mat, cv::Rect roi); void ImagePublish(const cv::Mat &bgr); bool SendTypeService(SetInt::Request &req, SetInt::Response &res); @@ -36,7 +38,7 @@ network yolo; ObjectFilter ballFilter, postFilter; std::atomic_int send_type(0); -std::shared_ptr camera; +std::shared_ptr camera; ros::Publisher imagePublisher; ros::Publisher resultPublisher; diff --git a/src/binocular/src/MYNTeye.cpp b/src/binocular/src/MYNTeye.cpp new file mode 100644 index 0000000..0c8eade --- /dev/null +++ b/src/binocular/src/MYNTeye.cpp @@ -0,0 +1,91 @@ +#include "MYNTeye.hpp" + +using namespace mynteye; + +MYNTeye::MYNTeye() +{ + raw_.color = cv::Mat(height, width, type); + raw_.depth = cv::Mat(height, width, depth_type); + alive_ = false; +} + +MYNTeye::~MYNTeye() +{ + if (td_.joinable()) + { + td_.join(); + } +} + +bool MYNTeye::Open() +{ + Context context; + auto &&devices = context.devices(); + if (!devices.size()) + { + return false; + } + + auto &&device = devices[0]; + api_ = API::Create(device); + if (!api_) + { + return false; + } + + const StreamRequest request(2 * width, height, Format::YUYV, 30); + api_->ConfigStreamRequest(request); + api_->EnableStreamData(Stream::LEFT_RECTIFIED); + api_->EnableStreamData(Stream::RIGHT_RECTIFIED); + api_->SetDisparityComputingMethodType(DisparityComputingMethod::BM); + api_->EnableStreamData(Stream::DEPTH); + // auto exposure + api_->SetOptionValue(Option::EXPOSURE_MODE, 0); + // max_gain: range [0,255], default 8 + api_->SetOptionValue(Option::MAX_GAIN, 8); + // max_exposure_time: range [0,655], default 333 + api_->SetOptionValue(Option::MAX_EXPOSURE_TIME, 333); + // desired_brightness: range [1,255], default 122 + api_->SetOptionValue(Option::DESIRED_BRIGHTNESS, 122); + // min_exposure_time: range [0,655], default 0 + api_->SetOptionValue(Option::MIN_EXPOSURE_TIME, 0); + + api_->Start(Source::VIDEO_STREAMING); + + return true; +} + +bool MYNTeye::Close() +{ + alive_ = false; + return true; +} + +void MYNTeye::Run() +{ + alive_ = true; + td_ = std::move(std::thread([this]() { + while (alive_) + { + api_->WaitForStreams(); // TODO: 这行应该放在哪？ + auto &&left_data = api_->GetStreamData(Stream::LEFT_RECTIFIED); + auto &&depth_data = api_->GetStreamData(Stream::DEPTH); + if (!left_data.frame.empty() && !depth_data.frame.empty()) // && !right_data.frame.empty()) + { + raw_mutex_.lock(); + time_ = get_clock(); + memcpy(raw_.color.data, left_data.frame.data, width * height); + memcpy(raw_.depth.data, depth_data.frame.data, width * height); + raw_mutex_.unlock(); + } + usleep(100); + } + })); +} + +MYNTdata &MYNTeye::GetData(uint32_t &t) +{ + std::lock_guard lk(raw_mutex_); + t = time_; + return raw_; +} \ No newline at end of file diff --git a/src/binocular/src/MYNTeye.hpp b/src/binocular/src/MYNTeye.hpp new file mode 100644 index 0000000..983c9c3 --- /dev/null +++ b/src/binocular/src/MYNTeye.hpp @@ -0,0 +1,41 @@ +#ifndef __MYNTEYE_HPP +#define __MYNTEYE_HPP + +#include +#include +#include "mynteye/api/api.h" +#include "mynteye/device/context.h" +#define GET_CHANNELS(t) (((t)>>CV_CN_SHIFT)+1) + +struct MYNTdata +{ + cv::Mat color; + cv::Mat depth; +}; + +class MYNTeye +{ +public: + MYNTeye() = default; + ~MYNTeye(); + bool Open(); + bool Close(); + // bool Available(); 好像没什么意义，Open()返回的就够了 + int Type(); + void Run(); + MYNTdata &GetData(uint32_t &t); + const int width = 640; + const int height = 400; + const int type = CV_8UC3; + const int depth_type = CV_16UC1; + +protected: + bool alive_; + std::thread td_; + std::shared_ptr api_; + MYNTdata raw_; + uint32_t time_; + mutable std::mutex raw_mutex_; +}; + +#endif \ No newline at end of file diff --git a/src/common/include/common/ImageResult.h b/src/common/include/common/ImageResult.h new file mode 100644 index 0000000..e69de29 diff --git a/src/common/include/common/PlayerInfo.h b/src/common/include/common/PlayerInfo.h new file mode 100644 index 0000000..e69de29 diff --git a/src/common/include/common/SetInt.h b/src/common/include/common/SetInt.h new file mode 100644 index 0000000..e69de29 -- Gitee From c76b07cd50b4ca16292171e8092dd99599b2260d Mon Sep 17 00:00:00 2001 From: scirocco Date: Fri, 7 Feb 2020 23:20:11 +0800 Subject: [PATCH 04/12] remove empty files --- src/common/include/common/ImageResult.h | 0 src/common/include/common/PlayerInfo.h | 0 src/common/include/common/SetInt.h | 0 3 files changed, 0 insertions(+), 0 deletions(-) delete mode 100644 src/common/include/common/ImageResult.h delete mode 100644 src/common/include/common/PlayerInfo.h delete mode 100644 src/common/include/common/SetInt.h diff --git a/src/common/include/common/ImageResult.h b/src/common/include/common/ImageResult.h deleted file mode 100644 index e69de29..0000000 diff --git a/src/common/include/common/PlayerInfo.h b/src/common/include/common/PlayerInfo.h deleted file mode 100644 index e69de29..0000000 diff --git a/src/common/include/common/SetInt.h b/src/common/include/common/SetInt.h deleted file mode 100644 index e69de29..0000000 -- Gitee From 151e425c2132014fb7c1e0a903f2aa8ac156f40c Mon Sep 17 00:00:00 2001 From: scirocco Date: Fri, 7 Feb 2020 23:32:18 +0800 Subject: [PATCH 05/12] prerelease binocular --- src/binocular/CMakeLists.txt | 241 ++++++++++++++++++++++++++ src/binocular/launch/binocular.launch | 7 + src/binocular/package.xml | 80 +++++++++ 3 files changed, 328 insertions(+) create mode 100644 src/binocular/CMakeLists.txt create mode 100644 src/binocular/launch/binocular.launch create mode 100644 src/binocular/package.xml diff --git a/src/binocular/CMakeLists.txt b/src/binocular/CMakeLists.txt new file mode 100644 index 0000000..5178e48 --- /dev/null +++ b/src/binocular/CMakeLists.txt @@ -0,0 +1,241 @@ +cmake_minimum_required(VERSION 2.8.3) +project(binocular) + +enable_language(C) +enable_language(CXX) +enable_language(CUDA) +find_package(CUDA REQUIRED) +include_directories(${CUDA_INCLUDE_DIRS}) +cuda_select_nvcc_arch_flags(NVCC_FLAGS "Auto") +string (REPLACE ";" " " CUDA_ARCH_FLAGS_SPACE_SEPARATED "${NVCC_FLAGS}") +set(CMAKE_CUDA_FLAGS "${CUDA_ARCH_FLAGS_SPACE_SEPARATED}") +set(CMAKE_CXX_STANDARD 11) +find_package(OpenCV) +add_definitions(-DGPU) +add_definitions(-DCUDNN) +## Compile as C++11, supported in ROS Kinetic and newer +# add_compile_options(-std=c++11) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + message_generation + roscpp + rospy + sensor_msgs + std_msgs + config + common + seuimage + darknet +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a exec_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# CameraProperty.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# CameraInfo.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# std_msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES vision +# CATKIN_DEPENDS message_generation roscpp rospy sensor_msgs std_msgs +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/vision.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +# add_executable(camera src/camera.cpp src/vision_parser.cpp) +set(CAMERA_FILES src/MYNTeye.cpp) +add_executable(binocular src/Binocular.cpp ${CAMERA_FILES}) +# add_executable(sim_vision src/SimVision.cpp src/camera/TpCamera.cpp) +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(camera +# ${catkin_LIBRARIES} +# MVSDK +# config +# ) + +target_link_libraries(binocular + ${catkin_LIBRARIES} + cuda + cudart + darknet + seuimage + MVSDK + opencv_core + opencv_imgproc + opencv_imgcodecs + config + pthread +) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html +# install(TARGETS ${PROJECT_NAME}_node +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark libraries for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html +# install(TARGETS ${PROJECT_NAME} +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_vision.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/src/binocular/launch/binocular.launch b/src/binocular/launch/binocular.launch new file mode 100644 index 0000000..13ec2cc --- /dev/null +++ b/src/binocular/launch/binocular.launch @@ -0,0 +1,7 @@ + + + + + + + \ No newline at end of file diff --git a/src/binocular/package.xml b/src/binocular/package.xml new file mode 100644 index 0000000..cce8ee8 --- /dev/null +++ b/src/binocular/package.xml @@ -0,0 +1,80 @@ + + + binocular + 0.0.0 + The binocular package + + + + + Sciroccogti + + + + + + Apache2 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + message_generation + roscpp + rospy + sensor_msgs + std_msgs + common + config + seuimage + darknet + roscpp + rospy + sensor_msgs + std_msgs + message_generation + roscpp + rospy + sensor_msgs + std_msgs + message_runtime + common + seuimage + darknet + + + + + + + -- Gitee From 8f739ab7679fe1cf0de0d58b824b90b75624f931 Mon Sep 17 00:00:00 2001 From: scirocco Date: Fri, 7 Feb 2020 23:35:29 +0800 Subject: [PATCH 06/12] remove default construction of MYNTeye --- src/binocular/src/MYNTeye.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/binocular/src/MYNTeye.hpp b/src/binocular/src/MYNTeye.hpp index 983c9c3..c5aa009 100644 --- a/src/binocular/src/MYNTeye.hpp +++ b/src/binocular/src/MYNTeye.hpp @@ -16,7 +16,7 @@ struct MYNTdata class MYNTeye { public: - MYNTeye() = default; + MYNTeye(); ~MYNTeye(); bool Open(); bool Close(); -- Gitee From 83670834242f20da7ebec1bfb8bb91d21d51f2c5 Mon Sep 17 00:00:00 2001 From: scirocco Date: Fri, 7 Feb 2020 23:37:19 +0800 Subject: [PATCH 07/12] add lib mynteye --- src/binocular/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/binocular/CMakeLists.txt b/src/binocular/CMakeLists.txt index 5178e48..bf88ce2 100644 --- a/src/binocular/CMakeLists.txt +++ b/src/binocular/CMakeLists.txt @@ -177,7 +177,7 @@ target_link_libraries(binocular cudart darknet seuimage - MVSDK + mynteye opencv_core opencv_imgproc opencv_imgcodecs -- Gitee From 1f98447a0560a8c88dae66f9dd28daee4889faac Mon Sep 17 00:00:00 2001 From: scirocco Date: Fri, 7 Feb 2020 23:39:30 +0800 Subject: [PATCH 08/12] fix type() --- src/binocular/src/Binocular.cpp | 2 +- src/binocular/src/MYNTeye.hpp | 1 - 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/src/binocular/src/Binocular.cpp b/src/binocular/src/Binocular.cpp index e2b8b52..d8acf3e 100644 --- a/src/binocular/src/Binocular.cpp +++ b/src/binocular/src/Binocular.cpp @@ -280,7 +280,7 @@ bool MallocMemory() if (camera.get() == nullptr) return false; bool ret; - ret = srcMat.create(camera->height, camera->width, GET_CHANNELS(camera->Type())); + ret = srcMat.create(camera->height, camera->width, GET_CHANNELS(camera->type)); if (!ret) return false; diff --git a/src/binocular/src/MYNTeye.hpp b/src/binocular/src/MYNTeye.hpp index c5aa009..e8e70b4 100644 --- a/src/binocular/src/MYNTeye.hpp +++ b/src/binocular/src/MYNTeye.hpp @@ -21,7 +21,6 @@ public: bool Open(); bool Close(); // bool Available(); 好像没什么意义，Open()返回的就够了 - int Type(); void Run(); MYNTdata &GetData(uint32_t &t); const int width = 640; -- Gitee From 7ab8da9d626775710aebf2c2af4131bab7056551 Mon Sep 17 00:00:00 2001 From: scirocco Date: Fri, 7 Feb 2020 23:40:28 +0800 Subject: [PATCH 09/12] fix type*2 --- src/binocular/src/Binocular.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/binocular/src/Binocular.cpp b/src/binocular/src/Binocular.cpp index d8acf3e..04bc201 100644 --- a/src/binocular/src/Binocular.cpp +++ b/src/binocular/src/Binocular.cpp @@ -34,7 +34,7 @@ void Run(const ros::TimerEvent &event) ROS_ERROR("upload error"); return; } - if (GET_CHANNELS(camera->Type()) == 2) // YUYV + if (GET_CHANNELS(camera->type) == 2) // YUYV { ret = YUV422ToRGB(srcMat, rgbMat); ret = Resize(rgbMat, dstMat); -- Gitee From 68f04b43a3c4889b835a6e3bc6e686ae37f9405e Mon Sep 17 00:00:00 2001 From: scirocco Date: Fri, 7 Feb 2020 23:43:35 +0800 Subject: [PATCH 10/12] fix channel --- src/binocular/src/Binocular.cpp | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/src/binocular/src/Binocular.cpp b/src/binocular/src/Binocular.cpp index 04bc201..8126c5b 100644 --- a/src/binocular/src/Binocular.cpp +++ b/src/binocular/src/Binocular.cpp @@ -34,12 +34,24 @@ void Run(const ros::TimerEvent &event) ROS_ERROR("upload error"); return; } - if (GET_CHANNELS(camera->type) == 2) // YUYV + if (GET_CHANNELS(camera->Type()) == 1) // Bayer + { + ret = CudaBayerToRGB(srcMat, rgbMat, 1.12, 0.85, 1.37); + //ret = BayerToRGB(srcMat, rgbMat); + ret = Resize(rgbMat, dstMat); + ret = dstMat.copyTo(relMat); + } + else if (GET_CHANNELS(camera->Type()) == 2) // YUYV { ret = YUV422ToRGB(srcMat, rgbMat); ret = Resize(rgbMat, dstMat); ret = dstMat.copyTo(relMat); } + else if (GET_CHANNELS(camera->Type()) == 3) // RGB + { + ret = Resize(srcMat, dstMat); + ret = dstMat.copyTo(relMat); + } else { ROS_ERROR("channel error"); -- Gitee From e687a125d36c3b343ac5bebde5e196c10a23c2e9 Mon Sep 17 00:00:00 2001 From: scirocco Date: Fri, 7 Feb 2020 23:44:30 +0800 Subject: [PATCH 11/12] fix type*3 --- src/binocular/src/Binocular.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/binocular/src/Binocular.cpp b/src/binocular/src/Binocular.cpp index 8126c5b..eb9da0e 100644 --- a/src/binocular/src/Binocular.cpp +++ b/src/binocular/src/Binocular.cpp @@ -34,20 +34,20 @@ void Run(const ros::TimerEvent &event) ROS_ERROR("upload error"); return; } - if (GET_CHANNELS(camera->Type()) == 1) // Bayer + if (GET_CHANNELS(camera->type) == 1) // Bayer { ret = CudaBayerToRGB(srcMat, rgbMat, 1.12, 0.85, 1.37); //ret = BayerToRGB(srcMat, rgbMat); ret = Resize(rgbMat, dstMat); ret = dstMat.copyTo(relMat); } - else if (GET_CHANNELS(camera->Type()) == 2) // YUYV + else if (GET_CHANNELS(camera->type) == 2) // YUYV { ret = YUV422ToRGB(srcMat, rgbMat); ret = Resize(rgbMat, dstMat); ret = dstMat.copyTo(relMat); } - else if (GET_CHANNELS(camera->Type()) == 3) // RGB + else if (GET_CHANNELS(camera->type) == 3) // RGB { ret = Resize(srcMat, dstMat); ret = dstMat.copyTo(relMat); -- Gitee From 8e1fe7382995af73a44a272a4a76e0b029abc7e7 Mon Sep 17 00:00:00 2001 From: Sciroccogti Date: Sat, 8 Feb 2020 10:47:00 +0800 Subject: [PATCH 12/12] fix color, depth not fetched in the same time --- src/binocular/src/Binocular.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/binocular/src/Binocular.cpp b/src/binocular/src/Binocular.cpp index eb9da0e..920e493 100644 --- a/src/binocular/src/Binocular.cpp +++ b/src/binocular/src/Binocular.cpp @@ -27,7 +27,8 @@ void Run(const ros::TimerEvent &event) } bool ret; uint32_t image_time; - ret = srcMat.upload(camera->GetData(image_time).color); + MYNTdata raw = camera->GetData(image_time); + ret = srcMat.upload(raw.color); if (!ret) { @@ -162,7 +163,7 @@ void Run(const ros::TimerEvent &event) { cv::Rect ball_roi(ball_dets[0].x, ball_dets[0].y, ball_dets[0].w, ball_dets[0].h); cv::rectangle(bgr, ball_roi, cv::Scalar(255, 0, 0), 2); - int ball_dist = CountDistance(camera->GetData(image_time).depth, ball_roi); + int ball_dist = CountDistance(raw.depth, ball_roi); ROS_INFO("ball_dist=%d", ball_dist); // TODO: 接入接口而不是什么INFO！ } @@ -170,7 +171,7 @@ void Run(const ros::TimerEvent &event) { cv::Rect post_roi(post_dets[0].x, post_dets[0].y, post_dets[0].w, post_dets[0].h); cv::rectangle(bgr, post_roi, cv::Scalar(0, 0, 255), 2); - int post_dist = CountDistance(camera->GetData(image_time).depth, post_roi); + int post_dist = CountDistance(raw.depth, post_roi); ROS_INFO("post_dist=%d", post_dist); // TODO: 接入接口而不是什么INFO！ } } -- Gitee