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

better english node and file naming, & rm useless files

Browse source
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
Show stats Download patch file Download diff file Expand all files Collapse all files

24
CMakeLists.txt
View file

@ -35,8 +35,8 @@ pkg_check_modules ( ncursesw REQUIRED ncursesw)
${ncursesw_INCLUDE_DIRS}
)
add_executable(filtre src/filtre.cpp)
target_link_libraries(filtre ${catkin_LIBRARIES})
add_executable(filter src/filter.cpp)
target_link_libraries(filter ${catkin_LIBRARIES})
add_executable(random_pcl_publisher src/random_pcl_publisher.cpp)
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)
target_link_libraries(keyboard_cmd display ${catkin_LIBRARIES} ${ncursesw_LIBRARIES})
add_executable(normal_estimator_pca src/normal_estimator-pca.cpp)
target_link_libraries(normal_estimator_pca ${catkin_LIBRARIES})
add_dependencies(normal_estimator_pca hand_control_generate_messages_cpp)
add_executable(estimator src/estimator.cpp)
target_link_libraries(estimator ${catkin_LIBRARIES})
add_dependencies(estimator hand_control_generate_messages_cpp)
add_executable(commande-abs src/commande-abs.cpp)
target_link_libraries(commande-abs ${catkin_LIBRARIES})
add_dependencies(commande-abs hand_control_generate_messages_cpp)
add_executable(commander src/commander.cpp)
target_link_libraries(commander ${catkin_LIBRARIES})
add_dependencies(commander hand_control_generate_messages_cpp)
#add dynamic reconfigure api
generate_dynamic_reconfigure_options(
cfg/Filtre.cfg
cfg/Commande.cfg
cfg/Filter.cfg
cfg/Commander.cfg
)
add_dependencies(filtre ${PROJECT_NAME}_gencfg)
add_dependencies(commande-abs ${PROJECT_NAME}_gencfg)
add_dependencies(filter ${PROJECT_NAME}_gencfg)
add_dependencies(commander ${PROJECT_NAME}_gencfg)
catkin_package(
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("plan_vel", double_t, 0, "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("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.)
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 Normal file
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 <dynamic_reconfigure/server.h>
#include <hand_control/CommandeConfig.h>
#include <hand_control/CommanderConfig.h>
class Run
@ -111,7 +111,7 @@ class Run
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;
x_dev_min = c.x_minimal_deviation;
y_dev_min = c.y_minimal_deviation;
@ -134,15 +134,15 @@ class Run
int main(int argc, char** argv)
{
ros::init(argc, argv, "commande");
ros::NodeHandle node("commande");
ros::init(argc, argv, "commander");
ros::NodeHandle node("commander");
ros::Publisher cmd_pub = node.advertise<geometry_msgs::Twist>("/cmd_vel", 1);
Run run(cmd_pub);
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::CommandeConfig>::CallbackType f;
dynamic_reconfigure::Server<hand_control::CommanderConfig> server;
dynamic_reconfigure::Server<hand_control::CommanderConfig>::CallbackType f;
f = boost::bind(&Run::reconfigure, &run, _1, _2);
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 <assert.h>
#include <dynamic_reconfigure/server.h>
#include <hand_control/FiltreConfig.h>
#include <hand_control/FilterConfig.h>
typedef pcl::PointXYZRGB Point;
@ -43,7 +43,7 @@ class Callback {
}
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;
hue = c.hue;
delta_hue = c.delta_hue;
@ -95,16 +95,16 @@ class Callback {
int
main(int argc, char** argv)
{
ros::init(argc, argv, "filtre");
ros::NodeHandle node("filtre");
ros::init(argc, argv, "filter");
ros::NodeHandle node("filter");
// initialisation
ros::Publisher publisher = node.advertise<PointCloud>("output", 1);
Callback my_callback(publisher);
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;
dynamic_reconfigure::Server<hand_control::FilterConfig> server;
dynamic_reconfigure::Server<hand_control::FilterConfig>::CallbackType f;
f = boost::bind(&Callback::reconfigure, &my_callback, _1, _2);
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;
}