implémentation des descripteur dans le package ROS + ajout de dynamic_reconfigure pour changer les paramètres

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)

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"))

ROS/gesture_based_control/launch/gesture_control.launch

@@ -10,4 +10,5 @@
   <node name="descripteur" pkg="gesture_based_control" type="descripteur" output="screen">
     <remap from="/image_raw" to="/usb_cam/image_raw"/>
   </node>
+  <node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure"/>
 </launch>

ROS/gesture_based_control/src/descripteur.cpp

@@ -2,10 +2,20 @@
 #include <opencv2/opencv.hpp>
 #include <image_transport/image_transport.h>
 #include <cv_bridge/cv_bridge.h>
-//#include <math.hpp>
+#include <std_msgs/String.h>
+#include <dynamic_reconfigure/server.h>
 
-void callback(const sensor_msgs::ImageConstPtr& msg) {
+#include <gesture_based_control/DescriptorsConfig.h>
+#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<math::contour> contours;
+  std::vector<cv::Vec4i> 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<std_msgs::String>("/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<gesture_based_control::DescriptorsConfig> server;
+  dynamic_reconfigure::Server<gesture_based_control::DescriptorsConfig>::CallbackType f;
+
+  f = boost::bind(&callback, _1, _2, boost::ref(cmax), boost::ref(threshold));
+  server.setCallback(f);
   
   ros::spin();
 }

tests/src/math.hpp

@@ -14,7 +14,7 @@ namespace math {
   using contour = std::vector<cv::Point>;
   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,16 +65,13 @@ namespace math {
   
       if ((R>G) && (R>B)) {
         if (((R-B)>=seuil) || ((R-G)>=seuil)) {
-  				detect = true;
-        }
-      }
-      if (detect==1) {
           output.data[indexNB]=255;
         } else {
           output.data[indexNB]=0;
         }
       }
   	}
+  }
 
   csignal cont2sig(const contour& cont) {
     csignal sig;
@@ -109,15 +106,13 @@ namespace math {
       for (int n=0; n<size; ++n) {
         t += (input[n] * std::exp(complex(0, -2*pi()*n*k/size)));
       }
-      std::cout << t << std::endl;
       res->push_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<opt_size-input.size(); ++i) {
       sig.push_back(complex(0, 0));
@@ -212,7 +206,6 @@ namespace math {
     int kmin = tfd.size()/2 + cmin;
     int kmax = tfd.size()/2 + cmax;
     
-    //display(tfd);
     auto tfd_it = tfd.end() + cmin;
     for (int k=0; k<-cmin; ++k) {
       res.push_back(*(tfd_it++));
@@ -221,7 +214,6 @@ namespace math {
     for (int k=0; k<cmax+1; ++k) {
       res.push_back(*(tfd_it++));
     }
-    //display(res);
     return res;
   }