How to publish odom on Arduino

asked 2019-06-19 09:36:20 -0500

Shanika gravatar image

updated 2019-06-19 10:33:11 -0500

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

edit retag flag offensive close merge delete

Comments

The nav_msgs/Odometry message type is about 1000 bytes, which is too big to easily fit in RAM in most Arduinos. I recommend keep your current approach.

ahendrix gravatar imageahendrix ( 2019-06-19 11:02:13 -0500 )edit

Thank you ahendrix, I will try to publish odometry information on my Raspberry Pi. Just to confirm, how do I compute odometry from encoder ticks ? Some people use ICC and others don't use it.

Shanika gravatar imageShanika ( 2019-06-20 09:46:26 -0500 )edit