Ask Your Question
0

Publishing odometry with rosserial [closed]

asked 2019-04-11 10:19:18 -0500

Perrary gravatar image

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.

edit retag flag offensive reopen merge delete

Closed for the following reason too subjective or argumentative by Perrary
close date 2019-09-23 14:45:58.505538

Comments

Hi, did you find a solution for your issue ?

Shanika gravatar imageShanika ( 2019-06-18 07:57:23 -0500 )edit

Hi Shanika. I finally found a solution, after a hard seach in the Intertet.

Perrary gravatar imagePerrary ( 2019-06-27 13:30:25 -0500 )edit

How did you do it ? In my case, it is the RPI that publishes the odom msg by suscribing to my encoders ticks.

Shanika gravatar imageShanika ( 2019-07-01 02:30:25 -0500 )edit

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.

Perrary gravatar imagePerrary ( 2019-07-01 11:55:07 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
1

answered 2019-04-18 14:41:29 -0500

chrisalbertson gravatar image

updated 2019-04-19 15:47:50 -0500

Your code is not going to work.
In the Arduino environment code is only executed if it is inside either the "loop()" or "setup()" functions. Only declarations are allowed outside of those. You have code in global space. for for example "v = mensaje_motor.linear.x;" will never run. It needs to be inside a function that gets called.

Also, I don't see where you are publishing. It needs to be done periodically from the inside of loop()

I am working on this EXACT same problem, even using the same motor shield. Only my version runs on STM32 and not the AVR chip but still uses Arduino IDE.

As of this writing, my code has a few bugs but it is a little farther along than yours I'm now able to control the motor speeds and et odometry data.

I keep my code on GitHub and you can see how I do it. But be warned the GitHub code is a few days older than what I run locally, so some bugs are fixed but not yet committed.

If you want to use my code, go ahead it is GPL'd. I would be happy if you would contribute if you have better ideas https://github.com/chrisalbertson/ROS... You can contact me either here or via GitHub.

My target hardware is two different STM32 boards 1) "Blue Pill" https://wiki.stm32duino.com/index.php... (These sell for ~$2 on eBay) The price unbelievable 2) Nucleo STM32F4xx (where xx is either 01, 11, or 46) (these sell for $13 direct from STM) https://www.st.com/en/evaluation-tool... The "monster motor shield" fits directly in the Nucleo. The Nucleo has a 32-bit CPU with FPU and runs at about 100 MHz and is MUCH faster then the Arduino Mega and only 1/3 the price.

Had you asked in another week I'd have a complete working system to offer. But I've not finished yet. The PID and kinematics need tweeks and it all needs to be documented still.

edit flag offensive delete link more

Question Tools

2 followers

Stats

Asked: 2019-04-11 10:19:18 -0500

Seen: 231 times

Last updated: Apr 19