From 14ad23ddc3c86285d3340df0c05e10c44072c7ca Mon Sep 17 00:00:00 2001 From: Guillaume Courrier Date: Tue, 17 Dec 2019 08:23:35 +0100 Subject: [PATCH] =?UTF-8?q?impl=C3=A9mentation=20des=20descripteur=20dans?= =?UTF-8?q?=20le=20package=20ROS=20+=20ajout=20de=20dynamic=5Freconfigure?= =?UTF-8?q?=20pour=20changer=20les=20param=C3=A8tres?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- ROS/gesture_based_control/CMakeLists.txt | 6 ++ ROS/gesture_based_control/cfg/Descriptor.cfg | 11 ++++ .../launch/gesture_control.launch | 1 + ROS/gesture_based_control/src/descripteur.cpp | 63 +++++++++++++++++-- tests/src/math.hpp | 18 ++---- 5 files changed, 82 insertions(+), 17 deletions(-) create mode 100755 ROS/gesture_based_control/cfg/Descriptor.cfg diff --git a/ROS/gesture_based_control/CMakeLists.txt b/ROS/gesture_based_control/CMakeLists.txt index c77ce27..fad3984 100644 --- a/ROS/gesture_based_control/CMakeLists.txt +++ b/ROS/gesture_based_control/CMakeLists.txt @@ -9,6 +9,11 @@ find_package(catkin REQUIRED COMPONENTS std_msgs cv_bridge image_transport + dynamic_reconfigure +) + +generate_dynamic_reconfigure_options( + cfg/Descriptor.cfg ) catkin_package() @@ -22,3 +27,4 @@ target_link_libraries(controller ${catkin_LIBRARIES}) add_executable(descripteur src/descripteur.cpp) target_link_libraries(descripteur ${catkin_LIBRARIES}) +#add_dependencies(descripteur ${PROJECT_NAME}_gencfg) diff --git a/ROS/gesture_based_control/cfg/Descriptor.cfg b/ROS/gesture_based_control/cfg/Descriptor.cfg new file mode 100755 index 0000000..47d8e40 --- /dev/null +++ b/ROS/gesture_based_control/cfg/Descriptor.cfg @@ -0,0 +1,11 @@ +#!/usr/bin/env python +PACKAGE = "gesture_based_control" + +from dynamic_reconfigure.parameter_generator_catkin import * + +gen = ParameterGenerator() + +gen.add("cmax", int_t, 0, "cmax", 50, 0, 100) +gen.add("threshold", int_t, 0, "threshold", 50, 0, 255) + +exit(gen.generate(PACKAGE, "gesture_based_control", "Descriptors")) diff --git a/ROS/gesture_based_control/launch/gesture_control.launch b/ROS/gesture_based_control/launch/gesture_control.launch index 53a1e39..f911bb8 100644 --- a/ROS/gesture_based_control/launch/gesture_control.launch +++ b/ROS/gesture_based_control/launch/gesture_control.launch @@ -10,4 +10,5 @@ + diff --git a/ROS/gesture_based_control/src/descripteur.cpp b/ROS/gesture_based_control/src/descripteur.cpp index b50f715..cb370ca 100644 --- a/ROS/gesture_based_control/src/descripteur.cpp +++ b/ROS/gesture_based_control/src/descripteur.cpp @@ -2,10 +2,20 @@ #include #include #include -//#include +#include +#include -void callback(const sensor_msgs::ImageConstPtr& msg) { +#include +#include "math.hpp" + +void callback(gesture_based_control::DescriptorsConfig &config, uint32_t level, int& cmax, int& threshold) { + cmax = config.cmax; + threshold = config.threshold; +} + +void callback(const sensor_msgs::ImageConstPtr& msg, int& threshold, int& cmax, ros::Publisher& order_pub, image_transport::Publisher& image_pub) { cv_bridge::CvImagePtr cv_ptr; + std_msgs::String s; try { cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::RGB8); @@ -14,17 +24,62 @@ void callback(const sensor_msgs::ImageConstPtr& msg) { return; } - const cv::Mat& input = cv_ptr->image; + cv::Mat& input = cv_ptr->image; + cv::Mat binaire(input.rows, input.cols, CV_8UC1); + math::filter(input, binaire, threshold); + cv::GaussianBlur(input, input, cv::Size(7,7), 1.5, 1.5); + + std::vector contours; + std::vector hierarchy; + cv::findContours(binaire, contours, hierarchy, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE); + if (contours.size() != 0) { + int index = math::max_cont(contours); + math::csignal desc = math::descriptors(contours[index], cmax); + //TODO: implémenter algo ml + //TODO: publier résultat + int r = 0; + switch (r) { + case 0: + s.data = "left"; + break; + case 1: + s.data = "right"; + break; + case 2: + s.data = "stop"; + break; + case 3: + s.data = "forward"; + break; + default: + s.data = "None"; + break; + } + order_pub.publish(s); + cv::drawContours(input, contours, index, 255); + } + image_pub.publish(cv_bridge::CvImage(msg->header, "rgb8", input).toImageMsg()); } int main(int argc, char** argv) { ros::init(argc, argv, "descripteur"); + int threshold = 25; + int cmax = 10; + ros::NodeHandle n; image_transport::ImageTransport it(n); - image_transport::Subscriber sub = it.subscribe("/image_raw", 1, callback); + ros::Publisher order_pub = n.advertise("/order", 100); + image_transport::Publisher image_pub = it.advertise("/image_out", 1); + image_transport::Subscriber sub = it.subscribe("/image_raw", 1, boost::bind(callback, _1, boost::ref(threshold), boost::ref(cmax), boost::ref(order_pub), boost::ref(image_pub))); + + dynamic_reconfigure::Server server; + dynamic_reconfigure::Server::CallbackType f; + + f = boost::bind(&callback, _1, _2, boost::ref(cmax), boost::ref(threshold)); + server.setCallback(f); ros::spin(); } diff --git a/tests/src/math.hpp b/tests/src/math.hpp index 3e86700..9428bc6 100644 --- a/tests/src/math.hpp +++ b/tests/src/math.hpp @@ -14,7 +14,7 @@ namespace math { using contour = std::vector; constexpr double pi() {return std::atan(1)*4;} - void displayi_abs(const csignal& s) { + void display_abs(const csignal& s) { int count=0; for (auto d: s) { std::cout << count++ << ' ' << std::abs(d) << std::endl; @@ -65,13 +65,10 @@ namespace math { if ((R>G) && (R>B)) { if (((R-B)>=seuil) || ((R-G)>=seuil)) { - detect = true; + output.data[indexNB]=255; + } else { + output.data[indexNB]=0; } - } - if (detect==1) { - output.data[indexNB]=255; - } else { - output.data[indexNB]=0; } } } @@ -109,15 +106,13 @@ namespace math { for (int n=0; npush_back(t); } return *res; } - csignal fft_rec(const csignal& input) { + csignal fft_rec(const csignal& input) { //TODO: implémenter la fft !!! int size = input.size(); - std::cout << "Size: " << size << std::endl; if (size <= 1) { return input; @@ -175,7 +170,6 @@ namespace math { opt_size = 1 << (int)std::ceil(std::log(N)/std::log(2)); } opt_size = input.size(); - std::cout << opt_size << std::endl; csignal sig(input); for (int i=0; i