better english node and file naming, & rm useless files

This commit is contained in:
Louis-Guillaume DUBOIS 2015-05-27 22:27:57 +02:00
parent 9c10400e54
commit 6f4e6eda41
18 changed files with 43 additions and 574 deletions

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

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"))

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"))

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>

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>

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>

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>

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>

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>

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>

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>

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>

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>

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;
}

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);

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);

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;
}