diff --git a/CMakeLists.txt b/CMakeLists.txt index 9cf5d97..570e32d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -61,10 +61,12 @@ add_dependencies(commander hand_control_generate_messages_cpp) generate_dynamic_reconfigure_options( cfg/Filter.cfg cfg/Commander.cfg + cfg/Estimator.cfg ) add_dependencies(filter ${PROJECT_NAME}_gencfg) add_dependencies(commander ${PROJECT_NAME}_gencfg) +add_dependencies(estimator ${PROJECT_NAME}_gencfg) catkin_package( CATKIN_DEPENDS message_runtime diff --git a/cfg/Estimator.cfg b/cfg/Estimator.cfg new file mode 100755 index 0000000..711ac64 --- /dev/null +++ b/cfg/Estimator.cfg @@ -0,0 +1,6 @@ +#!/usr/bin/env python +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") +exit(gen.generate(PACKAGE, "hand_control", "Estimator")) diff --git a/src/estimator.cpp b/src/estimator.cpp index 80d3b19..220210a 100644 --- a/src/estimator.cpp +++ b/src/estimator.cpp @@ -5,6 +5,8 @@ #include #include #include +#include +#include #include @@ -43,9 +45,17 @@ class Callback { // this formula is good only if : // -pi/2 <= th <= pi/2 // ie cos(th) == m_x >= 0 - float m_x(eg(0,0)); - float m_y(eg(1,0)); - if(x < 0) + float m_x, m_y; + if (!reverse) + { + m_x = eg(0,0); + m_y = eg(1,0); + } else { + m_x = eg(1,0); + m_y = eg(0,0); + } + + if(m_x < 0) { m_x *= -1.; m_y *= -1.; @@ -62,12 +72,18 @@ class Callback { } } - Callback(ros::Publisher& pub):publisher(pub), _RAD2DEG(45.f/atan(1.)){}; + Callback(ros::Publisher& pub):publisher(pub), _RAD2DEG(45.f/atan(1.)), reverse(false) {}; + + + void reconfigure(const hand_control::EstimatorConfig& c, const uint32_t& level) { + reverse = c.reverse ; + } private: ros::Publisher publisher; pcl::PCA analyser; const float _RAD2DEG; + bool reverse; inline const hand_control::Plan::ConstPtr to_Plan(const float& x, const float& y, @@ -104,6 +120,11 @@ int main(int argc, char** argv) Callback callback(publisher); ros::Subscriber subscriber = node.subscribe("input", 1, callback); + dynamic_reconfigure::Server server; + dynamic_reconfigure::Server::CallbackType f; + f = boost::bind(&Callback::reconfigure, &callback, _1, _2); + server.setCallback(f); + ROS_INFO("node started"); ros::spin(); ROS_INFO("exit");