LGD/hand_control
1
0
Fork 0
Code Issues Pull requests Releases Wiki Activity

uses dynamic_reconfigure in «filtre» node

Browse source
This commit is contained in:
Louis-Guillaume DUBOIS 2015-05-23 15:41:37 +02:00
parent f18c12d64f
commit 4b0f934f59
3 changed files with 69 additions and 115 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

79
CMakeLists.txt
View file

@ -1,70 +1,77 @@
cmake_minimum_required(VERSION 2.8.3) cmake_minimum_required(VERSION 2.8.3)
project(hand_control) project(hand_control)
find_package(catkin REQUIRED COMPONENTS find_package(catkin REQUIRED COMPONENTS
pcl_ros pcl_ros
pcl_msgs pcl_msgs
roscpp roscpp
std_msgs std_msgs
geometry_msgs geometry_msgs
message_generation message_generation
ardrone_autonomy ardrone_autonomy
) dynamic_reconfigure
)
find_package( find_package(
PkgConfig REQUIRED PkgConfig REQUIRED
) )
pkg_check_modules ( ncursesw REQUIRED ncursesw) pkg_check_modules ( ncursesw REQUIRED ncursesw)
add_message_files( add_message_files(
FILES FILES
Plan.msg Plan.msg
) )
generate_messages( generate_messages(
DEPENDENCIES DEPENDENCIES
std_msgs std_msgs
geometry_msgs geometry_msgs
) )
catkin_package(
CATKIN_DEPENDS message_runtime
)
include_directories( include_directories(
${catkin_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}
${ncursesw_INCLUDE_DIRS} ${ncursesw_INCLUDE_DIRS}
) )
add_executable(filtre src/filtre.cpp) add_executable(filtre src/filtre.cpp)
target_link_libraries(filtre ${catkin_LIBRARIES}) 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}) 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}) target_link_libraries(pcl_displayer ${catkin_LIBRARIES})
add_library(display src/display.cpp) 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}) 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}) target_link_libraries(filtreRGB ${catkin_LIBRARIES})
#add_executable(filtreHue src/filtreHue.cpp) #add_executable(filtreHue src/filtreHue.cpp)
#target_link_libraries(filtreHue ${catkin_LIBRARIES}) #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}) target_link_libraries(normal_estimator ${catkin_LIBRARIES})
add_dependencies(normal_estimator hand_control_generate_messages_cpp) 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}) 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}) 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}) 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
)

4
cfg/Parameters.cfg → cfg/Filtre.cfg
View file

@ -1,4 +1,4 @@
#!/usr/bin/env python
PACKAGE = "hand_control" PACKAGE = "hand_control"
from dynamic_reconfigure.parameter_generator_catkin import * 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) #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"))

101
src/filtre.cpp
View file

@ -3,6 +3,8 @@
#include <pcl/point_types.h> #include <pcl/point_types.h>
#include <pcl/tracking/impl/hsv_color_coherence.hpp> #include <pcl/tracking/impl/hsv_color_coherence.hpp>
#include <assert.h> #include <assert.h>
#include <dynamic_reconfigure/server.h>
#include <hand_control/FiltreConfig.h>
typedef pcl::PointXYZRGB Point; typedef pcl::PointXYZRGB Point;
@ -11,7 +13,7 @@ typedef pcl::PointCloud<Point> PointCloud;
class Callback { class Callback {
public: public:
void void
operator()(const PointCloud::ConstPtr& msg) callback(const PointCloud::ConstPtr& msg)
{ {
PointCloud::Ptr pcl(new PointCloud()); PointCloud::Ptr pcl(new PointCloud());
copy_info(msg, pcl); copy_info(msg, pcl);
@ -25,8 +27,8 @@ class Callback {
publisher.publish(pcl); publisher.publish(pcl);
} }
Callback(ros::Publisher& pub, float z, float h, float delta_h, float smin, float smax, float vmin, float vmax) Callback(ros::Publisher& pub)
: publisher(pub), z_max(z), hue(h), delta_hue(delta_h), sat_min(smin), sat_max(smax), val_min(vmin), val_max(vmax) : 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(delta_hue > 0);
assert(z_max > 0); assert(z_max > 0);
@ -38,6 +40,17 @@ class Callback {
assert(val_max <= 1.); 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: private:
ros::Publisher publisher; ros::Publisher publisher;
float z_max, hue, delta_hue, val_min, val_max, sat_min, sat_max; 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::init(argc, argv, "filtre");
ros::NodeHandle node("filtre"); ros::NodeHandle node("filtre");
// récupération des paramètres // initialisation
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
ros::Publisher publisher = node.advertise<PointCloud>("output", 1); ros::Publisher publisher = node.advertise<PointCloud>("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); Callback my_callback(publisher);
ros::Subscriber subscriber = node.subscribe<PointCloud>("input", 1, callback); ros::Subscriber subscriber = node.subscribe<PointCloud>("input", 1, &Callback::callback, &my_callback);
dynamic_reconfigure::Server<hand_control::FiltreConfig> server;
dynamic_reconfigure::Server<hand_control::FiltreConfig>::CallbackType f;
f = boost::bind(&Callback::reconfigure, &my_callback, _1, _2);
server.setCallback(f);
// démarrage // démarrage
ROS_INFO("node started"); ROS_INFO("node started");