class ZumyROS:
def __init__(self):
self.zumy = Zumy()
rospy.init_node("zumy_ros")
self.cmd = (0, 0)
rospy.Subscriber("cmd_vel", Twist, self.cmd_callback, queue_size=1)
self.lock = Condition()
self.rate = rospy.Rate(30.0)
self.name = socket.gethostname()
self.heartBeat = rospy.Publisher("/" + self.name + "/heartBeat", String, queue_size=5)
self.imu_pub = rospy.Publisher("/" + self.name + "/imu", Imu, queue_size=1)
self.imu_count = 0
self.timestamp = time.time()
self.publisher = rospy.Publisher("from_zumy", Float64, queue_size=1)
self.msg = None
def cmd_callback(self, msg):
self.msg = msg
self.timestamp = time.time()
lv = 0.6
la = 0.4
v = msg.linear.x
print v
a = msg.angular.z
print a
r = lv * v + la * a
l = lv * v - la * a
self.lock.acquire()
self.cmd = (l, r)
self.lock.release()
def run(self):
while not rospy.is_shutdown():
time_now = time.time()
if time_now - self.timestamp > 0.5:
self.cmd = (0, 0)
self.lock.acquire()
self.zumy.cmd(*self.cmd)
imu_data = self.zumy.read_imu()
self.lock.release()
imu_msg = Imu()
imu_msg.header = Header(self.imu_count, rospy.Time.now(), self.name)
imu_msg.linear_acceleration.x = 9.81 * imu_data[0]
imu_msg.linear_acceleration.y = 9.81 * imu_data[1]
imu_msg.linear_acceleration.z = 9.81 * imu_data[2]
imu_msg.angular_velocity.x = 3.14 / 180.0 * imu_data[3]
imu_msg.angular_velocity.y = 3.14 / 180.0 * imu_data[4]
imu_msg.angular_velocity.z = 3.14 / 180.0 * imu_data[5]
self.imu_pub.publish(imu_msg)
self.heartBeat.publish("I am alive from Glew")
if self.msg != None:
self.publisher.publish(self.msg.linear.y)
self.rate.sleep()
# If shutdown, turn off motors
self.zumy.cmd(0, 0)
class ZumyROS:
def __init__(self):
self.zumy = Zumy()
rospy.init_node('zumy_ros')
self.cmd = (0,0)
rospy.Subscriber('cmd_vel', Twist, self.cmd_callback)
self.lock = Condition()
self.rate = rospy.Rate(30.0)
self.name = socket.gethostname()
self.heartBeat = rospy.Publisher('/' + self.name + '/heartBeat', String, queue_size=5)
self.imu_pub = rospy.Publisher('/' + self.name + '/imu', Imu, queue_size = 1)
self.imu_count = 0
# - - - - - - - - - - - - - - ELECTROMAGNET - - - - - - - - - - - - - - - #
rospy.Subscriber('/' + self.name + '/digital_out', Int32, self.do_callback)
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - #
# - - - - - EM CALLBACK - - - - #
def do_callback(self,msg):
self.lock.acquire()
self.zumy.write_do(msg.data)
self.lock.release()
# - - - - - - - - - - - - - - - #
def cmd_callback(self, msg):
lv = 0.6
la = 0.4
v = msg.linear.x
a = msg.angular.z
r = lv*v + la*a
l = lv*v - la*a
self.lock.acquire()
self.cmd = (l,r)
self.lock.release()
def run(self):
while not rospy.is_shutdown():
self.lock.acquire()
self.zumy.cmd(*self.cmd)
imu_data = self.zumy.read_imu()
self.lock.release()
imu_msg = Imu()
imu_msg.header = Header(self.imu_count,rospy.Time.now(),self.name)
imu_msg.linear_acceleration.x = 9.81 * imu_data[0]
imu_msg.linear_acceleration.y = 9.81 * imu_data[1]
imu_msg.linear_acceleration.z = 9.81 * imu_data[2]
imu_msg.angular_velocity.x = 3.14 / 180.0 * imu_data[3]
imu_msg.angular_velocity.y = 3.14 / 180.0 * imu_data[4]
imu_msg.angular_velocity.z = 3.14 / 180.0 * imu_data[5]
self.imu_pub.publish(imu_msg)
self.heartBeat.publish("I am alive")
self.rate.sleep()
# If shutdown, turn off motors
self.zumy.cmd(0,0)
class ZumyROS:
def __init__(self):
self.zumy = Zumy()
rospy.init_node('zumy_ros')
self.cmd = (0, 0)
rospy.Subscriber('cmd_vel', Twist, self.cmd_callback, queue_size=1)
rospy.Subscriber('enable', Bool, self.enable_callback, queue_size=1)
rospy.Subscriber(
'/base_computer', String, self.watchdog_callback, queue_size=1
) #/base_computer topic, the global watchdog. May want to investigate what happens when there moer than one computer and more than one zumy
self.lock = Condition()
self.rate = rospy.Rate(30.0)
self.name = socket.gethostname()
if rospy.has_param("~timeout"):
self.timeout = rospy.get_param('~timeout') #private value
else:
self.timeout = 2 #Length of watchdog timer in seconds, defaults to 2 sec.
#publishers
self.status_pub = rospy.Publisher("Status", ZumyStatus, queue_size=5)
self.imu_pub = rospy.Publisher('imu', Imu, queue_size=1)
self.r_enc_pub = rospy.Publisher('r_enc', Float32, queue_size=5)
self.l_enc_pub = rospy.Publisher('l_enc', Float32, queue_size=5)
self.r_vel_pub = rospy.Publisher('r_vel', Float32, queue_size=5)
self.l_vel_pub = rospy.Publisher('l_vel', Float32, queue_size=5)
self.imu_count = 0
self.batt_pub = rospy.Publisher('Batt', Float32, queue_size=5)
self.last_message_at = time.time()
self.watchdog = True
#an array of times, which will be updated, and used to figure out how fast the while loop is going.
self.laptimes = np.ones(6) * time.time()
# NEW!
self.IR_ai_side_pub = rospy.Publisher('/' + self.name + '/IR_ai_side',
Float32,
queue_size=1)
self.IR_ai_front_pub = rospy.Publisher("/" + self.name +
"/IR_ai_front",
Float32,
queue_size=1)
self.IR_ai_top_pub = rospy.Publisher('/' + self.name + '/IR_ai_top',
Float32,
queue_size=1)
self.IR_ai_bottom_pub = rospy.Publisher("/" + self.name +
"/IR_ai_bottom",
Float32,
queue_size=1)
# Publish twists to /cmd_vel in order to drive the Zumy.
def cmd_callback(self, msg):
self.lock.acquire()
self.cmd = twist_to_speeds(
msg
) #update the commanded speed, the next time the main loop in run comes through, it'll be update on the zumy.
self.lock.release()
def enable_callback(self, msg):
#enable or disable myself based on the content of the message.
if msg.data and self.watchdog:
self.zumy.enable()
else:
self.zumy.disable()
self.cmd = (0, 0) #turn the motors off
#do NOT update the watchdog, since the watchdog should do it itself.
#If i'm told to go but the host's watchdog is down, something's very wrong, and i won't be doing much
def watchdog_callback(self, msg):
#update the last time i got a messag!
self.last_message_at = time.time()
self.watchdog = True
def run(self):
while not rospy.is_shutdown():
self.lock.acquire()
self.zumy.cmd(*self.cmd)
try:
imu_data = self.zumy.read_imu()
imu_msg = Imu()
imu_msg.header = Header(self.imu_count, rospy.Time.now(),
self.name)
imu_msg.linear_acceleration.x = 9.81 * imu_data[0]
imu_msg.linear_acceleration.y = 9.81 * imu_data[1]
imu_msg.linear_acceleration.z = 9.81 * imu_data[2]
imu_msg.angular_velocity.x = 3.14 / 180.0 * imu_data[3]
imu_msg.angular_velocity.y = 3.14 / 180.0 * imu_data[4]
imu_msg.angular_velocity.z = 3.14 / 180.0 * imu_data[5]
self.imu_pub.publish(imu_msg)
except ValueError:
pass
try:
enc_data = self.zumy.enc_pos()
enc_msg = Float32()
enc_msg.data = enc_data[0]
self.r_enc_pub.publish(enc_msg)
enc_msg.data = enc_data[1]
self.l_enc_pub.publish(enc_msg)
except ValueError:
pass
try:
vel_data = self.zumy.enc_vel()
vel_msg = Float32()
vel_msg.data = vel_data[0]
self.l_vel_pub.publish(vel_msg)
vel_msg.data = vel_data[1]
self.r_vel_pub.publish(vel_msg)
except ValueError:
pass
try:
v_bat = self.zumy.read_voltage()
self.batt_pub.publish(v_bat)
except ValueError:
pass
try:
IR_ai_front_data = self.zumy.read_IR_ai_front()
scaled_front_data = IR_ai_front_data * 3.3
self.IR_ai_front_pub.publish(scaled_front_data)
except ValueError:
self.IR_ai_front_pub.publish(19)
pass
try:
IR_ai_side_data = self.zumy.read_IR_ai_side()
scaled_side_data = IR_ai_side_data * 3.3
self.IR_ai_side_pub.publish(scaled_side_data)
except ValueError:
self.IR_ai_side_pub.publish(17)
pass
try:
IR_ai_top_data = self.zumy.read_IR_ai_top()
scaled_top_data = IR_ai_top_data * 3.3
self.IR_ai_top_pub.publish(scaled_top_data)
except ValueError:
self.IR_ai_top_pub.publish(16)
pass
try:
IR_ai_bottom_data = self.zumy.read_IR_ai_bottom()
scaled_bottom_data = IR_ai_bottom_data * 3.3
self.IR_ai_bottom_pub.publish(scaled_bottom_data)
except ValueError:
self.IR_ai_bottom_pub.publish(18)
pass
#END NEW
if time.time() > (
self.last_message_at + self.timeout
): #i've gone too long without seeing the watchdog.
self.watchdog = False
self.zumy.disable()
self.zumy.battery_protection(
) # a function that needs to be called with some regularity.
#handle the robot status message
status_msg = ZumyStatus()
status_msg.enabled_status = self.zumy.enabled
status_msg.battery_unsafe = self.zumy.battery_unsafe()
#update the looptimes, which will get published in status_msg
self.laptimes = np.delete(self.laptimes, 0)
self.laptimes = np.append(self.laptimes, time.time())
#take average over the difference of the times... and then invert. That will give you average freq.
status_msg.loop_freq = 1 / float(np.mean(np.diff(self.laptimes)))
self.status_pub.publish(status_msg)
self.lock.release() #must be last command involving the zumy.
self.rate.sleep()
self.first_move = False
# If shutdown, turn off motors & disable anything else.
self.zumy.disable()
class ZumyROS:
def __init__(self):
self.zumy = Zumy()
rospy.init_node('zumy_ros')
self.cmd = (0,0)
rospy.Subscriber('cmd_vel', Twist, self.cmd_callback,queue_size=1)
rospy.Subscriber('cmd_left', Float32, self.cmd_callback_left, queue_size=1)
rospy.Subscriber('cmd_right', Float32, self.cmd_callback_right, queue_size=1)
self.lock = Condition()
self.rate = rospy.Rate(30.0)
self.name = socket.gethostname()
#publishers
self.heartBeat = rospy.Publisher('heartBeat', String, queue_size=5)
self.imu_pub = rospy.Publisher('imu', Imu, queue_size = 1)
self.r_enc_pub = rospy.Publisher('r_enc', Int32, queue_size = 5)
self.l_enc_pub = rospy.Publisher('l_enc', Int32, queue_size = 5)
self.imu_count = 0
self.batt_pub = rospy.Publisher('Batt',Float32,queue_size = 5)
def cmd_callback(self, msg):
lv = 0.6
la = 0.4
v = msg.linear.x
a = msg.angular.z
r = lv*v + la*a
l = lv*v - la*a
self.lock.acquire()
self.cmd = (l,r)
self.lock.release()
def cmd_callback_left(self, msg):
self.lock.acquire()
self.cmd = (0.6*msg.data, self.cmd[1])
self.lock.release()
def cmd_callback_right(self, msg):
self.lock.acquire()
self.cmd = (self.cmd[0], 0.6*msg.data)
self.lock.release()
def run(self):
while not rospy.is_shutdown():
self.lock.acquire()
self.zumy.cmd(*self.cmd)
imu_data = self.zumy.read_imu()
enc_data = self.zumy.read_enc()
self.lock.release()
imu_msg = Imu()
imu_msg.header = Header(self.imu_count,rospy.Time.now(),self.name)
imu_msg.linear_acceleration.x = 9.81 * imu_data[0]
imu_msg.linear_acceleration.y = 9.81 * imu_data[1]
imu_msg.linear_acceleration.z = 9.81 * imu_data[2]
imu_msg.angular_velocity.x = 3.14 / 180.0 * imu_data[3]
imu_msg.angular_velocity.y = 3.14 / 180.0 * imu_data[4]
imu_msg.angular_velocity.z = 3.14 / 180.0 * imu_data[5]
self.imu_pub.publish(imu_msg)
enc_msg = Int32()
enc_msg.data = enc_data[0]
self.r_enc_pub.publish(enc_msg)
enc_msg.data = enc_data[1]
self.l_enc_pub.publish(enc_msg)
v_bat = self.zumy.read_voltage()
self.batt_pub.publish(v_bat)
self.heartBeat.publish("I am alive")
self.rate.sleep()
# If shutdown, turn off motors
self.zumy.cmd(0,0)
class ZumyROS:
def __init__(self):
self.zumy = Zumy()
rospy.init_node('zumy_ros')
self.cmd = (0, 0)
rospy.Subscriber('cmd_vel', Twist, self.cmd_callback, queue_size=1)
self.lock = Condition()
self.rate = rospy.Rate(30.0)
self.name = socket.gethostname()
#publishers
self.heartBeat = rospy.Publisher('heartBeat', String, queue_size=5)
self.imu_pub = rospy.Publisher('imu', Imu, queue_size=1)
self.r_enc_pub = rospy.Publisher('r_enc', Int32, queue_size=5)
self.l_enc_pub = rospy.Publisher('l_enc', Int32, queue_size=5)
self.imu_count = 0
self.batt_pub = rospy.Publisher('Batt', Float32, queue_size=5)
def cmd_callback(self, msg):
lv = 0.6
la = 0.4
v = msg.linear.x
a = msg.angular.z
r = lv * v + la * a
l = lv * v - la * a
self.lock.acquire()
self.cmd = (l, r)
self.lock.release()
def run(self):
while not rospy.is_shutdown():
self.lock.acquire()
self.zumy.cmd(*self.cmd)
imu_data = self.zumy.read_imu()
enc_data = self.zumy.read_enc()
self.lock.release()
imu_msg = Imu()
imu_msg.header = Header(self.imu_count, rospy.Time.now(),
self.name)
imu_msg.linear_acceleration.x = 9.81 * imu_data[0]
imu_msg.linear_acceleration.y = 9.81 * imu_data[1]
imu_msg.linear_acceleration.z = 9.81 * imu_data[2]
imu_msg.angular_velocity.x = 3.14 / 180.0 * imu_data[3]
imu_msg.angular_velocity.y = 3.14 / 180.0 * imu_data[4]
imu_msg.angular_velocity.z = 3.14 / 180.0 * imu_data[5]
self.imu_pub.publish(imu_msg)
enc_msg = Int32()
enc_msg.data = enc_data[0]
self.r_enc_pub.publish(enc_msg)
enc_msg.data = enc_data[1]
self.l_enc_pub.publish(enc_msg)
v_bat = self.zumy.read_voltage()
self.batt_pub.publish(v_bat)
self.heartBeat.publish("I am alive")
self.rate.sleep()
# If shutdown, turn off motors
self.zumy.cmd(0, 0)
class ZumyROS:
def __init__(self):
self.zumy = Zumy()
rospy.init_node('zumy_ros')
self.cmd = (0,0)
rospy.Subscriber('keyboard', Twist, self.keyboard_callback, queue_size=1)
rospy.Subscriber('cmd_pwm', Vector3, self.pwm_callback, queue_size=1)
self.lock = Condition()
self.rate = rospy.Rate(300.0)
self.name = socket.gethostname()
self.imu_pub = rospy.Publisher('imu', Imu, queue_size = 5)
self.r_enc_pub = rospy.Publisher('r_enc', Int32, queue_size = 1)
self.l_enc_pub = rospy.Publisher('l_enc', Int32, queue_size = 1)
self.camera_pub = rospy.Publisher('camera', Image, queue_size = 5)
self.imu_count = 0
self.l_pwm, self.r_pwm = 0, 0
def pwm_callback(self, msg):
self.l_pwm, self.r_pwm = msg.x, msg.y
self.l_pwm = max(self.l_pwm, -1)
self.l_pwm = min(self.l_pwm, 1)
self.r_pwm = max(self.r_pwm, -1)
self.r_pwm = min(self.r_pwm, 1)
self.lock.acquire()
self.cmd = (self.l_pwm,self.r_pwm)
self.lock.release()
def keyboard_callback(self, msg):
v = msg.linear.x
a = msg.angular.z
if (a == 0):
if (v > 0):
self.l_pwm += 0.1
self.r_pwm += 0.1
else:
self.l_pwm -= 0.1
self.r_pwm -= 0.1
elif (a > 0):
self.l_pwm -= 0.1
self.r_pwm += 0.1
else:
self.l_pwm += 0.1
self.r_pwm -= 0.1
self.l_pwm = max(self.l_pwm, 0)
self.l_pwm = min(self.l_pwm, 1)
self.r_pwm = max(self.r_pwm, 0)
self.r_pwm = min(self.r_pwm, 1)
print((self.l_pwm, self.r_pwm))
self.lock.acquire()
self.cmd = (self.l_pwm,self.r_pwm)
self.lock.release()
def run(self):
while not rospy.is_shutdown():
self.lock.acquire()
self.zumy.cmd(*self.cmd)
imu_data = self.zumy.read_imu()
enc_data = self.zumy.read_enc()
self.lock.release()
imu_msg = Imu()
imu_msg.header = Header(self.imu_count,rospy.Time.now(),self.name)
imu_msg.linear_acceleration.x = 9.81 * imu_data[0]
imu_msg.linear_acceleration.y = 9.81 * imu_data[1]
imu_msg.linear_acceleration.z = 9.81 * imu_data[2]
imu_msg.angular_velocity.x = 3.14 / 180.0 * imu_data[3]
imu_msg.angular_velocity.y = 3.14 / 180.0 * imu_data[4]
imu_msg.angular_velocity.z = 3.14 / 180.0 * imu_data[5]
self.imu_pub.publish(imu_msg)
enc_msg = Int32()
enc_msg.data = enc_data[0]
self.r_enc_pub.publish(enc_msg)
enc_msg = Int32()
enc_msg.data = enc_data[1]
self.l_enc_pub.publish(enc_msg)
'''image = Image()
image.header.stamp = rospy.Time.now()
image.height = 1
image.width = 128
image.step = 128
image.encoding = "mono8"
raw_data = self.zumy.read_camera()
min_raw, max_raw = np.min(raw_data), np.max(raw_data)
image.data = [x // 256 for x in raw_data]
self.camera_pub.publish(image)'''
self.rate.sleep()
# If shutdown, turn off motors
self.zumy.cmd(0,0)
class ZumyROS:
def __init__(self):
self.zumy = Zumy()
# self.PID = PID()
# self.PID_another = PID_another()
# self.l_enc_v = l_encTospeed()
rospy.init_node('zumy_ros')
#self.cmd = (0,0)
#self.rate = rospy.Rate(50.0)
rospy.Subscriber('/cmd_vel1', Twist, self.cmd_callback,queue_size=1)
# rospy.Subscriber('/vel',Float64,self.updata_vel,queue_size = 1)
# rospy.Subscriber('/l_speed', Float64, self.l_enc_to_speed, queue_size = 10)
# rospy.Subscriber('/r_speed', Float64, self.r_enc_to_speed, queue_size = 10)
# rospy.Subscriber('/angular_return',Float64, self.kalman_callback, queue_size = 5)
self.lock = Condition()
# self.rate = rospy.Rate(1.0)
self.name = socket.gethostname()
# self.current_time = time.time()
global enc_data_r
global enc_data_l
# global a
# self.PID.clear()#set the arguments of PID
# self.PID.setKp(0.115)
# self.PID.setKi(0.0006)
# self.PID.setKd(0.0)
# self.PID.setSampleTime(0.0001)
# self.PID.setWindup(15)
# self.PID_another.clear()
# self.PID_another.setKp(0.45)
# self.PID_another.setKi(0.035)
# self.PID_another.setKd(0)
# self.PID_another.setSampleTime(0.001)
# self.PID_another.setWindup(15)
self.rate = rospy.Rate(70.0)
# self.heartBeat = rospy.Publisher('heartBeat', String, queue_size=5)
self.imu_pub = rospy.Publisher('Imu', Imu, queue_size = 1)
self.r_enc_pub = rospy.Publisher('/r_enc', Int16, queue_size = 1)
self.l_enc_pub = rospy.Publisher('/l_enc', Int16, queue_size = 1)
# self.ly_twist_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
# self.PID_pub = rospy.Publisher('/PID_gethey',Float64,queue_size=1)
self.kalman_pub = rospy.Publisher('/kalman_param', kalman, queue_size = 5)
self.nonekalman_linear = rospy.Publisher('/nonekalman_linear',Float64,queue_size=5)
self.nonekalman_angular = rospy.Publisher('/nonekalman_angular',Float64,queue_size=5)
self.enc_angular = rospy.Publisher('/enc_angular',Float64,queue_size=5)
# self.sudu_pub = rospy.Publisher('/vel',Float64,queue_size = 1)
self.linear_pub = rospy.Publisher('/linear_vel',Float64,queue_size = 1)
self.angular_pub = rospy.Publisher('/angular_vel',Float64,queue_size = 1)
self.imu_count = 0
self.a = 0
self.v = 0
self.enc_data_r = Int16()
self.enc_data_l = Int16()
self.enc_data_r.data = 0
self.enc_data_l.data = 0
self.hey = 0
# global enc_data_r
# global enc_data_l
# global a
# global radius
ly_date = [enc_data_r , enc_data_l]
global ly_date
self.enc_r_last = 0
self.enc_v_last = 0
self.radius = 3.81 / 2
# the robot`s wheel will make 1168 ticks/meter, and get the parament:1168*radius/100 = 0.212504
self.param = 0.213
#enc_data_r = 0
#enc_data_l = 0
# global enc_data_r
# global enc_data_l
numpy.save('ly_enc.npy',ly_date)
self.current_time = time.time()
self.last_time = self.current_time
def kalman_callback(self,data):
ll = data.data
# def l_enc_to_speed(self, data):
# print data.data,'l_vdata'
# def r_enc_to_speed(self, data):
# print data.data,'r_vdata'
# def updata_vel(self,msg):
# v = msg.data
# r = v +0.2*self.a
# l = (v/1.15) - 0.2*self.a
#self.lock.acquire()
#self.zumy.cmd(l,r)
#self.lock.release()
def cmd_callback(self, msg):
# lv = 0.6#???
# la = 0.4#???
# v = msg.linear.x
# a = msg.angular.z
# vv = v / 2
# self.PID.SetPoint = msg.linear.x
# print self.PID.SetPoint
self.v = msg.linear.x
self.a = msg.angular.z
v = self.v
# global a
# global radius
# vv = v*2
# self.linear_pub.publish(vv/10)
# self.PID.setPoint(vv)
# vvv = self.PID.setPoint(vv)
# print vvv #the PID about linear_vel
r = v + (self.param*self.a)
l = (v/1.15) - (self.param*self.a)
self.lock.acquire()
# vv = v*2
# self.PID.setPoint(vv)
# self.PID_another.setPoint(self.a)
# self.cmd = (l,r)
self.zumy.cmd(l,r)
print 'gogogogogogogogogogogogogogogo'
self.lock.release()
def enc_vel(self, rvv, lvv):
self.lock.acquire()
zumyv = ((rvv - lvv) / 2)
# return zumyv
self.lock.release()
return zumyv
def enc_rad(self, rvv, lvv):
self.lock.acquire()
zumyr = -((rvv + lvv) / 0.09)
#return zumyr
self.lock.release()
return zumyr
def run(self):
while not rospy.is_shutdown():
self.lock.acquire()
self.current_time = time.time()
self.last_tiem = self.current_time
# self.zumy.cmd(0.1,0.1)
# print imu and enc data
# self.zumy.read_data()
imu_data = self.zumy.read_imu()
enc_data = self.zumy.read_enc()
# imu_data = [0,0,0,0,0,0]
# enc_data = [0,0]
self.lock.release()
twistImu = Twist()
kalman_msg = kalman()
if len(imu_data) == 6:
imu_msg = Imu()
imu_msg.header = Header(self.imu_count,rospy.Time.now(),self.name)
imu_msg.linear_acceleration.x = 9.81 * imu_data[0]
imu_msg.linear_acceleration.y = 9.81 * imu_data[1]
imu_msg.linear_acceleration.z = 9.81 * imu_data[2]
imu_msg.angular_velocity.x = 3.14 / 180.0 * imu_data[3]
imu_msg.angular_velocity.y = 3.14 / 180.0 * imu_data[4]
imu_msg.angular_velocity.z = 3.14 / 180.0 * imu_data[5]
#twistImu.angular.x = 0
#twistImu.angular.y = 0
#twistImu.angular.z = imu_msg.angular_velocity.z
self.imu_pub.publish(imu_msg)
# kalman_msg = kalman()
kalman_msg.keyboard_linear = self.v
kalman_msg.keyboard_angular = -self.a*3.14
vvv = self.v
aaa = self.a*3.14
self.angular_pub.publish(-aaa)
self.linear_pub.publish(vvv)
kalman_msg.measure_angular = imu_msg.angular_velocity.z
# self.kalman_pub.publish(kalman_msg)
self.nonekalman_angular.publish(kalman_msg.measure_angular)
if len(enc_data) == 2:
enc_msg = Int16()
enc_dif_r = Int16()
enc_dif_l = Int16()
#global enc_data_r
#global enc_data_l
self.lock.acquire()
ly_daenc = numpy.load('ly_enc.npy')
enc_msg.data = enc_data[0]
self.lock.release()
self.r_enc_pub.publish(enc_msg)
#ly_daenc[0] = enc_data[0]
self.lock.acquire()
enc_data_r = ly_daenc[0]
enc_dif_r.data = enc_msg.data - enc_data_r # difference value
enc_msg.data = enc_data[1]
self.lock.release()
self.l_enc_pub.publish(enc_msg)
#ly_daenc[1] = enc_data[1]
self.lock.acquire()
enc_data_l = ly_daenc[1]
enc_dif_l.data = enc_msg.data - enc_data_l # difference value
ly_daenc = enc_data
numpy.save('ly_enc.npy',ly_daenc)
#this get the robocar`s wheels of velocity
# velocity =(( encoder`s value of change in one cycle ) / total number of encoder) * wheel`s perimeter
enc_sr = (float(enc_dif_r.data)/120.0)*30*3.14
# enc_vr =float(enc_dif_r /1167)*17.25 # it don`t work!!!wtf
enc_vr = enc_sr * 0.01725
enc_sl = (float(enc_dif_l.data)/120.0)*30*3.14
# enc_vl =float(enc_dif_l /1167)*17.25
enc_vl = enc_sl * 0.01725
#print enc_dif_r,'dif_r'
#print enc_dif_l,'dif_l'
#print enc_sr,'sr'
#print enc_sl,'sl'
#print enc_vr,'vr'
#print enc_vl,'vl'
self.lock.release()
enc_v_v = self.enc_vel(enc_vr,enc_vl)
enc_v = enc_v_v/4.4
if self.v > self.hey or self.v < self.hey:
if enc_v == self.hey:
enc_v = self.enc_v_last
self.enc_v_last = enc_v
print self.enc_v_last ,'enc_v'
enc_r_r = self.enc_rad(enc_vr,enc_vl)
enc_r = enc_r_r/3
if self.a > self.hey or self.a < self.hey:
if enc_r == 0:
enc_r = self.enc_r_last
self.enc_r_last = enc_r
print self.enc_r_last ,'enc_r'
twistImu.linear.x = self.enc_v_last
twistImu.linear.y = 0
twistImu.linear.z = 0
twistImu.angular.x = 0
twistImu.angular.y = 0
twistImu.angular.z = self.enc_r_last
kalman_msg.linear_vel = self.enc_v_last
self.kalman_pub.publish(kalman_msg)
self.enc_angular.publish(self.enc_r_last)
self.nonekalman_linear.publish(self.enc_v_last)
self.current_time = time.time()
# time_dif = self.current_time - self.last_time
# self.last_time = self.current_time
# print time_dif,'tiem_dif'
# numpy.save('ly_enc.npy',ly_daenc)
#self.ly_twist_pub.publish(twistImu)
#enc_data_r = enc_data[0]#keep last value
# enc_data_l = enc_data[1]
# print enc_data_r
# print enc_data_l
# self.lock.acquire()
# linear_output = self.PID.update(self.enc_v_last)
# angular_output = self.PID_another.update(enc_r)
# print output,'output'
# print linear_output,'linear_output'
# v = linear_output/2
# r = v + (0.2*self.a)
# l =(v/1.15) - (0.2*self.a)
# print a,'a'
# self.lock.acquire()
# vv = v*2
# self.sudu_pub.publish(vv)
# self.lock.acquire()
# self.zumy.cmd(l,r)
# self.lock.release()
time_dif = self.current_time - self.last_time
self.last_time = self.current_time
# print time_dif,'tiem_dif'
# self.sudu_pub.publish(vv)
#self.ly_twist_pub.publish(twistImu)
self.rate.sleep()
class ZumyROS:
def __init__(self):
self.zumy = Zumy()
rospy.init_node('zumy_ros')
self.cmd = (0, 0)
rospy.Subscriber('cmd_vel', Twist, self.cmd_callback, queue_size=1)
rospy.Subscriber('enable', Bool, self.enable_callback, queue_size=1)
rospy.Subscriber(
'/base_computer', String, self.watchdog_callback, queue_size=1
) #/base_computer topic, the global watchdog. May want to investigate what happens when there moer than one computer and more than one zumy
self.lock = Condition()
self.rate = rospy.Rate(30.0)
self.name = socket.gethostname()
if rospy.has_param("~timeout"):
self.timeout = rospy.get_param('~timeout') #private value
else:
self.timeout = 2 #Length of watchdog timer in seconds, defaults to 2 sec.
#publishers
self.status_pub = rospy.Publisher("Status", ZumyStatus, queue_size=5)
self.imu_pub = rospy.Publisher('imu', Imu, queue_size=1)
self.r_enc_pub = rospy.Publisher('r_enc', Float32, queue_size=5)
self.l_enc_pub = rospy.Publisher('l_enc', Float32, queue_size=5)
self.r_vel_pub = rospy.Publisher('r_vel', Float32, queue_size=5)
self.l_vel_pub = rospy.Publisher('l_vel', Float32, queue_size=5)
self.imu_count = 0
self.batt_pub = rospy.Publisher('Batt', Float32, queue_size=5)
self.last_message_at = time.time()
self.watchdog = True
#an array of times, which will be updated, and used to figure out how fast the while loop is going.
self.laptimes = np.ones(6) * time.time()
# NEW!
self.is_turning = False
self.IR_ai_side_pub = rospy.Publisher('/' + self.name + '/IR_ai_side',
Float32,
queue_size=1)
self.IR_ai_front_pub = rospy.Publisher("/" + self.name +
"/IR_ai_front",
Float32,
queue_size=1)
self.directions = {
"F": 0.,
"L": 90.,
"R": -90.,
"B": 180.
} # (CCW, 0 is N) the direction that the the zumy will face
# Publish twists to /cmd_vel in order to drive the Zumy.
self.zumy_vel_pub = rospy.Publisher('/' + self.name + '/cmd_vel',
Twist,
queue_size=2)
def cmd_callback(self, msg):
self.lock.acquire()
self.cmd = twist_to_speeds(
msg
) #update the commanded speed, the next time the main loop in run comes through, it'll be update on the zumy.
self.lock.release()
def enable_callback(self, msg):
#enable or disable myself based on the content of the message.
if msg.data and self.watchdog:
self.zumy.enable()
else:
self.zumy.disable()
self.cmd = (0, 0) #turn the motors off
#do NOT update the watchdog, since the watchdog should do it itself.
#If i'm told to go but the host's watchdog is down, something's very wrong, and i won't be doing much
def watchdog_callback(self, msg):
#update the last time i got a messag!
self.last_message_at = time.time()
self.watchdog = True
# NEW!
def turn_zumy(self, direction):
# angle in degrees
# returns a Twist that is published and turns zumy
omega = np.array([0, 0, 1]) # rotate about z
translation = np.array([0, 0, 0])
angle = self.directions[direction]
theta = deg2rad(angle)
omega = omega * theta
rbt = create_rbt(omega, theta, translation)
v = find_v(omega, theta, translation)
linear = Vector3(v[0] / 5, v[1] / 5, v[2] / 5)
angular = Vector3(omega[0], omega[1], omega[2] / 3)
twist = Twist(linear, angular)
self.zumy_vel_pub.publish(twist)
while True:
if time.time() > start + 3:
self.stop()
break
# self.is_turning = True
return twist
# NEW!
def stop(self):
linear = Vector3(0, 0, 0)
angular = Vector3(0, 0, 0)
stop_twist = Twist(linear, angular)
self.zumy_vel_pub.publish(stop_twist)
# self.is_turning = False
def run(self):
while not rospy.is_shutdown():
self.lock.acquire()
self.zumy.cmd(*self.cmd)
# self.is_turning = False
try:
imu_data = self.zumy.read_imu()
imu_msg = Imu()
imu_msg.header = Header(self.imu_count, rospy.Time.now(),
self.name)
imu_msg.linear_acceleration.x = 9.81 * imu_data[0]
imu_msg.linear_acceleration.y = 9.81 * imu_data[1]
imu_msg.linear_acceleration.z = 9.81 * imu_data[2]
imu_msg.angular_velocity.x = 3.14 / 180.0 * imu_data[3]
imu_msg.angular_velocity.y = 3.14 / 180.0 * imu_data[4]
imu_msg.angular_velocity.z = 3.14 / 180.0 * imu_data[5]
self.imu_pub.publish(imu_msg)
except ValueError:
pass
try:
enc_data = self.zumy.enc_pos()
enc_msg = Float32()
enc_msg.data = enc_data[0]
self.r_enc_pub.publish(enc_msg)
enc_msg.data = enc_data[1]
self.l_enc_pub.publish(enc_msg)
except ValueError:
pass
try:
vel_data = self.zumy.enc_vel()
vel_msg = Float32()
vel_msg.data = vel_data[0]
self.l_vel_pub.publish(vel_msg)
vel_msg.data = vel_data[1]
self.r_vel_pub.publish(vel_msg)
except ValueError:
pass
try:
v_bat = self.zumy.read_voltage()
self.batt_pub.publish(v_bat)
except ValueError:
pass
# NEW!
# Note: zumy code MUST be between acquire/release calls!
# Note: read must be in a try-catch block!
try:
# self.zumy_vel_pub.publish(move_zumy_forward())
# side port is p16
IR_ai_side_data = self.zumy.read_IR_ai_side()
scaled_side_data = IR_ai_side_data * 3.3
self.IR_ai_side_pub.publish(scaled_side_data)
'''
if IR_ai_data < 0.5:
twist = turn_zumy(self.directions['L'])
self.zumy_vel_pub.publish(twist)
else:
twist = turn_zumy(self.directions['R'])
self.zumy_vel_pub.publish(twist)
'''
except ValueError:
self.IR_ai_side_pub.publish(33)
pass
try:
# NEW!
# checks if acceleration about z axis is below a certain level.
if imu_msg.angular_velocity.z < .05:
IR_ai_front_data = self.zumy.read_IR_ai_front()
scaled_front_data = IR_ai_front_data * 3.3
self.IR_ai_front_pub.publish(scaled_front_data)
if scaled_front_data > 2.0:
self.IR_ai_front_pub.publish(9999)
self.turn_zumy('L')
# self.is_turning = True
except ValueError:
self.IR_ai_front_pub.publish(36)
pass
#END NEW
if time.time() > (
self.last_message_at + self.timeout
): #i've gone too long without seeing the watchdog.
self.watchdog = False
self.zumy.disable()
self.zumy.battery_protection(
) # a function that needs to be called with some regularity.
#handle the robot status message
status_msg = ZumyStatus()
status_msg.enabled_status = self.zumy.enabled
status_msg.battery_unsafe = self.zumy.battery_unsafe()
#update the looptimes, which will get published in status_msg
self.laptimes = np.delete(self.laptimes, 0)
self.laptimes = np.append(self.laptimes, time.time())
#take average over the difference of the times... and then invert. That will give you average freq.
status_msg.loop_freq = 1 / float(np.mean(np.diff(self.laptimes)))
self.status_pub.publish(status_msg)
self.lock.release() #must be last command involving the zumy.
self.rate.sleep()
# If shutdown, turn off motors & disable anything else.
self.zumy.disable()
class ZumyROS:
def __init__(self):
self.zumy = Zumy()
rospy.init_node('zumy_ros')
self.cmd = (0,0)
rospy.Subscriber('cmd_vel', Twist, self.cmd_callback,queue_size=1)
rospy.Subscriber('enable', Bool, self.enable_callback,queue_size=1)
rospy.Subscriber('/base_computer',String,self.watchdog_callback,queue_size=1) #/base_computer topic, the global watchdog. May want to investigate what happens when there moer than one computer and more than one zumy
self.lock = Condition()
self.rate = rospy.Rate(30.0)
self.name = socket.gethostname()
if rospy.has_param("~timeout"):
self.timeout = rospy.get_param('~timeout') #private value
else:
self.timeout = 2 #Length of watchdog timer in seconds, defaults to 2 sec.
#publishers
self.status_pub = rospy.Publisher("Status",ZumyStatus, queue_size=5)
self.imu_pub = rospy.Publisher('imu', Imu, queue_size = 1)
self.r_enc_pub = rospy.Publisher('r_enc',Float32, queue_size = 5)
self.l_enc_pub = rospy.Publisher('l_enc',Float32, queue_size = 5)
self.r_vel_pub = rospy.Publisher('r_vel',Float32,queue_size = 5)
self.l_vel_pub = rospy.Publisher('l_vel',Float32,queue_size = 5)
self.imu_count = 0
self.batt_pub = rospy.Publisher('Batt',Float32,queue_size = 5)
self.last_message_at = time.time()
self.watchdog = True
#an array of times, which will be updated, and used to figure out how fast the while loop is going.
self.laptimes = np.ones(6)*time.time()
def cmd_callback(self, msg):
self.lock.acquire()
self.cmd = twist_to_speeds(msg) #update the commanded speed, the next time the main loop in run comes through, it'll be update on the zumy.
self.lock.release()
def enable_callback(self,msg):
#enable or disable myself based on the content of the message.
if msg.data and self.watchdog:
self.zumy.enable()
else:
self.zumy.disable()
self.cmd = (0,0) #turn the motors off
#do NOT update the watchdog, since the watchdog should do it itself.
#If i'm told to go but the host's watchdog is down, something's very wrong, and i won't be doing much
def watchdog_callback(self,msg):
#update the last time i got a messag!
self.last_message_at = time.time()
self.watchdog = True
def run(self):
while not rospy.is_shutdown():
self.lock.acquire()
self.zumy.cmd(*self.cmd)
try:
imu_data = self.zumy.read_imu()
imu_msg = Imu()
imu_msg.header = Header(self.imu_count,rospy.Time.now(),self.name)
imu_msg.linear_acceleration.x = 9.81 * imu_data[0]
imu_msg.linear_acceleration.y = 9.81 * imu_data[1]
imu_msg.linear_acceleration.z = 9.81 * imu_data[2]
imu_msg.angular_velocity.x = 3.14 / 180.0 * imu_data[3]
imu_msg.angular_velocity.y = 3.14 / 180.0 * imu_data[4]
imu_msg.angular_velocity.z = 3.14 / 180.0 * imu_data[5]
self.imu_pub.publish(imu_msg)
except ValueError:
pass
try:
enc_data = self.zumy.enc_pos()
enc_msg = Float32()
enc_msg.data = enc_data[0]
self.r_enc_pub.publish(enc_msg)
enc_msg.data = enc_data[1]
self.l_enc_pub.publish(enc_msg)
except ValueError:
pass
try:
vel_data = self.zumy.enc_vel()
vel_msg = Float32()
vel_msg.data = vel_data[0]
self.l_vel_pub.publish(vel_msg)
vel_msg.data = vel_data[1]
self.r_vel_pub.publish(vel_msg)
except ValueError:
pass
try:
v_bat = self.zumy.read_voltage()
self.batt_pub.publish(v_bat)
except ValueError:
pass
if time.time() > (self.last_message_at + self.timeout): #i've gone too long without seeing the watchdog.
self.watchdog = False
self.zumy.disable()
self.zumy.battery_protection() # a function that needs to be called with some regularity.
#handle the robot status message
status_msg = ZumyStatus()
status_msg.enabled_status = self.zumy.enabled
status_msg.battery_unsafe = self.zumy.battery_unsafe()
#update the looptimes, which will get published in status_msg
self.laptimes = np.delete(self.laptimes,0)
self.laptimes = np.append(self.laptimes,time.time())
#take average over the difference of the times... and then invert. That will give you average freq.
status_msg.loop_freq = 1/float(np.mean(np.diff(self.laptimes)))
self.status_pub.publish(status_msg)
self.lock.release() #must be last command involving the zumy.
self.rate.sleep()
# If shutdown, turn off motors & disable anything else.
self.zumy.disable()
