Software implementation

Ros process tree

Code
Piencoder node

#!/usr/bin/env python

import RPi.GPIO as GPIO

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 std_msgs.msg import Char

GPIO.setmode(GPIO.BCM)

renc=2

lenc=3

global sl

global sr

sl='b'

sr='f'

GPIO.setup(lenc, GPIO.IN)

GPIO.setup(renc, GPIO.IN)

global lcnt

lcnt=0

global rcnt

rcnt=0

def odl(lenc):

global lcnt

global sl

if sl=='f':

lcnt=lcnt+1

elif sl=='b' :

lcnt=lcnt-1

def rdl(renc):

global rcnt
 global sr

if sr=='f':

rcnt=rcnt+1

elif sr=='b' :

rcnt=rcnt-1

GPIO.add_event_detect(lenc,GPIO.RISING)

GPIO.add_event_callback(lenc,odl)

GPIO.add_event_detect(renc,GPIO.RISING)

GPIO.add_event_callback(renc,rdl)

def callback(info):

global lcnt

global lcnt

global sl

global sr

#rospy.loginfo(rospy.get_caller_id() + "I heard %s", info)

#rospy.loginfo("leftwheel count%d", lcnt)

#rospy.loginfo( "right wheel count %d",rcnt)

if info.data==0:

sl='f'

sr='f'

elif info.data==1:

sl='b'

sr='b'

elif info.data==2:

sl='b'

sr='f'

elif info.data==3:

sl='f'

sr='b'

#rospy.loginfo( "status %c",sr)

def listener():

global lcnt

global rcnt
 rospy.init_node('Encoder', anonymous=True)

rospy.Subscriber("Direction", Char, callback)

encoder_pub = rospy.Publisher("ticks", Vector3, queue_size=50)

r = rospy.Rate(1.0)

while not rospy.is_shutdown():

# publish the message

encdata=Vector3()

encdata.x=lcnt

encdata.y=rcnt

encoder_pub.publish(encdata)

#rospy.loginfo("leftwheel count%d", lcnt)

#rospy.loginfo( "right wheel count %d",rcnt)

r.sleep()

if __name__ == '__main__':

listener()

Base controller

#!/usr/bin/env python

import rospy

from std_msgs.msg import Char

import RPi.GPIO as GPIO

GPIO.setmode(GPIO.BCM)

M1A=14

M1B=4

M2A=15

M2B=18

GPIO.setup(M1A, GPIO.OUT)

GPIO.setup(M1B, GPIO.OUT)

GPIO.setup(M2A, GPIO.OUT)

GPIO.setup(M2B, GPIO.OUT)

def callback(info):

rospy.loginfo("I heard %s",info.data)

 if info.data==0:

rospy.loginfo("F")

forward()

elif info.data==1:

rospy.loginfo("B")

backward()

elif info.data==2:

rospy.loginfo("L")

turnccw()

elif info.data==3:

rospy.loginfo("R")

turncw()

elif info.data==5:

rospy.loginfo("S")

stop()

else:

rospy.loginfo("S")

stop()

def listener():

# In ROS, nodes are uniquely named. If two nodes witho the same

# node are launched, the previous one is kicked off. The

# anonymous=True flag means that rospy will choose a unique

# name for our 'listener' node so that multiple listeners can

# run simultaneously.

rospy.init_node('basecontroller', anonymous=True)

rospy.Subscriber("Direction", Char, callback)

# spin() simply keeps python from exiting until this node is stopped

rospy.spin()

def stop():
 GPIO.output(M1A, GPIO.HIGH)

GPIO.output(M1B, GPIO.HIGH)

GPIO.output(M2A, GPIO.HIGH)

GPIO.output(M2B, GPIO.HIGH)

def forward():

GPIO.output(M1A, GPIO.HIGH)

GPIO.output(M1B, GPIO.LOW)

GPIO.output(M2A, GPIO.HIGH)

