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

commande now must respect good signs

Browse source
This commit is contained in:
Louis-Guillaume DUBOIS 2015-05-12 18:42:55 +02:00
parent a373c47484
commit 329295a788
1 changed files with 18 additions and 9 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

27
src/commande.cpp
View file

@ -17,8 +17,17 @@ class Run
ros::Rate loop_rate;
// xx*plan_vel, yy*plan_vel and dz*dz_vel will be published
float xx, yy, dz;
// xx > 0 : forward
// xx < 0 : backward
// yy > 0 : right
// yy < 0 : left
// dz > 0 : up
// dz < 0 : down
float plan_vel, z_vel;
// to calculate dz
@ -44,13 +53,11 @@ class Run
}
if (fabs(xx) > x_dev_min)
{
mvt->linear.y = xx * plan_vel;
// because of the kinect orientation
mvt->linear.x = xx * plan_vel;
}
if (fabs(yy) > y_dev_min)
{
mvt->linear.x = yy * plan_vel;
// because of the kinect orientation
mvt->linear.y = yy * plan_vel;
}
pub.publish(mvt);
ROS_INFO("cmd published");
@ -92,10 +99,12 @@ class Run
ROS_INFO("dz = %f", dz);
}
xx = msg->normal.x;
ROS_INFO("xx = %f", xx);
yy = msg->normal.y;
ROS_INFO("yy = %f", yy);
if(msg->normal.z > 0)
yy = msg->normal.x;
xx = msg->normal.y;
else
yy = - msg->normal.x;
xx = - msg->normal.y;
t_previous = t_current;
z_previous = z_current;