Publishing odometry with rosserial [closed]
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.
Hi, did you find a solution for your issue ?
Hi Shanika. I finally found a solution, after a hard seach in the Intertet.
How did you do it ? In my case, it is the RPI that publishes the odom msg by suscribing to my encoders ticks.
What do you need exactly? I used the rosserial node in Arduino to send the odometry to the RPI. To measure the speed i counted the ticks of the arduino oscillator between two ticks of the encoders.
Hi @Perrary, I want to obtain the odometry with Arduino Uno too. Could you please share the ros and arduino code with me?