85 lines
2.6 KiB
C++
85 lines
2.6 KiB
C++
#include <ros/ros.h>
|
|
#include <opencv2/opencv.hpp>
|
|
#include <image_transport/image_transport.h>
|
|
#include <cv_bridge/cv_bridge.h>
|
|
#include <std_msgs/String.h>
|
|
#include <dynamic_reconfigure/server.h>
|
|
|
|
#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);
|
|
}catch(cv_bridge::Exception& e) {
|
|
ROS_ERROR("cv_bridge exception: %s", e.what());
|
|
return;
|
|
}
|
|
|
|
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);
|
|
|
|
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();
|
|
}
|