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

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