diff --git a/cfg/Estimator.cfg b/cfg/Estimator.cfg index 711ac64..9ebb11a 100755 --- a/cfg/Estimator.cfg +++ b/cfg/Estimator.cfg @@ -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")) diff --git a/cfg/Filter.cfg b/cfg/Filter.cfg index 5ca5303..ab4ddae 100755 --- a/cfg/Filter.cfg +++ b/cfg/Filter.cfg @@ -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.) diff --git a/commander.yaml b/commander.yaml new file mode 100644 index 0000000..bcd81e1 --- /dev/null +++ b/commander.yaml @@ -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: [] diff --git a/estimator.yaml b/estimator.yaml new file mode 100644 index 0000000..ff65b71 --- /dev/null +++ b/estimator.yaml @@ -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: [] diff --git a/filter.yaml b/filter.yaml new file mode 100644 index 0000000..ca1a6be --- /dev/null +++ b/filter.yaml @@ -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: [] diff --git a/src/estimator.cpp b/src/estimator.cpp index 220210a..381b892 100644 --- a/src/estimator.cpp +++ b/src/estimator.cpp @@ -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 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("output", 1); Callback callback(publisher); - ros::Subscriber subscriber = node.subscribe("input", 1, callback); + ros::Subscriber subscriber = node.subscribe("input", 1, &Callback::callback, &callback); dynamic_reconfigure::Server server; dynamic_reconfigure::Server::CallbackType f;