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

corrects error (s/x/m_x) and add a parameter to inverse x and y axes

Browse source
This commit is contained in:
Louis-Guillaume DUBOIS 2015-05-28 13:24:07 +02:00
parent 264dca57e2
commit 58e6cd4e6e
3 changed files with 33 additions and 4 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

2
CMakeLists.txt
View file

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

6
cfg/Estimator.cfg Executable file
View file

@ -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"))

29
src/estimator.cpp
View file

@ -5,6 +5,8 @@
#include <hand_control/Plan.h>
#include <time.h>
#include <math.h>
#include <dynamic_reconfigure/server.h>
#include <hand_control/EstimatorConfig.h>
#include <pcl/common/pca.h>
@ -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<Point> 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<PointCloud>("input", 1, callback);
dynamic_reconfigure::Server<hand_control::EstimatorConfig> server;
dynamic_reconfigure::Server<hand_control::EstimatorConfig>::CallbackType f;
f = boost::bind(&Callback::reconfigure, &callback, _1, _2);
server.setCallback(f);
ROS_INFO("node started");
ros::spin();
ROS_INFO("exit");