From 4ed7431eefee1542c754b8ecea225207a3885bce Mon Sep 17 00:00:00 2001 From: Paul Janin Date: Wed, 6 May 2015 15:26:49 +0200 Subject: [PATCH 1/3] Essai filtre RGB --- src/filtreRGB.cpp | 109 ++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 109 insertions(+) create mode 100644 src/filtreRGB.cpp diff --git a/src/filtreRGB.cpp b/src/filtreRGB.cpp new file mode 100644 index 0000000..2094c5a --- /dev/null +++ b/src/filtreRGB.cpp @@ -0,0 +1,109 @@ +#include +#include +#include +#include + +typedef pcl::PointXYZRGB Point; +typedef pcl::PointCloud PointCloud; + +class Callback { + public: + void + operator()(const PointCloud::ConstPtr& msg) + { + PointCloud::Ptr pcl(new PointCloud()); + copy_info(msg, pcl); + BOOST_FOREACH (const Point& pt, msg->points) + { + if (abs(pt.r-red) <= delta and abs(pt.g-green) <= delta and abs(pt.b-blue) <= delta) + pcl->push_back(pt); + } + pcl->height = 1; + pcl->width = pcl->points.size(); + publisher.publish(pcl); + } + + Callback(ros::Publisher& pub, uint8_t r, uint8_t g, uint8_t b, uint8_t d) + : publisher(pub), delta(d), red(r), green(g), blue(b) {} + + private: + ros::Publisher publisher; + uint8_t delta; + + uint8_t red; + uint8_t green; + uint8_t blue; + + inline + void + copy_info(const PointCloud::ConstPtr& a, + PointCloud::Ptr b) + { + b->header = a->header; + b->sensor_origin_ = a->sensor_origin_; + b->sensor_orientation_ = a->sensor_orientation_; + b->is_dense = a->is_dense; + } +}; + +int +main(int argc, char** argv) +{ + ros::init(argc, argv, "filtreRGB"); + ros::NodeHandle node("filtreRGB"); + + // récupération des paramètres + uint8_t red(0); + uint8_t blue(0); + uint8_t green(0); + + uint8_t delta(0); + + if (node.getParam("red", red)) + { + ROS_INFO("red : %f" , red); + } else { + node.setParam("red", 0); + node.getParam("red", red); + ROS_INFO("red : %f (default value)", red); + } + + if (node.getParam("blue", blue)) + { + + ROS_INFO("blue : %f" , blue); + } else { + node.setParam("blue", 0); + node.getParam("blue", blue); + ROS_INFO("blue : %f (default value)", blue); + } + + if (node.getParam("green", green)) + { + ROS_INFO("green : %f" , green); + } else { + node.setParam("green", 0); + node.getParam("green", green); + ROS_INFO("green : %f (default value)", green); + } + + if (node.getParam("delta", delta)) + { + ROS_INFO("delta : %f" , delta); + } else { + node.setParam("delta", 0); + node.getParam("delta", delta); + ROS_INFO("delta : %f (default value)", delta); + } + + // initialisatio + ros::Publisher publisher = node.advertise("output", 1); + Callback callback(publisher, (uint8_t) red, (uint8_t) green, (uint8_t) blue, (uint8_t) delta); + ros::Subscriber subscriber = node.subscribe("input", 1, callback); + + // démarrage + ROS_INFO("node started"); + ros::spin(); + ROS_INFO("exit"); + return 0; +} From 5cc52385266cc541800d704f011f2aa90f0de177 Mon Sep 17 00:00:00 2001 From: Paul Janin Date: Wed, 6 May 2015 16:03:29 +0200 Subject: [PATCH 2/3] filtreRGB works --- CMakeLists.txt | 3 +++ src/filtreRGB.cpp | 41 ++++++++++++++++++++++++----------------- 2 files changed, 27 insertions(+), 17 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index c8c9ac3..a5f9ae7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -50,6 +50,9 @@ add_library(display src/display.cpp) add_executable(keyboard_cmd src/keyboard_cmd.cpp) target_link_libraries(keyboard_cmd display ${catkin_LIBRARIES} ${ncursesw_LIBRARIES}) +add_executable(filtreRGB src/filtreRGB.cpp) +target_link_libraries(filtreRGB ${catkin_LIBRARIES}) + add_executable(normal_estimator src/normal_estimator.cpp) target_link_libraries(normal_estimator ${catkin_LIBRARIES}) add_dependencies(normal_estimator hand_control_generate_messages_cpp) diff --git a/src/filtreRGB.cpp b/src/filtreRGB.cpp index 2094c5a..ee1420a 100644 --- a/src/filtreRGB.cpp +++ b/src/filtreRGB.cpp @@ -59,46 +59,53 @@ main(int argc, char** argv) uint8_t delta(0); - if (node.getParam("red", red)) + int redInt; + int greenInt; + int blueInt; + + int deltaInt; + + if (node.getParam("red", redInt)) { - ROS_INFO("red : %f" , red); + ROS_INFO("red : %d" , redInt); } else { node.setParam("red", 0); - node.getParam("red", red); - ROS_INFO("red : %f (default value)", red); + node.getParam("red", redInt); + ROS_INFO("red : %d (default value)", redInt); } - if (node.getParam("blue", blue)) + if (node.getParam("blue", blueInt)) { - ROS_INFO("blue : %f" , blue); + ROS_INFO("blue : %d" , blueInt); } else { node.setParam("blue", 0); - node.getParam("blue", blue); - ROS_INFO("blue : %f (default value)", blue); + node.getParam("blue", blueInt); + ROS_INFO("blue : %d (default value)", blueInt); } - if (node.getParam("green", green)) + if (node.getParam("green", greenInt)) { - ROS_INFO("green : %f" , green); + ROS_INFO("green : %d" , greenInt); } else { node.setParam("green", 0); - node.getParam("green", green); - ROS_INFO("green : %f (default value)", green); + node.getParam("green", greenInt); + ROS_INFO("green : %d (default value)", greenInt); } - if (node.getParam("delta", delta)) + if (node.getParam("delta", deltaInt)) { - ROS_INFO("delta : %f" , delta); + ROS_INFO("delta : %d" , deltaInt); } else { node.setParam("delta", 0); - node.getParam("delta", delta); - ROS_INFO("delta : %f (default value)", delta); + node.getParam("delta", deltaInt); + ROS_INFO("delta : %d (default value)", deltaInt); } + // initialisatio ros::Publisher publisher = node.advertise("output", 1); - Callback callback(publisher, (uint8_t) red, (uint8_t) green, (uint8_t) blue, (uint8_t) delta); + Callback callback(publisher, (uint8_t) redInt, (uint8_t) greenInt, (uint8_t) blueInt, (uint8_t) deltaInt); ros::Subscriber subscriber = node.subscribe("input", 1, callback); // démarrage From 899a1f28d072217d977d1f69b85d1999adc754b2 Mon Sep 17 00:00:00 2001 From: Louis-Guillaume DUBOIS Date: Wed, 6 May 2015 16:08:42 +0200 Subject: [PATCH 3/3] lower init speeds in keyboard_cmd --- src/keyboard_cmd.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/keyboard_cmd.cpp b/src/keyboard_cmd.cpp index 96bf5cb..3bf0e73 100644 --- a/src/keyboard_cmd.cpp +++ b/src/keyboard_cmd.cpp @@ -57,10 +57,10 @@ class Run cmdvel_callback(terminal), term(terminal), loop_rate(30), - x_speed(0.2), - y_speed(0.3), - z_speed(0.5), - turn(0.5) { + x_speed(0.05), + y_speed(0.05), + z_speed(0.05), + turn(0.1) { cmd = n.advertise("/cmd_vel",1); pub_takeoff = n.advertise("/ardrone/takeoff", 1); pub_land = n.advertise("/ardrone/land", 1);