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() self.heartBeat = rospy.Publisher('heartBeat', String, queue_size=1) 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.volt_pub = rospy.Publisher('voltage', Float32, queue_size=1) self.imu_count = 0 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() volt_data = self.zumy.read_voltage() 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) volt_msg = Float32() volt_msg.data = volt_data self.volt_pub.publish(volt_msg) #self.heartBeat.publish("I am alive") self.rate.sleep() # If shutdown, turn off motors self.zumy.cmd(0, 0)