LGD
/
hand_control
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity
Browse Source

better english node and file naming, & rm useless files

master
Louis-Guillaume DUBOIS 11 years ago
parent
9c10400e54
commit
6f4e6eda41
18 changed files with 43 additions and 574 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 24
      CMakeLists.txt
  2. 2
      cfg/Commander.cfg
  3. 2
      cfg/Filter.cfg
  4. 19
      launch/all-abs.launch
  5. 19
      launch/all-new-1d.launch
  6. 20
      launch/all-new.launch
  7. 25
      launch/all-param.launch
  8. 19
      launch/all.launch
  9. 75
      launch/ardrone_aggressive.launch
  10. 26
      launch/essai-commande.launch
  11. 10
      launch/essai-estimator.launch
  12. 11
      launch/essai-filtre.launch
  13. 17
      launch/kinect_commander.launch
  14. 230
      src/commande-new-1d.cpp
  15. 12
      src/commander.cpp
  16. 0
      src/estimator.cpp
  17. 12
      src/filter.cpp
  18. 94
      src/normal_estimator.cpp

24
CMakeLists.txt
View File

@ -35,8 +35,8 @@ pkg_check_modules ( ncursesw REQUIRED ncursesw)
${ncursesw_INCLUDE_DIRS} ${ncursesw_INCLUDE_DIRS}
) )
add_executable(filtre src/filtre.cpp) add_executable(filter src/filter.cpp)
target_link_libraries(filtre ${catkin_LIBRARIES}) target_link_libraries(filter ${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})
@ -49,22 +49,22 @@ 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(normal_estimator_pca src/normal_estimator-pca.cpp) add_executable(estimator src/estimator.cpp)
target_link_libraries(normal_estimator_pca ${catkin_LIBRARIES}) target_link_libraries(estimator ${catkin_LIBRARIES})
add_dependencies(normal_estimator_pca hand_control_generate_messages_cpp) add_dependencies(estimator hand_control_generate_messages_cpp)
add_executable(commande-abs src/commande-abs.cpp) add_executable(commander src/commander.cpp)
target_link_libraries(commande-abs ${catkin_LIBRARIES}) target_link_libraries(commander ${catkin_LIBRARIES})
add_dependencies(commande-abs hand_control_generate_messages_cpp) add_dependencies(commander hand_control_generate_messages_cpp)
#add dynamic reconfigure api #add dynamic reconfigure api
generate_dynamic_reconfigure_options( generate_dynamic_reconfigure_options(
cfg/Filtre.cfg cfg/Filter.cfg
cfg/Commande.cfg cfg/Commander.cfg
) )
add_dependencies(filtre ${PROJECT_NAME}_gencfg) add_dependencies(filter ${PROJECT_NAME}_gencfg)
add_dependencies(commande-abs ${PROJECT_NAME}_gencfg) add_dependencies(commander ${PROJECT_NAME}_gencfg)
catkin_package( catkin_package(
CATKIN_DEPENDS message_runtime CATKIN_DEPENDS message_runtime

2
cfg/Commande.cfg → cfg/Commander.cfg
View File

@ -13,4 +13,4 @@ gen.add("theta_minimal_deviation", double_t, 0, "Absolute angular movement detec
gen.add("angle_vel", double_t, 0, "Angular velocity", 0.5, 0., 10.) gen.add("angle_vel", double_t, 0, "Angular velocity", 0.5, 0., 10.)
gen.add("plan_vel", double_t, 0, "Translation velocity", 2, 0., 10.) gen.add("plan_vel", double_t, 0, "Translation velocity", 2, 0., 10.)
gen.add("z_vel", double_t, 0, "Vertical translation velocity", 2, 0., 10.) gen.add("z_vel", double_t, 0, "Vertical translation velocity", 2, 0., 10.)
exit(gen.generate(PACKAGE, "hand_control", "Commande")) exit(gen.generate(PACKAGE, "hand_control", "Commander"))

2
cfg/Filtre.cfg → cfg/Filter.cfg
View File

@ -9,4 +9,4 @@ gen.add("sat_min", double_t, 0, "The minimum color saturation of the hand", 0.1,
gen.add("sat_max", double_t, 0, "The maximum color saturation of the hand", 1., 0., 1.) gen.add("sat_max", double_t, 0, "The maximum color saturation of the hand", 1., 0., 1.)
gen.add("val_min", double_t, 0, "The minimum color value of the hand", 0.1, 0., 1.) gen.add("val_min", double_t, 0, "The minimum color value of the hand", 0.1, 0., 1.)
gen.add("val_max", double_t, 0, "The maximum color value of the hand", 1., 0., 1.) gen.add("val_max", double_t, 0, "The maximum color value of the hand", 1., 0., 1.)
exit(gen.generate(PACKAGE, "hand_control", "Filtre")) exit(gen.generate(PACKAGE, "hand_control", "Filter"))

19
launch/all-abs.launch
View File

@ -1,19 +0,0 @@
<launch>
<include file="$(find hand_control)/launch/all-param.launch" />
<include file="$(find hand_control)/launch/freenect-registered-xyzrgb.launch"/>
<node name="filtre" pkg="hand_control" type="filtre">
<remap from="/filtre/input" to="/camera/xyzrgb/points"/>
</node>
<node name="estimator" pkg="hand_control" type="normal_estimator_pca">
<remap from="/estimator/input" to="/filtre/output"/>
</node>
<node name="commander" pkg="hand_control" type="commande-abs">
<remap from="/commande/input" to="/estimator/output"/>
</node>
</launch>

19
launch/all-new-1d.launch
View File

@ -1,19 +0,0 @@
<launch>
<include file="$(find hand_control)/launch/all-param.launch" />
<include file="$(find hand_control)/launch/freenect-registered-xyzrgb.launch"/>
<node name="filtre" pkg="hand_control" type="filtre">
<remap from="/filtre/input" to="/camera/xyzrgb/points"/>
</node>
<node name="estimator" pkg="hand_control" type="normal_estimator">
<remap from="/estimator/input" to="/filtre/output"/>
</node>
<node name="commander" pkg="hand_control" type="commande-new-1d">
<remap from="/commande/input" to="/estimator/output"/>
</node>
</launch>

20
launch/all-new.launch
View File

@ -1,20 +0,0 @@
<launch>
<include file="$(find hand_control)/launch/all-param.launch" />
<include file="$(find hand_control)/launch/freenect-registered-xyzrgb.launch"/>
<node name="filtre" pkg="hand_control" type="filtre">
<remap from="/filtre/input" to="/camera/xyzrgb/points"/>
</node>
<node name="estimator" pkg="hand_control" type="normal_estimator">
<remap from="/estimator/input" to="/filtre/output"/>
</node>
<node name="commander" pkg="hand_control" type="commande-new">
<remap from="/commande/input" to="/estimator/output"/>
</node>
</launch>

25
launch/all-param.launch
View File

@ -1,25 +0,0 @@
<launch>
<param name="/filtre/zmax" value="1.9" type="double"/>
<!--
<param name="/filtre/hue" value="0.0" type="double"/>
<param name="/filtre/hue" value="210.0" type="double"/>
-->
<param name="/filtre/hue" value="140.0" type="double"/>
<param name="/filtre/delta_hue" value="60.0" type="double"/>
<param name="/filtre/val_min" value="0.01" type="double"/>
<param name="/filtre/val_max" value="1." type="double"/>
<param name="/filtre/sat_min" value="0.02" type="double"/>
<param name="/filtre/sat_max" value="1." type="double"/>
<param name="/commande/max_curv" value="0.02" type="double" />
<param name="/commande/plan_vel" value="0.2" type="double" />
<param name="/commande/z_vel" value="1.0" type="double" />
<param name="/commande/min_number" value="2000" type="int" />
<param name="/commande/x_dev_min" value="0.1" type="double" />
<param name="/commande/y_dev_min" value="0.1" type="double" />
<param name="/commande/y_dev_min" value="0.1" type="double" />
<param name="/commande/dz_dev_min" value="0.05" type="double" />
</launch>

19
launch/all.launch
View File

@ -1,19 +0,0 @@
<launch>
<include file="$(find hand_control)/launch/all-param.launch" />
<include file="$(find hand_control)/launch/freenect-registered-xyzrgb.launch"/>
<node name="filtre" pkg="hand_control" type="filtre">
<remap from="/filtre/input" to="/camera/xyzrgb/points"/>
</node>
<node name="estimator" pkg="hand_control" type="normal_estimator">
<remap from="/estimator/input" to="/filtre/output"/>
</node>
<node name="commander" pkg="hand_control" type="commande-new-1d">
<remap from="/commande/input" to="/estimator/output"/>
</node>
</launch>

75
launch/ardrone_aggressive.launch
View File

@ -1,75 +0,0 @@
<!-- This is a sample lanuch file, please change it based on your needs -->
<launch>
<node name="ardrone_driver" pkg="ardrone_autonomy" type="ardrone_driver" output="screen" clear_params="true">
<!-- Modifies the drone's onboard parameters. If not specified, drone default will be used (consult SDK or ardrone_autonomy wiki) -->
<param name="outdoor" value="1" />
<param name="flight_without_shell" value="1" />
<param name="navdata_demo" value="0" />
<param name="max_bitrate" value="2000" />
<param name="bitrate" value="2000" />
<param name="video_codec" value="129" />
<param name="altitude_max" value="5000" />
<param name="altitude_min" value="300" />
<param name="control_vz_max" value="2000" />
<param name="control_yaw" value="6.11" />
<param name="euler_angle_max" value="0.35" />
<param name="detect_type" value="10" />
<param name="detections_select_h" value="128" />
<param name="detections_select_v_hsync" value="32" />
<param name="enemy_colors" value="3" />
<param name="enemy_without_shell" value="0" />
<param name="do_imu_caliberation" value="false" />
<param name="tf_prefix" value="mydrone" />
<!-- Enables the standard /ardrone/navdata, imu and mag topics. If not specified, defaults to TRUE -->
<param name="enable_legacy_navdata" value="true" />
<!-- Enables the new-style, full information navdata packets. If not specified, defaults to FALSE -->
<param name="enable_navdata_demo" value="true" />
<param name="enable_navdata_time" value="true" />
<param name="enable_navdata_raw_measures" value="true" />
<param name="enable_navdata_phys_measures" value="true" />
<param name="enable_navdata_gyros_offsets" value="true" />
<param name="enable_navdata_euler_angles" value="true" />
<param name="enable_navdata_references" value="true" />
<param name="enable_navdata_trims" value="true" />
<param name="enable_navdata_rc_references" value="true" />
<param name="enable_navdata_pwm" value="true" />
<param name="enable_navdata_altitude" value="true" />
<param name="enable_navdata_vision_raw" value="true" />
<param name="enable_navdata_vision_of" value="true" />
<param name="enable_navdata_vision" value="true" />
<param name="enable_navdata_vision_perf" value="true" />
<param name="enable_navdata_trackers_send" value="true" />
<param name="enable_navdata_vision_detect" value="true" />
<param name="enable_navdata_watchdog" value="true" />
<param name="enable_navdata_adc_data_frame" value="true" />
<param name="enable_navdata_video_stream" value="true" />
<param name="enable_navdata_games" value="true" />
<param name="enable_navdata_pressure_raw" value="true" />
<param name="enable_navdata_magneto" value="true" />
<param name="enable_navdata_wind_speed" value="true" />
<param name="enable_navdata_kalman_pressure" value="true" />
<param name="enable_navdata_hdvideo_stream" value="true" />
<param name="enable_navdata_wifi" value="true" />
<param name="enable_navdata_zimmu_3000" value="true" />
<!-- Tunes the speed at which the ros loop runs, and thus, the rate at which navdata is published -->
<param name="looprate" value="50" />
<!-- Do we want to publish new-style navdata when received (true), or every time the ros-loop runs (false)? -->
<!-- (does not affect legacy navdata, which is always published at ros-loop rate) -->
<param name="realtime_navdata" value="true" />
<param name="realtime_video" value="true" />
<!-- Covariance Values (3x3 matrices reshaped to 1x9)-->
<rosparam param="cov/imu_la">[0.1, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.1]</rosparam>
<rosparam param="cov/imu_av">[1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]</rosparam>
<rosparam param="cov/imu_or">[1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 100000.0]</rosparam>
</node>
</launch>

26
launch/essai-commande.launch
View File

@ -1,26 +0,0 @@
<launch>
<node name="filtre_z" pkg="hand_control" type="filtre">
<remap from="/filtre/input" to="/random_pcl"/>
</node>
<param name="/random/freq" value="10.0" type="double" />
<param name="/random/length" value="25" type="int" />
<node name="publisher" pkg="hand_control" type="random_pcl_publisher" >
<remap from="/random/output" to="/random_pcl"/>
</node>
<node name="estimator" pkg="hand_control" type="normal_estimator">
<remap from="/estimator/input" to="/filtre/output"/>
</node>
<param name="/commande/min_number" value="5" type="int"/>
<param name="/commande/x_dev_min" value="0." type="double"/>
<param name="/commande/y_dev_min" value="0." type="double"/>
<param name="/commande/dz_dev_min" value="0." type="double"/>
<param name="/commande/max_curv" value="0.3" type="double"/>
<node name="commande" pkg="hand_control" type="commande">
<remap from="/commande/input" to="/estimator/output"/>
</node>
</launch>

10
launch/essai-estimator.launch
View File

@ -1,10 +0,0 @@
<launch>
<include file="$(find hand_control)/launch/essai-filtre.launch"/>
<node name="estimator" pkg="hand_control" type="normal_estimator">
<remap from="/estimator/input" to="/filtre/output"/>
</node>
<!-- <param name="/random/freq" value="100." type="double" /> -->
</launch>

11
launch/essai-filtre.launch
View File

@ -1,11 +0,0 @@
<launch>
<node name="filtre_z" pkg="hand_control" type="filtre">
<remap from="/filtre/input" to="/random_pcl"/>
</node>
<node name="publisher" pkg="hand_control" type="random_pcl_publisher" >
<remap from="/random/output" to="/random_pcl"/>
</node>
</launch>

17
launch/kinect_commander.launch
View File

@ -0,0 +1,17 @@
<launch>
<include file="$(find hand_control)/launch/freenect-registered-xyzrgb.launch"/>
<node name="filter" pkg="hand_control" type="filter">
<remap from="/filter/input" to="/camera/xyzrgb/points"/>
</node>
<node name="estimator" pkg="hand_control" type="estimator">
<remap from="/estimator/input" to="/filter/output"/>
</node>
<node name="commander" pkg="hand_control" type="commander">
<remap from="/commander/input" to="/estimator/output"/>
</node>
</launch>

230
src/commande-new-1d.cpp
View File

@ -1,230 +0,0 @@
#include <ros/ros.h>
#include <ros/time.h>
#include <locale.h>
#include <limits>
#include <math.h>
#include <assert.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>
#include <hand_control/Plan.h>
#include <geometry_msgs/Twist.h>
#include <math.h>
class Run
{
private:
float xx, yy, dz;
// xx < 0 : forward
// xx > 0 : backward
// yy > 0 : right
// yy < 0 : left
// dz > 0 : up
// dz < 0 : down
float plan_vel, z_vel, up_factor;
// to calculate dz
float z_current, z_previous;
ros::Time t_current, t_previous;
// conditions to publish a message
float max_curv;
float dz_min, x_dev_min, y_dev_min;
uint64_t min_number;
bool first_msg;
ros::Publisher pub;
void publish()
{
geometry_msgs::Twist::Ptr mvt(new geometry_msgs::Twist());
if (fabs(dz) > dz_min)
{
if (dz > 0)
mvt->linear.z = dz * z_vel * up_factor ;
else
mvt->linear.z = dz * z_vel;
}
if (fabs(yy) > fabs(xx) && fabs(yy) > y_dev_min)
{
mvt->linear.y = yy * plan_vel;
}
else if (fabs(xx) > x_dev_min)
{
mvt->linear.x = - xx * plan_vel;
}
assert(mvt->linear.x == 0. || mvt->linear.y == 0.);
pub.publish(mvt);
ROS_INFO("cmd published");
}//end publish
public:
Run(const ros::Publisher& cmd_publisher, const float& max_curvature, const float& plan_velocity,
const float& z_velocity, const float& x_minimal_deviation, const float& y_minimal_deviation,
const float& dz_minimal_difference, const int& min_points_number, const float& up_fact) :
pub(cmd_publisher),
plan_vel(plan_velocity),
max_curv(max_curvature),
z_vel(z_velocity),
xx(0),
yy(0),
dz(0),
x_dev_min(x_minimal_deviation),
y_dev_min(y_minimal_deviation),
dz_min(dz_minimal_difference),
first_msg(true),
min_number(min_points_number),
up_factor(up_fact) {
z_current = z_previous = std::numeric_limits<float>::signaling_NaN();
t_previous.nsec = t_previous.sec =
t_previous.nsec = t_previous.sec = std::numeric_limits<uint32_t>::signaling_NaN();
}
void callback(const hand_control::Plan::ConstPtr& msg)
{
ROS_INFO("plan received");
if (msg->curvature < max_curv && msg->number > min_number)
{
t_current = msg->header.stamp;
z_current = msg->altitude;
if (!first_msg)
{
dz = (z_current - z_previous)/((t_current - t_previous).toSec());
ROS_INFO("dz = %f", dz);
}
if(msg->normal.z > 0)
{
yy = msg->normal.x;
xx = msg->normal.y;
}
else
{
yy = - msg->normal.x;
xx = - msg->normal.y;
}
t_previous = t_current;
z_previous = z_current;
z_current = std::numeric_limits<float>::signaling_NaN();
t_current.nsec = t_current.sec = std::numeric_limits<uint32_t>::signaling_NaN();
if (first_msg)
{
first_msg = false;
ROS_INFO("first msg received");
}
ROS_INFO("coords updated");
} else {
xx = yy = dz = 0.;
}
publish();
};
void run()
{
ros::spin();
}
};
int main(int argc, char** argv)
{
ros::init(argc, argv, "commande");
ros::NodeHandle node("commande");
double max_curv(0);
if (node.getParam("max_curv", max_curv))
{
ROS_INFO("max_curv : %f" , max_curv);
} else {
node.setParam("max_curv", 0.08);
node.getParam("max_curv", max_curv);
ROS_INFO("max_curv : %f (default value)", max_curv);
}
double plan_vel(0);
if (node.getParam("plan_vel", plan_vel))
{
ROS_INFO("plan_vel : %f" , plan_vel);
} else {
node.setParam("plan_vel", 0.8);
node.getParam("plan_vel", plan_vel);
ROS_INFO("plan_vel : %f (default value)", plan_vel);
}
double z_vel(0);
if (node.getParam("z_vel", z_vel))
{
ROS_INFO("z_vel : %f" , z_vel);
} else {
node.setParam("z_vel", 0.8);
node.getParam("z_vel", z_vel);
ROS_INFO("z_vel : %f (default value)", z_vel);
}
int min_number(0);
if (node.getParam("min_number", min_number))
{
ROS_INFO("min_number : %d" , min_number);
} else {
node.setParam("min_number", 500);
node.getParam("min_number", min_number);
ROS_INFO("min_number : %d (default value)", min_number);
}
double x_dev_min(0);
if (node.getParam("x_dev_min", x_dev_min))
{
ROS_INFO("x_dev_min : %f" , x_dev_min);
} else {
node.setParam("x_dev_min", 0.05);
node.getParam("x_dev_min", x_dev_min);
ROS_INFO("x_dev_min : %f (default value)", x_dev_min);
}
double y_dev_min(0);
if (node.getParam("y_dev_min", y_dev_min))
{
ROS_INFO("y_dev_min : %f" , y_dev_min);
} else {
node.setParam("y_dev_min", 0.05);
node.getParam("y_dev_min", y_dev_min);
ROS_INFO("y_dev_min : %f (default value)", y_dev_min);
}
double dz_dev_min(0);
if (node.getParam("dz_dev_min", dz_dev_min))
{
ROS_INFO("dz_dev_min : %f" , dz_dev_min);
} else {
node.setParam("dz_dev_min", 0.05);
node.getParam("dz_dev_min", dz_dev_min);
ROS_INFO("dz_dev_min : %f (default value)", dz_dev_min);
}
double up_fact(0);
if (node.getParam("up_fact", up_fact))
{
ROS_INFO("up_fact : %f" , up_fact);
} else {
node.setParam("up_fact", 1.5);
node.getParam("up_fact", up_fact);
ROS_INFO("up_fact : %f (default value)", up_fact);
}
ros::Publisher cmd_pub = node.advertise<geometry_msgs::Twist>("/cmd_vel", 1);
Run run(cmd_pub, max_curv, plan_vel, z_vel, x_dev_min, y_dev_min, dz_dev_min, min_number, up_fact);
ros::Subscriber plan_sub = node.subscribe<hand_control::Plan>("input", 1, &Run::callback, &run);
run.run();
return 0;
}

12
src/commande-abs.cpp → src/commander.cpp
View File

@ -13,7 +13,7 @@
#include <math.h> #include <math.h>
#include <dynamic_reconfigure/server.h> #include <dynamic_reconfigure/server.h>
#include <hand_control/CommandeConfig.h> #include <hand_control/CommanderConfig.h>
class Run class Run
@ -111,7 +111,7 @@ class Run
publish(); publish();
}; };
void reconfigure(const hand_control::CommandeConfig& c, const uint32_t& level) { void reconfigure(const hand_control::CommanderConfig& c, const uint32_t& level) {
max_curv = c.max_curvature; max_curv = c.max_curvature;
x_dev_min = c.x_minimal_deviation; x_dev_min = c.x_minimal_deviation;
y_dev_min = c.y_minimal_deviation; y_dev_min = c.y_minimal_deviation;
@ -134,15 +134,15 @@ class Run
int main(int argc, char** argv) int main(int argc, char** argv)
{ {
ros::init(argc, argv, "commande"); ros::init(argc, argv, "commander");
ros::NodeHandle node("commande"); ros::NodeHandle node("commander");
ros::Publisher cmd_pub = node.advertise<geometry_msgs::Twist>("/cmd_vel", 1); ros::Publisher cmd_pub = node.advertise<geometry_msgs::Twist>("/cmd_vel", 1);
Run run(cmd_pub); Run run(cmd_pub);
ros::Subscriber plan_sub = node.subscribe<hand_control::Plan>("input", 1, &Run::callback, &run); ros::Subscriber plan_sub = node.subscribe<hand_control::Plan>("input", 1, &Run::callback, &run);
dynamic_reconfigure::Server<hand_control::CommandeConfig> server; dynamic_reconfigure::Server<hand_control::CommanderConfig> server;
dynamic_reconfigure::Server<hand_control::CommandeConfig>::CallbackType f; dynamic_reconfigure::Server<hand_control::CommanderConfig>::CallbackType f;
f = boost::bind(&Run::reconfigure, &run, _1, _2); f = boost::bind(&Run::reconfigure, &run, _1, _2);
server.setCallback(f); server.setCallback(f);

0
src/normal_estimator-pca.cpp → src/estimator.cpp
View File

12
src/filtre.cpp → src/filter.cpp
View File

@ -4,7 +4,7 @@
#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 <dynamic_reconfigure/server.h>
#include <hand_control/FiltreConfig.h> #include <hand_control/FilterConfig.h>
typedef pcl::PointXYZRGB Point; typedef pcl::PointXYZRGB Point;
@ -43,7 +43,7 @@ class Callback {
} }
void void
reconfigure(const hand_control::FiltreConfig& c, const uint32_t& level) { reconfigure(const hand_control::FilterConfig& c, const uint32_t& level) {
z_max = c.z_max; z_max = c.z_max;
hue = c.hue; hue = c.hue;
delta_hue = c.delta_hue; delta_hue = c.delta_hue;
@ -95,16 +95,16 @@ class Callback {
int int
main(int argc, char** argv) main(int argc, char** argv)
{ {
ros::init(argc, argv, "filtre"); ros::init(argc, argv, "filter");
ros::NodeHandle node("filtre"); ros::NodeHandle node("filter");
// initialisation // initialisation
ros::Publisher publisher = node.advertise<PointCloud>("output", 1); ros::Publisher publisher = node.advertise<PointCloud>("output", 1);
Callback my_callback(publisher); Callback my_callback(publisher);
ros::Subscriber subscriber = node.subscribe<PointCloud>("input", 1, &Callback::callback, &my_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::FilterConfig> server;
dynamic_reconfigure::Server<hand_control::FiltreConfig>::CallbackType f; dynamic_reconfigure::Server<hand_control::FilterConfig>::CallbackType f;
f = boost::bind(&Callback::reconfigure, &my_callback, _1, _2); f = boost::bind(&Callback::reconfigure, &my_callback, _1, _2);
server.setCallback(f); server.setCallback(f);

94
src/normal_estimator.cpp
View File

@ -1,94 +0,0 @@
#include <ros/ros.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/features/normal_3d_omp.h>
#include <hand_control/Plan.h>
#include <time.h>
typedef pcl::PointXYZRGB Point;
typedef pcl::PointCloud<Point> PointCloud;
class Callback {
public:
void
operator()(const PointCloud::ConstPtr& msg)
{
ROS_INFO("PointCloud received");
float x, y, z, h, c;
x = y = z = h = c = 0.;
// indices : tout le PointCloud
std::vector<int> indices;
for (int i = 0; i < msg->points.size(); ++i)
indices.push_back(i);
// vérifier signature
estimator.computePointNormal(*msg, indices, x, y, z, c);
h = altitude(msg);
// publication
ROS_INFO("Plan published");
publisher.publish(to_Plan(x, y, z, h, c, msg->header.seq, msg->header.stamp, msg->width));
}
Callback(ros::Publisher& pub) :
publisher(pub), estimator() {}
private:
ros::Publisher publisher;
pcl::NormalEstimationOMP<Point, pcl::Normal> estimator;
inline
const hand_control::Plan::ConstPtr
to_Plan(const float& x, const float& y,
const float& z, const float& h,
const float& c, const uint32_t& seq,
const uint64_t& msec64, const uint64_t& number)
{
hand_control::Plan::Ptr ros_msg(new hand_control::Plan());
ros_msg->normal.x = x;
ros_msg->normal.y = y;
ros_msg->normal.z = z;
ros_msg->altitude = h;
ros_msg->curvature = c;
ros_msg->number = number;
// uint64_t msec64 is in ms (10-6)
uint64_t sec64 = msec64 / 1000000;
uint64_t nsec64 = (msec64 % 1000000) * 1000;
ros_msg->header.stamp.sec = (uint32_t) sec64;
ros_msg->header.stamp.nsec = (uint32_t) nsec64;
ros_msg->header.seq = seq;
ros_msg->header.frame_id = "0";
return ros_msg;
}
inline
float
altitude(const PointCloud::ConstPtr& pcl)
{
int s = pcl->points.size();
float h(0);
for (int i = 0; i < s; ++i)
h += pcl->points[i].z;
return h/( (float) s );
}
};
int
main(int argc, char** argv)
{
ros::init(argc, argv, "normal_estimator");
ros::NodeHandle node("estimator");
// initialisation
ros::Publisher publisher = node.advertise<hand_control::Plan>("output", 1);
Callback callback(publisher);
ros::Subscriber subscriber = node.subscribe<PointCloud>("input", 1, callback);
// démarrage
ROS_INFO("node started");
ros::spin();
ROS_INFO("exit");
return 0;
}
Write Preview
Loading…
Cancel
Save