corrects error (s/x/m_x) and add a parameter to inverse x and y axes
This commit is contained in:
parent
264dca57e2
commit
58e6cd4e6e
3 changed files with 33 additions and 4 deletions
|
@ -61,10 +61,12 @@ add_dependencies(commander hand_control_generate_messages_cpp)
|
||||||
generate_dynamic_reconfigure_options(
|
generate_dynamic_reconfigure_options(
|
||||||
cfg/Filter.cfg
|
cfg/Filter.cfg
|
||||||
cfg/Commander.cfg
|
cfg/Commander.cfg
|
||||||
|
cfg/Estimator.cfg
|
||||||
)
|
)
|
||||||
|
|
||||||
add_dependencies(filter ${PROJECT_NAME}_gencfg)
|
add_dependencies(filter ${PROJECT_NAME}_gencfg)
|
||||||
add_dependencies(commander ${PROJECT_NAME}_gencfg)
|
add_dependencies(commander ${PROJECT_NAME}_gencfg)
|
||||||
|
add_dependencies(estimator ${PROJECT_NAME}_gencfg)
|
||||||
|
|
||||||
catkin_package(
|
catkin_package(
|
||||||
CATKIN_DEPENDS message_runtime
|
CATKIN_DEPENDS message_runtime
|
||||||
|
|
6
cfg/Estimator.cfg
Executable file
6
cfg/Estimator.cfg
Executable 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"))
|
|
@ -5,6 +5,8 @@
|
||||||
#include <hand_control/Plan.h>
|
#include <hand_control/Plan.h>
|
||||||
#include <time.h>
|
#include <time.h>
|
||||||
#include <math.h>
|
#include <math.h>
|
||||||
|
#include <dynamic_reconfigure/server.h>
|
||||||
|
#include <hand_control/EstimatorConfig.h>
|
||||||
|
|
||||||
#include <pcl/common/pca.h>
|
#include <pcl/common/pca.h>
|
||||||
|
|
||||||
|
@ -43,9 +45,17 @@ class Callback {
|
||||||
// this formula is good only if :
|
// this formula is good only if :
|
||||||
// -pi/2 <= th <= pi/2
|
// -pi/2 <= th <= pi/2
|
||||||
// ie cos(th) == m_x >= 0
|
// ie cos(th) == m_x >= 0
|
||||||
float m_x(eg(0,0));
|
float m_x, m_y;
|
||||||
float m_y(eg(1,0));
|
if (!reverse)
|
||||||
if(x < 0)
|
{
|
||||||
|
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_x *= -1.;
|
||||||
m_y *= -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:
|
private:
|
||||||
ros::Publisher publisher;
|
ros::Publisher publisher;
|
||||||
pcl::PCA<Point> analyser;
|
pcl::PCA<Point> analyser;
|
||||||
const float _RAD2DEG;
|
const float _RAD2DEG;
|
||||||
|
bool reverse;
|
||||||
|
|
||||||
inline const hand_control::Plan::ConstPtr
|
inline const hand_control::Plan::ConstPtr
|
||||||
to_Plan(const float& x, const float& y,
|
to_Plan(const float& x, const float& y,
|
||||||
|
@ -104,6 +120,11 @@ int main(int argc, char** argv)
|
||||||
Callback callback(publisher);
|
Callback callback(publisher);
|
||||||
ros::Subscriber subscriber = node.subscribe<PointCloud>("input", 1, callback);
|
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_INFO("node started");
|
||||||
ros::spin();
|
ros::spin();
|
||||||
ROS_INFO("exit");
|
ROS_INFO("exit");
|
||||||
|
|
Loading…
Reference in a new issue