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

it works :-) yaml with naked hand

master
Louis-Guillaume DUBOIS 11 years ago
parent
58e6cd4e6e
commit
1b601b9f22
6 changed files with 113 additions and 23 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 1
      cfg/Estimator.cfg
  2. 4
      cfg/Filter.cfg
  3. 37
      commander.yaml
  4. 19
      estimator.yaml
  5. 29
      filter.yaml
  6. 46
      src/estimator.cpp

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

Write Preview
Loading…
Cancel
Save