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

it works :-) yaml with naked hand

Browse source
This commit is contained in:
Louis-Guillaume DUBOIS 2015-05-28 19:10:13 +02:00
parent 58e6cd4e6e
commit 1b601b9f22
6 changed files with 113 additions and 23 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

1
cfg/Estimator.cfg
View file

@ -3,4 +3,5 @@ PACKAGE = "hand_control"
from dynamic_reconfigure.parameter_generator_catkin import *
gen = ParameterGenerator()
gen.add("reverse", bool_t, 0, "Pose the kinect in parallel with the arm")
gen.add("reverse_angle", bool_t, 0, "Change the angle sign")
exit(gen.generate(PACKAGE, "hand_control", "Estimator"))

4
cfg/Filter.cfg
View file

@ -2,8 +2,8 @@
PACKAGE = "hand_control"
from dynamic_reconfigure.parameter_generator_catkin import *
gen = ParameterGenerator()
gen.add("z_max", double_t, 0, "The maximal altitude of the hand", 50., 0.)
gen.add("hue", double_t, 0, "The color hue of the hand", 0., 0., 360.)
gen.add("z_max", double_t, 0, "The maximal altitude of the hand", 1.9, 0., 50.)
gen.add("hue", double_t, 0, "The color hue of the hand", 140., 0., 360.)
gen.add("delta_hue", double_t, 0, "The tolerance of the hue filter", 20., 0., 180.)
gen.add("sat_min", double_t, 0, "The minimum color saturation of the hand", 0.1, 0., 1.)
gen.add("sat_max", double_t, 0, "The maximum color saturation of the hand", 1., 0., 1.)

37
commander.yaml Normal file
View file

@ -0,0 +1,37 @@
!!python/object/new:dynamic_reconfigure.encoding.Config
dictitems:
angle_vel: 0.01
groups: !!python/object/new:dynamic_reconfigure.encoding.Config
dictitems:
angle_vel: 0.01
groups: !!python/object/new:dynamic_reconfigure.encoding.Config
state: []
id: 0
max_curvature: 0.5
min_points_number: 1000
name: Default
neutral_alt: 0.8
parameters: !!python/object/new:dynamic_reconfigure.encoding.Config
state: []
parent: 0
plan_vel: 0.5
state: true
theta_minimal_deviation: 10.0
type: ''
up_fact: 1.5
x_minimal_deviation: 0.2
y_minimal_deviation: 0.2
z_minimal_deviation: 0.05
z_vel: 2.0
state: []
max_curvature: 0.5
min_points_number: 1000
neutral_alt: 0.8
plan_vel: 0.5
theta_minimal_deviation: 10.0
up_fact: 1.5
x_minimal_deviation: 0.2
y_minimal_deviation: 0.2
z_minimal_deviation: 0.05
z_vel: 2.0
state: []

19
estimator.yaml Normal file
View file

@ -0,0 +1,19 @@
!!python/object/new:dynamic_reconfigure.encoding.Config
dictitems:
groups: !!python/object/new:dynamic_reconfigure.encoding.Config
dictitems:
groups: !!python/object/new:dynamic_reconfigure.encoding.Config
state: []
id: 0
name: Default
parameters: !!python/object/new:dynamic_reconfigure.encoding.Config
state: []
parent: 0
reverse: false
reverse_angle: true
state: true
type: ''
state: []
reverse: false
reverse_angle: true
state: []

29
filter.yaml Normal file
View file

@ -0,0 +1,29 @@
!!python/object/new:dynamic_reconfigure.encoding.Config
dictitems:
delta_hue: 10.0
groups: !!python/object/new:dynamic_reconfigure.encoding.Config
dictitems:
delta_hue: 10.0
groups: !!python/object/new:dynamic_reconfigure.encoding.Config
state: []
hue: 0.0
id: 0
name: Default
parameters: !!python/object/new:dynamic_reconfigure.encoding.Config
state: []
parent: 0
sat_max: 1.0
sat_min: 0.13
state: true
type: ''
val_max: 1.0
val_min: 0.31
z_max: 2.0
state: []
hue: 0.0
sat_max: 1.0
sat_min: 0.13
val_max: 1.0
val_min: 0.31
z_max: 2.0
state: []

46
src/estimator.cpp
View file

@ -16,7 +16,7 @@ typedef Eigen::Matrix3f& Matrix;
class Callback {
public:
void operator()(const PointCloud::ConstPtr& msg)
void callback(const PointCloud::ConstPtr& msg)
{
ROS_INFO("PointCloud received");
@ -37,7 +37,13 @@ class Callback {
Eigen::Vector3f v = eg.col(0).cross(eg.col(1));
// norm(v) == 1
v.normalize();
x = v(0); y=v(1); z=v(2);
if (!reverse)
{
x = v(0); y=v(1);
} else {
x = v(1); y = v(0);
}
z=v(2);
// h is the altitude
h = (analyser.getMean())(2);
@ -46,23 +52,20 @@ class Callback {
// -pi/2 <= th <= pi/2
// ie cos(th) == m_x >= 0
float m_x, m_y;
if (!reverse)
if (!reverse_angle)
{
m_x = eg(0,0);
m_y = eg(1,0);
m_x = eg(0,0);
m_y = eg(1,0);
} else {
m_x = eg(1,0);
m_y = eg(0,0);
m_x = eg(1,0);
m_y = eg(0,0);
}
if(m_x < 0)
{
m_x *= -1.;
m_y *= -1.;
}
th = 2 * atan(m_y / (1 + m_x));
// -pi/2 <= th <= pi/2
if (m_x < 0.)
m_y *= -1;
th = - asin(m_y / sqrt(pow(m_y,2)+ pow(m_x,2)));
// 0 <= th <= pi
th *= _RAD2DEG;
// -90 <= th <= 90
@ -72,18 +75,19 @@ class Callback {
}
}
Callback(ros::Publisher& pub):publisher(pub), _RAD2DEG(45.f/atan(1.)), reverse(false) {};
Callback(ros::Publisher& pub):publisher(pub), _RAD2DEG(45.f/atan(1.)), reverse(false), reverse_angle(false) {};
void reconfigure(const hand_control::EstimatorConfig& c, const uint32_t& level) {
reverse = c.reverse ;
}
void reconfigure(const hand_control::EstimatorConfig& c, const uint32_t& level) {
reverse = c.reverse ;
reverse_angle = c.reverse_angle;
}
private:
ros::Publisher publisher;
pcl::PCA<Point> analyser;
const float _RAD2DEG;
bool reverse;
bool reverse, reverse_angle;
inline const hand_control::Plan::ConstPtr
to_Plan(const float& x, const float& y,
@ -118,7 +122,7 @@ int main(int argc, char** argv)
ros::Publisher publisher = node.advertise<hand_control::Plan>("output", 1);
Callback callback(publisher);
ros::Subscriber subscriber = node.subscribe<PointCloud>("input", 1, callback);
ros::Subscriber subscriber = node.subscribe<PointCloud>("input", 1, &Callback::callback, &callback);
dynamic_reconfigure::Server<hand_control::EstimatorConfig> server;
dynamic_reconfigure::Server<hand_control::EstimatorConfig>::CallbackType f;