Ros Tutorials: Communications Node V1

Let's keep on moving...

Here is the next installment in the ROS Tutorials series. Today, we are going to be writing a ROS node to control the Mega from Twist messages. Last time we defined a serial protocol for communicating motor data. First, the computer sends 0x41 to start a new message. Then the computer sends two bytes, one for the linear axis, and one for the angular. This then gets translated to servo movements that are then sent to the Sabertooth.
So here is the code:

	#!/usr/bin/env python
	'''
	*****************************************
	pythonToArduino.ino
	Created October 1, 2016
	Adapted from the Simple Subscriber example from the ROS wiki
	which is licensed under the Createive Commons Attribution 3.0 License.
	A copy of the license can be found here: https://creativecommons.org/licenses/by/3.0/legalcode
	Modified By: thecentralthinkingunit.blogspot.com
	Released into the public domain.
	Purpose:
	This node communicates between ROS and the Arduino Mega onboard the robot.
	******************************************
	'''
	import rospy
	from geometry_msgs.msg import Twist
	import serial
	def clamp(n, minn, maxn):
	if n < minn:
	return minn
	elif n > maxn:
	return maxn
	else:
	return n
	def callback(data):
	global ser
	rospy.loginfo(rospy.get_caller_id() + "I heard %s", data)
	ser.write(chr(0x41))
	ser.write(chr(int(clamp(data.linear.x*30+90, 0, 180))))
	ser.write(chr(int(clamp(data.angular.z*30+90, 0, 180))))
	#print "L" + str(clamp(data.linear.x*30+90, 0, 180)) + "A" + str(clamp(data.angular.x*30+90, 0, 180)) # Uncomment this line for debug purposes
	def listener():
	rospy.init_node('python_to_arduino', anonymous=True)
	rospy.Subscriber("cmd_vel", Twist, callback)
	global ser
	ser = serial.Serial("/dev/ttyACM0", 9600)
	# spin() simply keeps python from exiting until this node is stopped
	rospy.spin()
	if __name__ == '__main__':
	listener()

view raw pythonToArduino.py hosted with ❤ by GitHub

The next step is to calibrate it and make sure that everything works.
That's all for now!