guillaume/miniprojet
1
0
Fork 0
Code Issues Pull requests Releases Wiki Activity

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

Browse source
This commit is contained in:
Guillaume Courrier 2019-12-17 08:23:35 +01:00
parent c132000776
commit 14ad23ddc3
5 changed files with 82 additions and 17 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

6
ROS/gesture_based_control/CMakeLists.txt
View file

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

11
ROS/gesture_based_control/cfg/Descriptor.cfg Executable file
View file

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

1
ROS/gesture_based_control/launch/gesture_control.launch
View file

@ -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>

63
ROS/gesture_based_control/src/descripteur.cpp
View file

@ -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();
}

18
tests/src/math.hpp
View file

@ -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,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; 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;
}