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