Hello, i am working with an Arduino Mega and a RPI using the rosserial, and i have a couple of doubts. I am working with a differential robot and i am using the Arduino for controlling the motors (i have a subscriber node), and now i need to publish the odometry information back to ROS (my intention is to use the Navigation stack in the future), but i am having a few problems with this. In the first place, i don't know how to create de odometry message (i read the following posts [link text](http://wiki.ros.org/navigation/Tutorials/RobotSetup/Odom) and [link text](https://answers.ros.org/question/241118/odometry-message-from-arduino/) but i still don't know understand this). In second place, once i have create the odometry message, i want to publish it to ros like in this example [link text](http://wiki.ros.org/rosserial_arduino/Tutorials/Hello%20World). Is my code correct for doing this? (only see the publishing parts, the others work well)
Here is the full code
#include #include #include #include "DualVNH5019MotorShield.h" #include #include
const byte pinEnc1a = 21; const byte pinEnc2a = 18; const byte pinEnc1b = 20; const byte pinEnc2b = 19;
int Direc; volatile long paso1=0; volatile long paso2=0; unsigned long ta1, tb1, ta2, tb2, dT1, dT2;
int aux=0;
float R = 0.035; // Radio ruedas
float L = 0.24; // Longitud entre ruedas
volatile float v=0;
volatile float w=0;
double Vr, Vl =0;
double Input1, Output1;
double Input2, Output2;
double Kp=.5;
double Ki=0.3;
double Kd=0;
PID myPID1(&Input1, &Output1, &Vr, Kp, Ki, Kd, DIRECT);
PID myPID2(&Input2, &Output2, &Vl, Kp, Ki, Kd, DIRECT);
DualVNH5019MotorShield md;
ros::NodeHandle nh;
v = mensaje_motor.linear.x;
w = mensaje_motor.angular.z;
Vl = v + (w*L)/2;
Vr = v - (w*L)/2;
if (v==0 && w==0){
aux=1;
md.setM1Speed(0);
md.setM2Speed(0);
while(aux==1){
if (v!=0 || w!=0){
Input1=0;
Input2=0;
ta1=ta2=0;
aux=0;
}
nh.spinOnce();
}
}
}
nav_msgs::Odometry odom;
ros::Publisher odometria ("nav_msgs::Odometry", &odom);
ros::Subscriber sub("cmd_vel", &Cbmotor );
void setup()
{
pinMode(pinEnc1a, INPUT);
attachInterrupt(digitalPinToInterrupt(pinEnc1a), interrupcion1, FALLING);
pinMode(pinEnc2a, INPUT);
attachInterrupt(digitalPinToInterrupt(pinEnc2a), interrupcion2, FALLING);
pinMode(pinEnc1b, INPUT);
pinMode(pinEnc2b, INPUT);
Output1 = 0;
Output2 = 0;
md.init();
myPID1.SetMode(AUTOMATIC);
myPID2.SetMode(AUTOMATIC);
myPID1.SetOutputLimits(-50,50 );
myPID2.SetOutputLimits(-50,50 );
md.setM1Speed(0);
md.setM2Speed(0);
ta1=ta2=micros();
nh.initNode();
nh.subscribe(sub);
md.init();
Serial.begin(57600);
nh.advertise(odometria);
}
void loop()
{
myPID1.Compute();
md.setM1Speed(-(Vr+Output1));
myPID2.Compute();
md.setM2Speed(Vl+Output2);
odometria.publish(&odom);
nh.spinOnce();
}
void interrupcion1()
{
paso1++;
if (paso1%(70*16/6)==0)
{
tb1 = micros();
if (ta1!=0) {
dT1 = tb1-ta1;
Input1= (60000000UL*400)/(dT1*90);
if (Vr<0) {
Input1= -Input1;;
}
}
ta1=tb1;
}
}
void interrupcion2()
{
paso2++;
if (paso2%(70*16/6)==0)
{
tb2 = micros();
if (ta2!=0) {
dT2 = tb2-ta2;
Input2= (60000000UL*400)/(dT2*90);
if (Vl<0) {
Input2= -Input2;
}
}
ta2=tb2;
}
}
Sorry for my english, but i am from Argentina. Thanks for your help.
↧