Ejemplo n.º 1
0
    def blockCallback(self, data, args):
	blockInfo = self.parsePose(data)

	xv = burlap_value()
        xv.attribute = 'x'
        xv.value = str(blockInfo[0])

        yv = burlap_value()
        yv.attribute = 'y'
        yv.value = str(blockInfo[1])

	cv = burlap_value()
	cv.attribute = 'color'
	cv.value = 'yellow'
	
	sv = burlap_value()
	sv.attribute = 'shape'
	sv.value = 'chair'

	block = burlap_object()
	block.name = 'block' + str(args)
	block.object_class = 'block'
	block.values = [xv, yv, cv, sv]

	self.blocks[args] = block

	self.publishState()
Ejemplo n.º 2
0
    def blockCallback(self, data, args):
        blockInfo = self.parsePose(data)

        xv = burlap_value()
        xv.attribute = 'x'
        xv.value = str(blockInfo[0])

        yv = burlap_value()
        yv.attribute = 'y'
        yv.value = str(blockInfo[1])

        cv = burlap_value()
        cv.attribute = 'color'
        cv.value = 'yellow'

        sv = burlap_value()
        sv.attribute = 'shape'
        sv.value = 'chair'

        block = burlap_object()
        block.name = 'block' + str(args)
        block.object_class = 'block'
        block.values = [xv, yv, cv, sv]

        self.blocks[args] = block

        self.publishState()
Ejemplo n.º 3
0
    def turtlesimCallback(self, data):
        x = int(math.floor(data.x))
        y = int(math.floor(data.y))

        normAngle = data.theta / (2.0 * math.pi)
        if normAngle < 0.0:
            normAngle = 1.0 + normAngle

        dir = ""
        if normAngle < 1.0 / 8.0 or normAngle >= 7.0 / 8.0:
            dir = "east"
        elif normAngle >= 1.0 / 8.0 and normAngle < 3.0 / 8.0:
            dir = "north"
        elif normAngle >= 3.0 / 8.0 and normAngle < 5.0 / 8.0:
            dir = "west"
        elif normAngle >= 5.0 / 8.0 and normAngle < 7.0 / 8.0:
            dir = "south"
        else:
            print "error in angle:", data.theta, normAngle

        xv = burlap_value()
        xv.attribute = "x"
        xv.value = str(x)

        yv = burlap_value()
        yv.attribute = "y"
        yv.value = str(y)

        dirv = burlap_value()
        dirv.attribute = "direction"
        dirv.value = dir

        agent = burlap_object()
        agent.name = "turtle"
        agent.object_class = "agent"
        agent.values = [xv, yv, dirv]

        self.agent = agent

        self.publishState()
Ejemplo n.º 4
0
    def turtlesimCallback(self, data):
        x = int(math.floor(data.x))
        y = int(math.floor(data.y))

        normAngle = data.theta / (2. * math.pi)
        if normAngle < 0.:
            normAngle = 1. + normAngle

        dir = ''
        if normAngle < 1. / 8. or normAngle >= 7. / 8.:
            dir = 'east'
        elif normAngle >= 1. / 8. and normAngle < 3. / 8.:
            dir = 'north'
        elif normAngle >= 3. / 8. and normAngle < 5. / 8.:
            dir = 'west'
        elif normAngle >= 5. / 8. and normAngle < 7. / 8.:
            dir = 'south'
        else:
            print 'error in angle:', data.theta, normAngle

        xv = burlap_value()
        xv.attribute = 'x'
        xv.value = str(x)

        yv = burlap_value()
        yv.attribute = 'y'
        yv.value = str(y)

        dirv = burlap_value()
        dirv.attribute = 'direction'
        dirv.value = dir

        agent = burlap_object()
        agent.name = 'turtle'
        agent.object_class = 'agent'
        agent.values = [xv, yv, dirv]

        self.agent = agent

        self.publishState()
Ejemplo n.º 5
0
    def turtlebotCallback(self, data):
	botInfo = self.parsePose(data)
	xv = burlap_value()
        xv.attribute = 'x'
        xv.value = str(botInfo[0])

	yv = burlap_value()
        yv.attribute = 'y'
        yv.value = str(botInfo[1])

	dv = burlap_value()
	dv.attribute = 'direction'
	dv.value = botInfo[2]

	agent = burlap_object()
	agent.name = 'turtlebot'
	agent.object_class = 'agent'
	agent.values = [xv, yv, dv]

	self.agent = agent

	self.publishState()
Ejemplo n.º 6
0
    def turtlebotCallback(self, data):
        botInfo = self.parsePose(data)
        xv = burlap_value()
        xv.attribute = 'x'
        xv.value = str(botInfo[0])

        yv = burlap_value()
        yv.attribute = 'y'
        yv.value = str(botInfo[1])

        dv = burlap_value()
        dv.attribute = 'direction'
        dv.value = botInfo[2]

        agent = burlap_object()
        agent.name = 'turtlebot'
        agent.object_class = 'agent'
        agent.values = [xv, yv, dv]

        self.agent = agent

        self.publishState()
Ejemplo n.º 7
0
def talker():
    pub = rospy.Publisher('burlap_state', burlap_state, queue_size=10)
    rospy.init_node('talker', anonymous=True)
    rate = rospy.Rate(10)  # 10hz
    while not rospy.is_shutdown():
        xv = burlap_value()
        xv.attribute = 'x'
        xv.value = '3'

        yv = burlap_value()
        yv.attribute = 'y'
        yv.value = '5'

        agent = burlap_object()
        agent.name = 'turtlebot'
        agent.object_class = 'agent'
        agent.values = [xv, yv]

        s = burlap_state()
        s.objects = [agent]

        pub.publish(s)
        rate.sleep()
Ejemplo n.º 8
0
def talker():
    pub = rospy.Publisher("burlap_state", burlap_state, queue_size=10)
    rospy.init_node("talker", anonymous=True)
    rate = rospy.Rate(10)  # 10hz
    while not rospy.is_shutdown():
        xv = burlap_value()
        xv.attribute = "x"
        xv.value = "3"

        yv = burlap_value()
        yv.attribute = "y"
        yv.value = "5"

        agent = burlap_object()
        agent.name = "turtlebot"
        agent.object_class = "agent"
        agent.values = [xv, yv]

        s = burlap_state()
        s.objects = [agent]

        pub.publish(s)
        rate.sleep()