diff --git a/CMakeLists.txt b/CMakeLists.txt index 5597922..afd72fe 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,70 +1,77 @@ cmake_minimum_required(VERSION 2.8.3) project(hand_control) -find_package(catkin REQUIRED COMPONENTS - pcl_ros - pcl_msgs - roscpp - std_msgs - geometry_msgs - message_generation - ardrone_autonomy -) + find_package(catkin REQUIRED COMPONENTS + pcl_ros + pcl_msgs + roscpp + std_msgs + geometry_msgs + message_generation + ardrone_autonomy + dynamic_reconfigure + ) find_package( - PkgConfig REQUIRED -) + PkgConfig REQUIRED + ) pkg_check_modules ( ncursesw REQUIRED ncursesw) -add_message_files( - FILES - Plan.msg -) + add_message_files( + FILES + Plan.msg + ) -generate_messages( - DEPENDENCIES - std_msgs - geometry_msgs -) + generate_messages( + DEPENDENCIES + std_msgs + geometry_msgs + ) -catkin_package( -CATKIN_DEPENDS message_runtime -) -include_directories( - ${catkin_INCLUDE_DIRS} - ${ncursesw_INCLUDE_DIRS} -) + include_directories( + ${catkin_INCLUDE_DIRS} + ${ncursesw_INCLUDE_DIRS} + ) -add_executable(filtre src/filtre.cpp) + add_executable(filtre src/filtre.cpp) target_link_libraries(filtre ${catkin_LIBRARIES}) -add_executable(random_pcl_publisher src/random_pcl_publisher.cpp) + add_executable(random_pcl_publisher src/random_pcl_publisher.cpp) target_link_libraries(random_pcl_publisher ${catkin_LIBRARIES}) -add_executable(pcl_displayer src/pcl_displayer.cpp) + add_executable(pcl_displayer src/pcl_displayer.cpp) target_link_libraries(pcl_displayer ${catkin_LIBRARIES}) add_library(display src/display.cpp) -add_executable(keyboard_cmd src/keyboard_cmd.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) + add_executable(filtreRGB src/filtreRGB.cpp) target_link_libraries(filtreRGB ${catkin_LIBRARIES}) #add_executable(filtreHue src/filtreHue.cpp) #target_link_libraries(filtreHue ${catkin_LIBRARIES}) -add_executable(normal_estimator src/normal_estimator.cpp) + add_executable(normal_estimator src/normal_estimator.cpp) target_link_libraries(normal_estimator ${catkin_LIBRARIES}) add_dependencies(normal_estimator hand_control_generate_messages_cpp) -add_executable(commande src/commande.cpp) + add_executable(commande src/commande.cpp) target_link_libraries(commande ${catkin_LIBRARIES}) -add_executable(commande-new src/commande-new.cpp) + add_executable(commande-new src/commande-new.cpp) target_link_libraries(commande-new ${catkin_LIBRARIES}) -add_executable(commande-new-1d src/commande-new-1d.cpp) + add_executable(commande-new-1d src/commande-new-1d.cpp) target_link_libraries(commande-new-1d ${catkin_LIBRARIES}) + +#add dynamic reconfigure api + generate_dynamic_reconfigure_options( + cfg/Filtre.cfg + ) +add_dependencies(filtre ${PROJECT_NAME}_gencfg) +catkin_package( + CATKIN_DEPENDS message_runtime + ) diff --git a/cfg/Parameters.cfg b/cfg/Filtre.cfg similarity index 91% rename from cfg/Parameters.cfg rename to cfg/Filtre.cfg index febd9a8..94bab96 100755 --- a/cfg/Parameters.cfg +++ b/cfg/Filtre.cfg @@ -1,4 +1,4 @@ - +#!/usr/bin/env python PACKAGE = "hand_control" from dynamic_reconfigure.parameter_generator_catkin import * @@ -19,4 +19,4 @@ gen.add("val_max", double_t, 0, "The maximum color val of the hand", 0., 0., 1.) #gen.add("bool_param", bool_t, 0, "A Boolean parameter", True) -exit(gen.generate(PACKAGE, "hand_control", "Parameters")) +exit(gen.generate(PACKAGE, "hand_control", "Filtre")) diff --git a/src/filtre.cpp b/src/filtre.cpp index 13086a0..1de7d30 100644 --- a/src/filtre.cpp +++ b/src/filtre.cpp @@ -3,6 +3,8 @@ #include #include #include +#include +#include typedef pcl::PointXYZRGB Point; @@ -11,7 +13,7 @@ typedef pcl::PointCloud PointCloud; class Callback { public: void - operator()(const PointCloud::ConstPtr& msg) + callback(const PointCloud::ConstPtr& msg) { PointCloud::Ptr pcl(new PointCloud()); copy_info(msg, pcl); @@ -25,8 +27,8 @@ class Callback { publisher.publish(pcl); } - Callback(ros::Publisher& pub, float z, float h, float delta_h, float smin, float smax, float vmin, float vmax) - : publisher(pub), z_max(z), hue(h), delta_hue(delta_h), sat_min(smin), sat_max(smax), val_min(vmin), val_max(vmax) + Callback(ros::Publisher& pub) + : publisher(pub), z_max(90.), hue(0.), delta_hue(20.), sat_min(0.3), sat_max(1.), val_min(0.3), val_max(1.) { assert(delta_hue > 0); assert(z_max > 0); @@ -38,6 +40,17 @@ class Callback { assert(val_max <= 1.); } + void + reconfigure(hand_control::FiltreConfig& c, uint32_t level) { + z_max = c.z_max; + hue = c.hue; + delta_hue = c.delta_hue; + val_min = c.val_min; + val_max = c.val_max; + sat_min = c.sat_min; + sat_max = c.sat_max; + } + private: ros::Publisher publisher; float z_max, hue, delta_hue, val_min, val_max, sat_min, sat_max; @@ -89,81 +102,15 @@ main(int argc, char** argv) ros::init(argc, argv, "filtre"); ros::NodeHandle node("filtre"); - // récupération des paramètres - double z_max(0); - if (node.getParam("z_max", z_max)) - { - ROS_INFO("z_max : %f" , z_max); - } else { - node.setParam("z_max", 50.0); - node.getParam("z_max", z_max); - ROS_INFO("z_max : %f (default value)", z_max); - } - - double hue(0); - if (node.getParam("hue", hue)) - { - ROS_INFO("hue : %f" , hue); - } else { - node.setParam("hue", 0.0); - node.getParam("hue", hue); - ROS_INFO("hue : %f (default value)", hue); - } - - double delta_hue(0); - if (node.getParam("delta_hue", delta_hue)) - { - ROS_INFO("delta_hue : %f" , delta_hue); - } else { - node.setParam("delta_hue", 10.0); - node.getParam("delta_hue", delta_hue); - ROS_INFO("delta_hue : %f (default value)", delta_hue); - } - - double sat_min(0); - if (node.getParam("sat_min", sat_min)) - { - ROS_INFO("sat_min : %f" , sat_min); - } else { - node.setParam("sat_min", 0.1); - node.getParam("sat_min", sat_min); - ROS_INFO("sat_min : %f (default value)", sat_min); - } - - double sat_max(0); - if (node.getParam("sat_max", sat_max)) - { - ROS_INFO("sat_max : %f" , sat_max); - } else { - node.setParam("sat_max", 1.); - node.getParam("sat_max", sat_max); - ROS_INFO("sat_max : %f (default value)", sat_max); - } - - double val_min(0); - if (node.getParam("val_min", val_min)) - { - ROS_INFO("val_min : %f" , val_min); - } else { - node.setParam("val_min", 0.4); - node.getParam("val_min", val_min); - ROS_INFO("val_min : %f (default value)", val_min); - } - - double val_max(0); - if (node.getParam("val_max", val_max)) - { - ROS_INFO("val_max : %f" , val_max); - } else { - node.setParam("val_max", 1.); - node.getParam("val_max", val_max); - ROS_INFO("val_max : %f (default value)", val_max); - } - - // initialisatio + // initialisation ros::Publisher publisher = node.advertise("output", 1); - Callback callback(publisher, (float) z_max, (float) hue, (float) delta_hue, (float) sat_min, (float) sat_max, (float) val_min, (float) val_max); - ros::Subscriber subscriber = node.subscribe("input", 1, callback); + Callback my_callback(publisher); + ros::Subscriber subscriber = node.subscribe("input", 1, &Callback::callback, &my_callback); + + dynamic_reconfigure::Server server; + dynamic_reconfigure::Server::CallbackType f; + f = boost::bind(&Callback::reconfigure, &my_callback, _1, _2); + server.setCallback(f); // démarrage ROS_INFO("node started");