Publishing odometry with rosserial [closed]

edit

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 and link text 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. Is my code correct for doing this? (only see the publishing parts, the others work well)

Here is the full code

#include <ros.h>                                                                                                                                                             #include <geometry_msgs/Twist.h>                                                                                                                     #include <geometry_msgs/Twist.h>                                                                                                                                                      #include "DualVNH5019MotorShield.h"                                                                                                                                                                                                                      #include <PID_v1.h>                                                                                                                                                                       #include <nav_msgs/Odometry.h>       
                                                                                                                                                                                                          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<geometry_msgs::Twist> 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.