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):
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.zumy = Zumy()
self.name = socket.gethostname()
self.heartBeat = rospy.Publisher('/' + self.name + '/heartBeat', String, 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)
self.heartBeat.publish("I am alive")
self.lock.release()
self.rate.sleep()
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('cmd_vel', Twist, self.cmd_callback_test, queue_size=1)
self.lock = Condition()
self.rate = rospy.Rate(30)
self.name = socket.gethostname()
self.vl = 0
self.vr = 0
self.feedback = True
self.alpha = 0.8
self.heartBeat = rospy.Publisher('/' + self.name + '/heartBeat', String, queue_size=5)
self.imu_pub = rospy.Publisher('/' + self.name + '/imu', Imu, queue_size = 1)
self.r_enc_pub = rospy.Publisher('/' + self.name + '/r_enc', Int32, queue_size = 5)
self.l_enc_pub = rospy.Publisher('/' + self.name + '/l_enc', Int32, queue_size = 5)
self.l_spd_pub = rospy.Publisher('/'+ self.name + '/l_spd_pub', Float32, queue_size = 5)
self.r_spd_pub = rospy.Publisher('/' + self.name + '/r_spd_pub', Float32, queue_size = 5)
self.l_denc_pub = rospy.Publisher('/' + self.name + '/l_denc_pub', Float32, queue_size = 5)
self.r_denc_pub = rospy.Publisher('/' + self.name + '/r_denc_pub', Float32, queue_size = 5)
self.duration_pub = rospy.Publisher('/'+ self.name + '/Duration', Float32, queue_size = 5)
self.imu_count = 0
#self.timestamp = rospy.Time.now()
#self.prevtimestamp = rospy.Time.now()
#self.duration = (self.timestamp - self.prevtimestamp).to_sec()
#self.pl = PID(0.0008,0.000055,0.00005)
#self.pr = PID(0.0009,0.000065,0.00006)
self.l_enc_count = 0
self.l_enc_prev_count = 0
self.l_enc_count_diff = 0
self.prev_l_enc_count_diff = 0
self.r_enc_count = 0
self.r_ecn_prev_count = 0
self.r_enc_count_diff = 0
self.prev_r_enc_count_diff = 0
self.l_vel_list = []
self.r_vel_list = []
def cmd_callback(self, msg):
lv = 1
la = 1
v = msg.linear.x
a = msg.angular.z
self.vr = lv*v - la*a
self.vl = lv*v + la*a
self.zumy.PID_l.setPoint(self.vl)
self.zumy.PID_r.setPoint(self.vr)
def cmd_callback_test(self,msg):
self.vl = msg.linear.x
self.vr = msg.angular.z
self.zumy.PID_l.setPoint(self.vl)
self.zumy.PID_r.setPoint(self.vr)
def run(self):
while not rospy.is_shutdown():
self.zumy.update_enc_denc()
self.zumy.update_time()
# self.prevtimestamp = self.timestamp
# self.timestamp = rospy.Time.now()
# self.duration = (self.timestamp - self.prevtimestamp).to_sec()
# self.duration_pub.publish(self.duration)
# if time_now - self.timestamp > .5:
# self.cmd = (0,0)
# self.lock.acquire()
# 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[1]
# self.r_enc_pub.publish(enc_msg)
# enc_msg.data = enc_data[0]
# self.l_enc_pub.publish(enc_msg)
# self.l_enc_prev_count = self.l_enc_count
# self.r_enc_prev_count = self.r_enc_count
# self.l_enc_count = enc_data[0]
# self.r_enc_count = enc_data[1]
# self.prev_l_enc_count_diff = self.l_enc_count_diff
# self.l_enc_count_diff = self.alpha*\
# (self.l_enc_count - self.l_enc_prev_count)/\
# self.duration + \
# self.alpha*\
# self.prev_l_enc_count_diff
# self.prev_r_enc_count_diff = self.r_enc_count_diff
# self.r_enc_count_diff = self.alpha*\
# (self.r_enc_count - self.r_enc_prev_count)/\
# self.duration + \
# self.alpha*\
# self.prev_r_enc_count_diff
pid_l = self.zumy.PID_l.update(self.zumy.denc[0]/self.zumy.duration)
pid_r = self.zumy.PID_r.update(self.zumy.denc[1]/self.zumy.duration)
#pid_l = confined(pid_l, 0.2)
#pid_r = confined(pid_r, 0.2)
# pid_l_pub = Float32()
# pid_r_pub = Float32()
# l_set_point_pub = Float32()
# r_set_point_pub = Float32()
# l_set_point_pub.data = self.l_enc_count_diff
# r_set_point_pub.data = self.r_enc_count_diff
# pid_l_pub.data = pid_l
# pid_r_pub.data = pid_r
# self.l_spd_pub.publish(pid_l_pub)
# self.r_spd_pub.publish(pid_r_pub)
self.l_denc_pub.publish(self.zumy.denc[0]/self.zumy.duration)
self.r_denc_pub.publish(self.zumy.denc[1]/self.zumy.duration)
self.l_vel_list.append(self.zumy.denc[0]/self.zumy.duration)
self.r_vel_list.append(self.zumy.denc[1]/self.zumy.duration)
self.heartBeat.publish("I am alive from Glew!!")
self.lock.acquire()
if self.feedback:
self.zumy.cmd(pid_l,pid_r)
else:
self.zumy.cmd(self.vl, self.vr)
self.lock.release()
self.rate.sleep()
# If shutdown, turn off motors
self.zumy.cmd(0,0)
f = open('/home/glew/coop_slam_workspace/src/ros_zumy/src/test.txt', 'a')
for ii in range(len(self.l_vel_list)):
f.write(("%.2f\t" % self.l_vel_list[ii]))
f.write(("%.2f\n" % self.r_vel_list[ii]))
f.close()
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()
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_test, queue_size=1)
self.lock = Condition()
self.rate = rospy.Rate(30)
self.name = socket.gethostname()
self.vl_enc = 0
self.vr_enc = 0
self.feedback = True
self.heartBeat = rospy.Publisher('/' + self.name + '/heartBeat', String, queue_size=5)
self.imu_pub = rospy.Publisher('/' + self.name + '/imu', Imu, queue_size = 1)
self.r_enc_pub = rospy.Publisher('/' + self.name + '/r_enc', Int32, queue_size = 5)
self.l_enc_pub = rospy.Publisher('/' + self.name + '/l_enc', Int32, queue_size = 5)
self.l_spd_pub = rospy.Publisher('/'+ self.name + '/l_spd_pub', Float32, queue_size = 5)
self.r_spd_pub = rospy.Publisher('/' + self.name + '/r_spd_pub', Float32, queue_size = 5)
self.l_denc_pub = rospy.Publisher('/' + self.name + '/l_denc_pub', String, queue_size = 5)
self.r_denc_pub = rospy.Publisher('/' + self.name + '/r_denc_pub', String, queue_size = 5)
self.duration_pub = rospy.Publisher('/'+ self.name + '/Duration', Float32, queue_size = 5)
self.imu_count = 0
self.timestamp = rospy.Time.now()
#self.prevtimestamp = rospy.Time.now()
#self.duration = (self.timestamp - self.prevtimestamp).to_sec()
#self.pl = PID(0.0008,0.000055,0.00005)
#self.pr = PID(0.0009,0.000065,0.00006)
self.l_enc_count = 0
self.l_enc_prev_count = 0
self.l_enc_count_diff = 0
self.prev_l_enc_count_diff = 0
self.r_enc_count = 0
self.r_ecn_prev_count = 0
self.r_enc_count_diff = 0
self.prev_r_enc_count_diff = 0
self.l_vel_list = []
self.r_vel_list = []
def cmd_callback(self, msg):
v_tr = msg.linear.x
omega_rot = msg.angular.z
zumy_width = 0.086*8/4 # distance between the center of two front wheels of zumy in meter
v_l = v_tr - omega_rot*zumy_width/2
v_r = v_tr + omega_rot*zumy_width/2
enc_cnt = 600 #encoder count per rev
wheel_radius = 0.02 #Wheel radius in meter
self.vl_enc = v_l*enc_cnt/(2*math.pi*wheel_radius)
self.vr_enc = v_r*enc_cnt/(2*math.pi*wheel_radius)
self.zumy.PID_l.setPoint(self.vl_enc)
self.zumy.PID_r.setPoint(self.vr_enc)
def cmd_callback_test(self,msg):
self.vl = msg.linear.x
self.vr = msg.angular.z
self.zumy.PID_l.setPoint(self.vl)
self.zumy.PID_r.setPoint(self.vr)
def run(self):
# i=0
# msg = Twist()
# msg.linear.x = 0.01
# msg.angular.z = 0
# self.cmd_callback(msg)
# time1 = time.time()
while not rospy.is_shutdown():# and i<900:
i = i+1
#Get the feedback and update the feecback control
self.zumy.update_enc_denc()
self.zumy.update_time()
pid_l = self.zumy.PID_l.update(self.zumy.denc[0]/self.zumy.duration)
pid_r = self.zumy.PID_r.update(self.zumy.denc[1]/self.zumy.duration)
# self.prevtimestamp = self.timestamp
# self.timestamp = rospy.Time.now()
# self.duration = (self.timestamp - self.prevtimestamp).to_sec()
# self.duration_pub.publish(self.duration)
#if time_now - self.timestamp > .5:
# self.cmd = (0,0)
# self.lock.acquire()
# 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)
#Publish current encoder value
enc_msg = Int32()
enc_msg.data = self.zumy.enc[1]
self.r_enc_pub.publish(enc_msg)
enc_msg.data = self.zumy.enc[0]
self.l_enc_pub.publish(enc_msg)
#Publish current encoder difference value
l_denc_pub = String()
r_denc_pub = String()
l_denc_pub.data = "%.2f" % (self.zumy.denc[0]/self.zumy.duration)
r_denc_pub.data = "%.2f" % (self.zumy.denc[1]/self.zumy.duration)
self.l_denc_pub.publish(l_denc_pub)
self.r_denc_pub.publish(r_denc_pub)
#Append current encoder speed to save to txt file
self.l_vel_list.append(self.zumy.denc[0]/self.zumy.duration)
self.r_vel_list.append(self.zumy.denc[1]/self.zumy.duration)
self.heartBeat.publish("I am alive from Glew!!")
print "Hello"
self.lock.acquire()
if self.feedback:
self.zumy.cmd(pid_l,pid_r)
else:
self.zumy.cmd(0,0)
self.lock.release()
self.rate.sleep()
time2 = time.time()
duration = time2-time1
# If shutdown, turn off motors
self.zumy.cmd(0,0)
class PID: #PID controller
def __init__(self, P=0.2, I=0.0, D=0.0):
self.Kp = P
self.Ki = I
self.kd = D
self.sample_time = 0.00
self.current_time = time.time()
self.last_time = self.current_time
self.zumy = Zumy()
self.clear()
self.setKp(0.5)
self.setKi(0.4)
self.setKd(0.0001)
rospy.init_node('PID', anonymous=True)
self.rate = rospy.Rate(70.0)
self.lock = Condition()
rospy.Subscriber('/cmd_vel1', Twist, self.cmd_callback, queue_size=1)
rospy.Subscriber('/PID_gethey', Float64, self.PID_go, queue_size=1)
self.sudu_pub = rospy.Publisher('/vel', Float64, queue_size=1)
self.a = 0
# self.clear()
def cmd_callback(self, msg):
self.v_set = msg.linear.x
self.a = msg.angular.z
def clear(self):
"""Clears PID computations and coefficients"""
self.SetPoint = 0.0
self.PTerm = 0.0
self.ITerm = 0.0
self.DTerm = 0.0
self.last_error = 0.0
# Windup Guard
self.int_error = 0.0
self.windup_guard = 5.0
self.output = 0.0
def PID_go(self, feedback_value):
"""Calculates PID value for given reference feedback
.. math::
u(t) = K_p e(t) + K_i \int_{0}^{t} e(t)dt + K_d {de}/{dt}
.. figure:: images/pid_1.png
:align: center
Test PID with Kp=1.2, Ki=1, Kd=0.001 (test_pid.py)
"""
error = self.SetPoint - feedback_value.data
self.current_time = time.time()
delta_time = self.current_time - self.last_time
delta_error = error - self.last_error
if (delta_time >= self.sample_time):
self.PTerm = self.Kp * error
self.ITerm += error * delta_time
if (self.ITerm < -self.windup_guard):
self.ITerm = -self.windup_guard
elif (self.ITerm > self.windup_guard):
self.ITerm = self.windup_guard
self.DTerm = 0.0
if delta_time > 0:
self.DTerm = delta_error / delta_time
# Remember last time and last error for next calculation
self.last_time = self.current_time
self.last_error = error
self.output = self.PTerm + (self.Ki * self.ITerm) + (self.Kd *
self.DTerm)
def run(self):
while not rospy.is_shutdown():
#self.lock.acquire()
self.sudu_pub.publish(self.output)
v = self.output
aa = self.a
r = v + 0.2 * aa
l = v - 0.2 * aa
self.lock.acquire()
self.zumy.cmd(l, r)
self.lock.release()
self.rate.sleep()
def setKp(self, proportional_gain):
"""Determines how aggressively the PID reacts to the current error with setting Proportional Gain"""
self.Kp = proportional_gain
def setKi(self, integral_gain):
"""Determines how aggressively the PID reacts to the current error with setting Integral Gain"""
self.Ki = integral_gain
def setKd(self, derivative_gain):
"""Determines how aggressively the PID reacts to the current error with setting Derivative Gain"""
self.Kd = derivative_gain
def setWindup(self, windup):
"""Integral windup, also known as integrator windup or reset windup,
refers to the situation in a PID feedback controller where
a large change in setpoint occurs (say a positive change)
and the integral terms accumulates a significant error
during the rise (windup), thus overshooting and continuing
to increase as this accumulated error is unwound
(offset by errors in the other direction).
The specific problem is the excess overshooting.
"""
self.windup_guard = windup
def setSampleTime(self, sample_time):
"""PID that should be updated at a regular interval.
Based on a pre-determined sampe time, the PID decides if it should compute or return immediately.
"""
self.sample_time = sample_time