def go():
pub = rospy.Publisher('overhead_map_objs', Overhead_Map_Objs)
rospy.init_node('omclock')
# main control loop
r = rospy.Rate(10) # hz
while not rospy.is_shutdown():
# Get time. Make an analog clock face with center (centerX,centerY)
# and largest radius "radius"
# This one's set up for the soccer field's center
centerX = 2
centerY = 1.25
radius = 1
curTime = time.localtime()
hours = curTime.tm_hour
minutes = curTime.tm_min
seconds = curTime.tm_sec + (time.time() % 1) # for partial seconds
# Hour hand is an arrow
hourAngle = clockAngle(
(hours + minutes * 1. / 60 + seconds * 1. / 3600) * 1. / 12)
hourHand = Overhead_Map_Obj()
hourHand.name = "arrow"
hourHand.tuple = [
centerX, centerY, centerX + 0.5 * radius * cos(hourAngle),
centerY + 0.5 * radius * sin(hourAngle)
]
# Minute hand is a line
minuteAngle = clockAngle((minutes + seconds * 1. / 60) * 1. / 60)
minuteHand = Overhead_Map_Obj()
minuteHand.name = "line"
minuteHand.tuple = [
centerX, centerY, centerX + 0.75 * radius * cos(minuteAngle),
centerY + 0.75 * radius * sin(minuteAngle)
]
# Second "hand" is a robot going around the outside
secondAngle = clockAngle(seconds * 1. / 60)
secondHand = Overhead_Map_Obj()
secondHand.name = "irobot-create"
secondHand.tuple = [
centerX + radius * cos(secondAngle),
centerY + radius * sin(secondAngle), secondAngle - pi / 2
]
# Center "dot"
centerDot = Overhead_Map_Obj()
centerDot.name = "point"
centerDot.tuple = [centerX, centerY]
# Put it all together (front drawn first)
drawing = Overhead_Map_Objs()
drawing.objs = [secondHand, minuteHand, hourHand, centerDot]
pub.publish(drawing)
# wait for next loop
r.sleep()
def go():
pub = rospy.Publisher('overhead_map_objs',Overhead_Map_Objs)
rospy.init_node('omclock')
# main control loop
r = rospy.Rate(10) # hz
while not rospy.is_shutdown():
# Get time. Make an analog clock face with center (centerX,centerY)
# and largest radius "radius"
# This one's set up for the soccer field's center
centerX = 2
centerY = 1.25
radius = 1
curTime = time.localtime();
hours = curTime.tm_hour
minutes = curTime.tm_min
seconds = curTime.tm_sec + (time.time()%1) # for partial seconds
# Hour hand is an arrow
hourAngle = clockAngle((hours+minutes*1./60+seconds*1./3600)*1./12)
hourHand = Overhead_Map_Obj()
hourHand.name = "arrow"
hourHand.tuple = [centerX,
centerY,
centerX+0.5*radius*cos(hourAngle),
centerY+0.5*radius*sin(hourAngle)]
# Minute hand is a line
minuteAngle = clockAngle((minutes+seconds*1./60)*1./60)
minuteHand = Overhead_Map_Obj()
minuteHand.name = "line"
minuteHand.tuple = [centerX,
centerY,
centerX+0.75*radius*cos(minuteAngle),
centerY+0.75*radius*sin(minuteAngle)]
# Second "hand" is a robot going around the outside
secondAngle = clockAngle(seconds*1./60)
secondHand = Overhead_Map_Obj()
secondHand.name = "irobot-create"
secondHand.tuple = [centerX+radius*cos(secondAngle),
centerY+radius*sin(secondAngle),
secondAngle-pi/2]
# Center "dot"
centerDot = Overhead_Map_Obj()
centerDot.name = "point"
centerDot.tuple = [centerX, centerY]
# Put it all together (front drawn first)
drawing = Overhead_Map_Objs()
drawing.objs = [secondHand, minuteHand, hourHand, centerDot]
pub.publish(drawing)
# wait for next loop
r.sleep()
def tpup(tps):
global pub
omos = Overhead_Map_Objs()
omos.objs = []
# copy all the tags from tag_positions over
for i in range(len(tps.tag_positions)):
tp = tps.tag_positions[i]
omo = Overhead_Map_Obj()
# set the tuple now because we know we want an image,
# since we are getting arTag / blob info. from planar_tracker.
omo.tuple = [tp.x, tp.y, tp.theta]
if tp.id < 0: # blob from cmvision; assume it's a ball
omo.name = "ball"
else: # arTag; assume it's an iRobot Create
omo.name = "irobot-create"
omos.objs.append(omo)
pub.publish(omos)
def tpup(tps):
global pub
omos = Overhead_Map_Objs()
omos.objs = []
# copy all the tags from tag_positions over
for i in range(len(tps.tag_positions)):
tp = tps.tag_positions[i]
omo = Overhead_Map_Obj()
# set the tuple now because we know we want an image,
# since we are getting arTag / blob info. from planar_tracker.
omo.tuple = [tp.x, tp.y, tp.theta]
if tp.id < 0: # blob from cmvision; assume it's a ball
omo.name = "ball"
else: # arTag; assume it's an iRobot Create
omo.name = "irobot-create"
omos.objs.append(omo)
pub.publish(omos)