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