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