예제 #1
0
파일: omdc.py 프로젝트: jwcjdenissen/ROSMAV
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)
예제 #2
0
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)
예제 #3
0
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()
예제 #4
0
파일: omclock.py 프로젝트: rmeertens/ROSMAV
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()