How to publish odom (nav_msgs/Odometry) from MD49 encoders outputs?

Question

I am using MD49 Motor Drive with its motors

https://www.robot-electronics.co.uk/htm/md49tech.htm

http://wiki.ros.org/md49_base_controller

How to subscribe (encoder_l and encoder_r) from md49_base_controller package and publish (vx , vy ,and vth ) as a form odom (nav_msgs/Odometry) ?

There are two problem:

1-The first is that the encoders outputs are not correct "the package needs to be modified.

2-The second is the I want to create a package that subscribe the right and left wheels encoder counts (encoder_l and encoder_r) and publish (vx , vy ,and vth) as a form odom (nav_msgs/Odometry) to be compatable wth imu MPU9250

http://wiki.ros.org/robot_pose_ekf

The proposed package is:

1- We have to convert (encoder_l and encoder_r) to (RPM_l and RPM_r) as follow:

if (speed_l>128){newposition1 = encoder_l;}
else if  (speed_l<128){ newposition1 = 0xFFFFFFFF-encoder_l;}
else if  (speed_l==128) {newposition1=0;}

newtime1 = millis();
RPM_l = ((newposition1-oldposition1)*1000*60)/((newtime1-oldtime1)*980);
oldposition1 = newposition1;
oldtime1 = newtime1;
delay(250);

if (speed_r>128){ newposition2 = encoder_r;}
else if  (speed_r<128){ newposition2 = 0xFFFFFFFF-encoder_r;}
else if   (speed_r==128) { newposition2=0;}
newtime2 = millis();
RPM_r = ((newposition2-oldposition2)*1000*60)/((newtime2-oldtime2)*980);
oldposition2 = newposition2;
oldtime2= newtime2;
delay(250);

2- We have to convert (RPM_l and RPM_r) to (vx, vy, and vth) as follow:

vx=(r/2)*RPM_l*math.cos(th)+(r/2)*RPM_r*math.cos(th);
vx=(r/2)*RPM_l*math.sin(th)+(r/2)*RPM_r*math.sin(th);
vth=(r/B)*omega_l-(r/B)*omega_r;

Hint: r and B are wheel radius and vehicle width respectively.

3- The odom (nav_msgs/Odometry) package is:

#!/usr/bin/env python

import math
from math import sin, cos, pi

import rospy
import tf
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Point, Pose, Quaternion, Twist, Vector3
from md49_messages.msg import md49_encoders

rospy.init_node('odometry_publisher')

odom_pub = rospy.Publisher("odom", Odometry, queue_size=50)
odom_broadcaster = tf.TransformBroadcaster()


x = 0.0
y = 0.0
th = 0.0

vx =0.1
vy = -0.1
vth = 0.1

current_time = rospy.Time.now()
last_time = rospy.Time.now()

r = rospy.Rate(1.0)
while not rospy.is_shutdown():
    current_time = rospy.Time.now()


    # compute odometry in a typical way given the velocities of the robot
    dt = (current_time - last_time).to_sec()
    delta_x = (vx * cos(th) - vy * sin(th)) * dt
    delta_y = (vx * sin(th) + vy * cos(th)) * dt
    delta_th = vth * dt

    x += delta_x
    y += delta_y
    th += delta_th

    # since all odometry is 6DOF we'll need a quaternion created from yaw
    odom_quat = tf.transformations.quaternion_from_euler(0, 0, th)

    # first, we'll publish the transform over tf
    odom_broadcaster.sendTransform(
        (x, y, 0.),
        odom_quat,
        current_time,
        "base_link",
        "odom"
    )

    # next, we'll publish the odometry message over ROS
    odom = Odometry()
    odom.header.stamp = current_time
    odom.header.frame_id = "odom"

    # set the position
    odom.pose.pose = Pose(Point(x, y, 0.), Quaternion(*odom_quat))

    # set the velocity
    odom.child_frame_id = "base_link"
    odom.twist.twist = Twist(Vector3(vx, vy, 0), Vector3(0, 0, vth))

    # publish the message
    odom_pub.publish(odom)

    last_time = current_time
    r.sleep()

Answer 1

The problem was in serial comunication setup not for the code.

Setup UART on the raspi 3 GPIO

For some strange reason the default for Pi3 using the latest 4.4.9 kernel is to DISABLE UART. To enable it you need to change enable_uart=1 in /boot/config.txt. (There is no longer necessary to add core_freq=250 to fix the core frequency to get stable baudrate.) If you don’t use Bluetooth (or have undemanding uses) it is possible to swap the ports back in the Device Tree. There is a pi3-miniuart-bt and pi3-disable-bt module which are described in /boot/overlays/README here.

As mentioned, by default the new GPIO UART is not enabled so the first thing to do is to edit the config.txt file to enable the serial UART:

sudo nano /boot/config.txt

and add the line (at the bottom):

enable_uart=1

You have to reboot for the changes to take effect.

To check where your serial ports are pointing you can use the list command with the long “-l” listing format: ls -l /dev

In the long listing you will find: serial0 -> ttyS0 serial1 -> ttyAMA0

Thus on a Raspberry Pi 3 and Raspberry Pi Zero W, serial0 will point to GPIO J8 pins 8 and 10 and use the /dev/ttyS0. On other Raspberry Pi’s it will point to /dev/ttyAMA0.

So where possible refer to the serial port via it’s alias of “serial0” and your code should work on both Raspberry Pi 3 and other Raspberry Pi’s.

You also need to remove the console from the cmdline.txt. If you edit this with:

sudo nano /boot/cmdline.txt

you will see something like this:

dwc_otg.lpm_enable=0 console=serial0,115200 console=tty1 root=/dev/mmcblk0p2 rootfstype=ext4 elevator=deadline fsck.repair=yes root wait

remove the line: console=serial0,115200 and save and reboot for changes to take effect:

reboot

Answer 2

First off, you need to import nav_msgs/Odometry as the following:

from nav_msgs.msg import Odometry

You must have a function that performs those conversions and then in rospy.Subscriber import those variables, like this:

def example(data):
    data.vx=<conversion>
    data.vth=<conversion>

def listener():
    rospy.Subscriber('*topic*', Odometry, example)
    rospy.spin()

if __name__ == 'main':
    listener()

I think this would work