GPIO.output(M2B, GPIO.LOW)

def backward():

GPIO.output(M1A, GPIO.LOW)

GPIO.output(M1B, GPIO.HIGH)

GPIO.output(M2A, GPIO.LOW)

GPIO.output(M2B, GPIO.HIGH)

def turncw():

GPIO.output(M1A, GPIO.LOW)

GPIO.output(M1B, GPIO.HIGH)

GPIO.output(M2A, GPIO.HIGH)

GPIO.output(M2B, GPIO.LOW)

def turnccw():

GPIO.output(M1A, GPIO.HIGH)

GPIO.output(M1B, GPIO.LOW)

GPIO.output(M2A, GPIO.LOW)

GPIO.output(M2B, GPIO.HIGH)

if __name__ == '__main__':

listener()

Odometry

#!/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 std_msgs.msg import Char

global s

global sr

global sl

global th

global th0

global x

global y

global x0

global yo

global pl,pr

pl=0

pr=0

def callback(info):

global s

global sr

global sl

global th

global th0

global x

global y

global x0

global yo

global pl,pr

#rospy.loginfo(rospy.get_caller_id() + "I heard %d", info.x)

#rospy.loginfo(rospy.get_caller_id() + "I heard %d", info.y)

#rospy.loginfo(rospy.get_caller_id() + "oldx %d", pl)

#rospy.loginfo(rospy.get_caller_id() + "oldy %d", pr)

#wt=info.x-pl
 #print wt

#print ((info.x-pl)*(3.14159265*0.051)/60)

#rospy.loginfo("wtf %d"), wt)

sl=((info.x-pl)*(3.14159265*0.051)/60)

sr=((info.y-pr)*(3.14159265*0.051)/60)

pl=info.x

pr=info.y

def listener():

global sr

sr=0

global sl

sl=0

global s

s=(sl+sr)/2.0

global th

th=(sr-sl)/15.0

global th0

th0=0

global x

x=0.0

global y

y=0.0

global x0

x0=0.0

global yo

y0=0.0

rospy.init_node('odompublisher', anonymous=True)

rospy.Subscriber("ticks",Vector3, callback)

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

odom_broadcaster = tf.TransformBroadcaster()

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()

s=(sr+sl)/2.0

print s

th=(sr-sl)/0.15+th0

x=s*cos(th)+x0

y=s*sin(th)+y0

x0=x

y0=y

th0=th

rospy.loginfo("X coordinate%f",x)

rospy.loginfo("Y coordinate%f",y)

rospy.loginfo("Theta coordinate%f",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

#rospy.spin()

r.sleep()

if __name__ == '__main__':

listener()

piremote
#!/usr/bin/env python

# license removed for brevity

import rospy

from std_msgs.msg import Char

from getch import getch

def talker():

pub = rospy.Publisher('Direction',Char, queue_size=10)

rospy.init_node('piremote', anonymous=True)

rate = rospy.Rate(10) # 10hz

while not rospy.is_shutdown():

#hello_str = "hello world %s" % rospy.get_time()

 print "Input command"

msg=getch()

print msg

if msg=='w':

msg=0

elif msg=='q':

break

elif msg=='s':

msg=1

elif msg=='a':

msg=2

elif msg=='d':

msg=3

else:

msg=5

rospy.loginfo(msg)

pub.publish(msg)

rate.sleep()

if __name__ == '__main__':

try:

talker()

except rospy.ROSInterruptException:

pass

Tf broadcast

#include <ros/ros.h>

#include <tf/transform_broadcaster.h>

int main(int argc, char** argv){

ros::init(argc, argv, "robot_tf_publisher");

ros::NodeHandle n;
 ros::Rate r(10);

tf::TransformBroadcaster broadcaster;

while(n.ok()){

broadcaster.sendTransform(

tf::StampedTransform(

tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.1, 0.0, 0.2)),

ros::Time::now(),"base_link", "laser"));

r.sleep();