How to publish odom on Arduino

edit

Hello,

I have a robot which uses a Me Auriga, equivalent to an Arduino Mega2650 board, and 180 optical encoder motors. On the Me Auriga board, I have uploaded the following Arduino firmware CreativeBot_type_A_firmware . I am using rosserial_python on my Raspberry Pi 3+

RPi Terminal

 rosrun rosserial_python serial_node.py 

I am trying to publish odometry information using encoder ticks on Arduino. If I'm right, We can do this by two ways :

1. Publishing left and right encoder ticks from Arduino so that the Rpi subscribe and use them to publish odom message on RPi (Python)
2. Publishing odom message from Arduino using the encoder ticks (I would like to do this). Could you please help me?

1. Arduino A few lines of the robot's firmware :

#include <ros.h>
#include <ros time.h="">
#include <std_msgs float32.h="">
#include <std_msgs int16.h="">
#include "MeAuriga.h"

MeOnBoardTemp OnBoardTemp(PORT_13);
MeEncoderOnBoard Encoder_1(SLOT1);
MeEncoderOnBoard Encoder_2(SLOT2);

ros::NodeHandle  nh;

std_msgs::Int16 left_ticks_msg;
std_msgs::Int16 right_ticks_msg;

ros::Publisher pub_left_ticks("/left_ticks",&left_ticks_msg);
ros::Publisher pub_right_ticks("/right_ticks",&right_ticks_msg);

long ticks_time;

void lwheel_vtarget(const std_msgs::Float32& vel){
  if(motorOn){
    Encoder_2.runSpeed(vel.data);
  }
  else {
    Encoder_2.runSpeed(0);
  }
}

void rwheel_vtarget(const std_msgs::Float32& vel){
  if(motorOn){
    Encoder_1.runSpeed(-vel.data);
  }
  else {
    Encoder_1.runSpeed(0);
  }
}

void isr_process_encoder1(void){
  if(digitalRead(Encoder_1.getPortB()) == 0){
    Encoder_1.pulsePosMinus();
  }
  else {
    Encoder_1.pulsePosPlus();;
  }
}

void isr_process_encoder2(void)
{
  if(digitalRead(Encoder_2.getPortB()) == 0)
  {
    Encoder_2.pulsePosMinus();
  }
  else
  {
    Encoder_2.pulsePosPlus();
  }
}
ros::Subscriber<std_msgs::float32> sub_left("lwheel_vtarget", &lwheel_vtarget);
ros::Subscriber<std_msgs::float32> sub_right("rwheel_vtarget", &rwheel_vtarget);

void setup()
{
  /* Encoder motor drives */
  attachInterrupt(Encoder_1.getIntNum(), isr_process_encoder1, RISING);
  attachInterrupt(Encoder_2.getIntNum(), isr_process_encoder2, RISING);

  //Set PWM 8KHz
  TCCR1A = _BV(WGM10);
  TCCR1B = _BV(CS11) | _BV(WGM12);

  TCCR2A = _BV(WGM21) | _BV(WGM20);
  TCCR2B = _BV(CS21);

  Encoder_1.setPulse(9);
  Encoder_2.setPulse(9);
  Encoder_1.setRatio(39.267);
  Encoder_2.setRatio(39.267);
  Encoder_1.setPosPid(0.18,0,0);
  Encoder_2.setPosPid(0.18,0,0);
  Encoder_1.setSpeedPid(0.18,0,0);
  Encoder_2.setSpeedPid(0.18,0,0);
  Encoder_1.runSpeed(0);
  Encoder_2.runSpeed(0); 
  Encoder_1.setMotionMode(PID_MODE);
  Encoder_2.setMotionMode(PID_MODE);

  nh.getHardware()->setBaud(115200);
  nh.initNode();
  nh.advertise(pub_left_ticks);
  nh.advertise(pub_right_ticks);
  nh.subscribe(sub_left);
  nh.subscribe(sub_right);
}

void loop()
{   
    // publish wheel pos in ticks for each wheel
  if(millis() >= ticks_time){
    left_ticks_msg.data = Encoder_2.getPulsePos();
    pub_left_ticks.publish(&left_ticks_msg);
    right_ticks_msg.data = Encoder_1.getPulsePos();
    pub_right_ticks.publish(&right_ticks_msg);
    ticks_time = millis()+10;
  }
  if(!motorOn){
    Encoder_1.runSpeed(0);
    Encoder_2.runSpeed(0);
  }
  Encoder_1.loop();
  Encoder_2.loop();
  nh.spinOnce();
  delay(100);
}
}

Thank you, :)

Shanika