jjj

#!/usr/bin/env python

import roslib
import rospy
import serial
from math import pi
import threading
import tf

from geometry_msgs.msg import Twist

#joint_states_pub = None
stop_time = 0;
tf_listener = tf.TransformListener()
tf_sender = tf.TransformBroadcaster()

wide = 0.153 + 0.030 #wide of car
step = 2*pi*0.30 / (1000.0/3.0)  # meters in 1 tic

old_tickL = 0
old_tickR = 0

def publish_joint_states(tickL, tickR):
	global old_tickL
	global old_tickL

	odom_frame = '/odom'
	base_frame = '/base_link'

	## get current pos
	trans,rot = tf_listener.lookupTransform(odom_frame, base_frame, rospy.Time(0))

	## calc radius, angle, from angR, angL
	l = (tickL - old_tickL) * step
	r = (tickR - oldtickR) * step
	theta = rot.getAngle()

	if r != l:       # moving along the arc
		if tickL > tickR: # turn to the right
			R = r/alpha
			alpha = (l-r) / wide
		elif tickR > tickL:             # turn to the left
			R = l/alpha
			alpha = (r-l) / wide
		else:
			R = 0
			alpha = 0

		newX = trans[0] + ( (R+wide/2) * (math.sin(theta + alpha) - math.sin(theta)) )
		newY = trans[1] + ( (R+wide/2) * (-math.cos(theta + alpha) + math.cos(theta)) )
		newTheta = theta + alpha



	else:       # facing
		newX = trans[0] + l*math.cos(theta)
		newY = trans[1] + l*math.sin(theta)
		newTheta = 0.0


	## send trans, rot
	tf_sender. sendTransform( (newX, newY, .0), tf.transformations.quaternion_about_axis(newTheta, (0,0,1) ), \
			rospy.Time.now(), base_frame, odom_frame )

	old_tickL = tickL
	old_tickR = tickR



ser = serial.Serial("/dev/ttyACM0",115200)

def read_from_port():
	rospy.loginfo("got from serial: "+ ser.readline() )
	try:
		while True:
			l = ser.readline()
			arr = l.split(",")
			if len(arr) < 4: continue
			publish_joint_states( int(arr[1]), int(arr[3]) )
	except Exception as e:
		rospy.signal_shutdown( str(e) )

def recv_cmd(data):
	if data.linear.x > 0 :
		ser.write("w\n")
	elif data.linear.x < 0:
		ser.write("s\n")
	elif data.angular.z > 0:
		ser.write("a\n")
	elif data.angular.z < 0:
		ser.write("d\n")
	stop_time = rospy.Time.now() + rospy.Duration.from_sec(1)

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

if __name__ == '__main__':

	thread = threading.Thread(target=read_from_port)
	thread.daemon = True
	thread.start()

	try:
		rospy.init_node('arduino_node')

		#joint_states_pub = rospy.Publisher('/joint_states', JointState, queue_size=10)

		rospy.Subscriber("/turtle1/cmd_vel", Twist, recv_cmd)

		rospy.loginfo("Starting arduino node")

		rate = rospy.get_param('~rate', 20)
		r = rospy.Rate(rate)
		while not rospy.is_shutdown():
			if stop_time < rospy.Time.now():
				ser.write("s\n")
				stop_time = rospy.Time.now() + rospy.Duration.from_sec(10)
			r.sleep()

	except rospy.ROSInterruptException as e:
		print e