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

with «angle» & «theta» between -90 and 90

Browse source
This commit is contained in:
Louis-Guillaume DUBOIS 2015-05-27 23:28:43 +02:00
parent 6f4e6eda41
commit 264dca57e2
3 changed files with 81 additions and 105 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

8
cfg/Commander.cfg
View file

@ -6,11 +6,11 @@ gen.add("max_curvature", double_t, 0, "Maximum curvature of the estimated plane"
gen.add("x_minimal_deviation", double_t, 0, "Absolute horizontal movement detection treshold", 0.05, 0.)
gen.add("y_minimal_deviation", double_t, 0, "Absolute lateral movement detection treshold", 0.05, 0.)
gen.add("z_minimal_deviation", double_t, 0, "Absolute vertical movement detection treshold", 0.1, 0.)
gen.add("neutral_alt", double_t, 0, "Reference altitude for vertical movement command", 1.5, 0.)
gen.add("min_points_number", int_t, 0, "Minimal number of plane points needed for a valid estimation", 3000, 0)
gen.add("neutral_alt", double_t, 0, "Reference altitude for vertical movement command", 0.8, 0.)
gen.add("min_points_number", int_t, 0, "Minimal number of plane points needed for a valid estimation", 2000, 0)
gen.add("up_fact", double_t, 0, "Upward command amplification factor", 1.5, 1)
gen.add("theta_minimal_deviation", double_t, 0, "Absolute angular movement detection treshold", 15., 0., 180.)
gen.add("angle_vel", double_t, 0, "Angular velocity", 0.5, 0., 10.)
gen.add("theta_minimal_deviation", double_t, 0, "Absolute angular movement detection treshold", 5., 0., 45.)
gen.add("angle_vel", double_t, 0, "Angular velocity", 0.002, 0., 10.)
gen.add("plan_vel", double_t, 0, "Translation velocity", 2, 0., 10.)
gen.add("z_vel", double_t, 0, "Vertical translation velocity", 2, 0., 10.)
exit(gen.generate(PACKAGE, "hand_control", "Commander"))

6
src/commander.cpp
View file

@ -20,8 +20,6 @@ class Run
{
private:
float xx, yy, zz, theta;
float DEMI_PI;
// xx < 0 : forward
// xx > 0 : backward
@ -75,7 +73,6 @@ class Run
Run(const ros::Publisher& cmd_publisher) :
pub(cmd_publisher)
{
DEMI_PI = 2*atan(1.);
}
void callback(const hand_control::Plan::ConstPtr& msg)
@ -97,7 +94,8 @@ class Run
zz = msg->altitude - neutral_z;
theta = msg->angle - DEMI_PI;
theta = msg->angle;
// theta between -90 and 90
if (first_msg)
{

80
src/estimator.cpp
View file

@ -14,49 +14,47 @@ typedef Eigen::Matrix3f& Matrix;
class Callback {
public:
void
operator()(const PointCloud::ConstPtr& msg)
void operator()(const PointCloud::ConstPtr& msg)
{
ROS_INFO("PointCloud received");
if (msg->width > 3){
analyser.setInputCloud(msg);
Matrix eg = analyser.getEigenVectors();
ROS_INFO("PointCloud received");
float x, y, z, th, h, c;
x = y = z = th = h = c = 0.;
// indices : tout le PointCloud
// we consider the whole PointCloud
std::vector<int> indices;
for (int i = 0; i < msg->points.size(); ++i)
indices.push_back(i);
// vérifier signature
//estimator.computePointNormal(*msg, indices, x, y, z, c);
//Produit vectoriel des deux première colonnes de Matrix3f
/* x = eg(2,1)*eg(3,2)
- eg(3,1)*eg(2,2);
y = eg(3,1)*eg(1,2)
- eg(1,1)*eg(3,2);
z = eg(1,1)*eg(2,2)
- eg(2,1)*eg(1,2);*/
// v = eg_1 ^ eg_2 is the plan normal
Eigen::Vector3f v = eg.col(0).cross(eg.col(1));
// norm(v) == 1
v.normalize();
/*
x = x/sqrt(x*x+y*y+z*z);
y = x/sqrt(x*x+y*y+z*z);
z = x/sqrt(x*x+y*y+z*z);
*/
x = v(0); y=v(1); z=v(2);
// h is the altitude
h = (analyser.getMean())(2);
//h = altitude(msg);
th =
2 * atan (eg(1,0)
/(1 + eg(0,0)));
// 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)
{
m_x *= -1.;
m_y *= -1.;
}
th = 2 * atan(m_y / (1 + m_x));
// -pi/2 <= th <= pi/2
th *= _RAD2DEG;
// -90 <= th <= 90
// publication
ROS_INFO("Plan published");
@ -64,16 +62,14 @@ class Callback {
}
}
Callback(ros::Publisher& pub) :
publisher(pub) {}
Callback(ros::Publisher& pub):publisher(pub), _RAD2DEG(45.f/atan(1.)){};
private:
ros::Publisher publisher;
pcl::PCA<Point> analyser;
//pcl::NormalEstimationOMP<Point, pcl::Normal> estimator;
const float _RAD2DEG;
inline
const hand_control::Plan::ConstPtr
inline const hand_control::Plan::ConstPtr
to_Plan(const float& x, const float& y,
const float& z, const float& h,
const float& th,
@ -97,35 +93,17 @@ class Callback {
ros_msg->header.frame_id = "0";
return ros_msg;
}
/* inline
float
altitude(const PointCloud::ConstPtr& pcl)
{
int s = pcl->points.size();
float h(0);
for (int i = 0; i < s; ++i)
h += pcl->points[i].z;
return h/( (float) s );
getmean
getVector4fMap
}*/
};
int
main(int argc, char** argv)
int main(int argc, char** argv)
{
ros::init(argc, argv, "normal_estimator_pca");
ros::NodeHandle node("estimator");//`A vérifier ?
ros::init(argc, argv, "estimator");
ros::NodeHandle node("estimator");
// initialisation
ros::Publisher publisher = node.advertise<hand_control::Plan>("output", 1);
Callback callback(publisher);
ros::Subscriber subscriber = node.subscribe<PointCloud>("input", 1, callback);
// démarrage
ROS_INFO("node started");
ros::spin();
ROS_INFO("exit");