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

Transmet theta à cmd_vel

Browse source
This commit is contained in:
_Luc_ 2015-05-27 20:55:04 +02:00
parent 5a38dae71c
commit acde40fcdb
1 changed files with 6 additions and 3 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

9
src/commande-abs.cpp
View file

@ -20,6 +20,7 @@ class Run
{ {
private: private:
float xx, yy, zz, theta; float xx, yy, zz, theta;
float DEMI_PI;
// xx < 0 : forward // xx < 0 : forward
// xx > 0 : backward // xx > 0 : backward
@ -62,7 +63,7 @@ class Run
} }
if (fabs(theta) > th_dev_min) { if (fabs(theta) > th_dev_min) {
mvt->angular.z * angle_vel; mvt->angular.z = theta * angle_vel;
} }
assert(mvt->linear.x == 0. || mvt->linear.y == 0.); assert(mvt->linear.x == 0. || mvt->linear.y == 0.);
@ -73,7 +74,9 @@ class Run
public: public:
Run(const ros::Publisher& cmd_publisher) : Run(const ros::Publisher& cmd_publisher) :
pub(cmd_publisher) pub(cmd_publisher)
{} {
DEMI_PI = 2*atan(1.);
}
void callback(const hand_control::Plan::ConstPtr& msg) void callback(const hand_control::Plan::ConstPtr& msg)
{ {
@ -94,7 +97,7 @@ class Run
zz = msg->altitude - neutral_z; zz = msg->altitude - neutral_z;
theta = msg->angle; theta = msg->angle - DEMI_PI;
if (first_msg) if (first_msg)
{ {