better english node and file naming, & rm useless files
This commit is contained in:
Louis-Guillaume DUBOIS
parent
9c10400e54
commit
6f4e6eda41
18 changed files with 43 additions and 574 deletions
|
|
@ -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
|
||||||
|
|
|
||||||
|
|
@ -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"))
|
||||||
|
|
@ -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"))
|
||||||
|
|
@ -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>
|
|
||||||
|
|
@ -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>
|
|
||||||
|
|
@ -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>
|
|
||||||
|
|
@ -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>
|
|
||||||
|
|
@ -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>
|
|
||||||
|
|
@ -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>
|
|
||||||
|
|
@ -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>
|
|
||||||
|
|
@ -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>
|
|
||||||
|
|
@ -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
17
launch/kinect_commander.launch
Normal 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>
|
||||||
|
|
@ -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;
|
|
||||||
}
|
|
||||||
|
|
@ -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);
|
||||||
|
|
||||||
|
|
@ -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);
|
||||||
|
|
||||||
|
|
@ -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;
|
|
||||||
}
|
|
||||||
Loading…
Reference in a new issue