def __init__(self):
self.goal_pub = rospy.Publisher("/goal", String, queue_size=10)
self.goal_num_pub = rospy.Publisher("/goal_number", String, queue_size=10)
self.subscriber = rospy.Subscriber("/goal_reached", String, self.goal_callback)
self.goals = {1: [-18,18],2: [-3.5,19],3: [3,19],4: [18,19],5: [-12,13],6: [-3.5,13],8: [12,12],9: [-18,10],10: [18,12],11: [-18,5],12: [-3,6],13: [2.5,7],14: [-4,0],15: [0,0],16: [6.5,2],17: [18,2],18: [-10,-3],19: [-2.5,-2.5], 20: [-18,-4], 21: [-13,-10], 22: [4,-9], 23: [11,-11], 24: [18,-11], 25: [-18,-18], 26: [-7,-16], 27: [4,-15], 28:[18,-18], 29: [2,-3], 30:[3,14], 31: [6,14], 32: [6,11], 33:[2.5,10.5],7:[-12,18.5], 34:[18,0]}
self.next_goal = {1: [5,9,7],2: [3,6,7],3: [2,4,30],4: [3,10],5: [1,6,9,7],6: [2,5,12],8: [10],9: [1,5,11,7],10: [4,8,17],11: [9,18,20],12: [6,13,14,15],13: [12,15,16,33],14: [12,15,18,19],15: [12,13,14,16,19,29],16: [13,15,17,29],17: [10,16,34],18: [11,14],19: [14,15,21,29],20: [11,21,25], 21: [19,20,26], 22:[34,26,27], 23:[24], 24:[34,28,23], 25:[20,26], 26: [21,22,25,27], 27: [22,26,28], 28: [24,27],29:[15,16,19], 30:[3,31], 31:[30,32], 32:[31,33],33:[13,32],7:[1,2,5,9],34: [17,24,22]}
l = len(self.goals)
self.createDict()
self.path = []
random.seed()
while len(self.path) < 4:
self.start = random.randint(1,l)
self.end = self.start
while self.end == self.start:
self.end = random.randint(1,l)
self.path = shortestPath(self.distances,self.start,self.end)
self.current_goal = self.start
print "Start: %i,\tEnd:%i" % (self.start,self.end)
print "Path: ", self.path
self.count = 1
world = open(os.environ['MMDAE'] + 'Worlds/complex/world.world').read()
text_file = open(os.environ['MMDAE'] + 'Worlds/complex/pos_world.world', "w")
a = self.goals[self.path[0]]
b = self.goals[self.path[1]]
text_file.write(world % tuple(self.goals[self.start] + [180*atan2((b[1]-a[1]),(b[0]-a[0]))/pi]))
text_file.close()
self.pause = True
self.msg = String()
time.sleep(2)
self.goal_callback(self.msg)
self.msg.data = str(self.path)
self.goal_num_pub.publish(self.msg)
print 'Ready, start Stage ROS then DAE_node.py.'
def __init__(self):
self.goal_pub = rospy.Publisher("/goal", String, queue_size=10)
self.goal_num_pub = rospy.Publisher("/goal_number", String, queue_size=10)
self.subscriber = rospy.Subscriber("/goal_reached", String, self.goal_callback)
self.goals = {1: [-18.5,18.5], 2: [-11,18.5], 3: [-3.5,18.5], 4: [3.5,18.5], 5: [11,18.5], 6: [18.5,18.5], 7: [-18.5,10.5], 8: [-11,10.5], 9: [-3.5,10.5], 10: [3.5,10.5], 11: [11,10.5], 12: [18.5,10.5], 13: [-18.5,3.5], 14: [-11,3.5], 15: [-3.5,3.5], 16: [3.5,3.5], 17: [11,3.5], 18: [18.5,3.5], 19: [-18.5,-3.5], 20: [-11,-3.5], 21: [-3.5,-3.5], 22: [3.5,-3.5], 23: [11,-3.5], 24: [18.5,-3.5], 25: [-18.5,-10.5], 26: [-11,-10.5], 27: [-3.5,-10.5], 28: [3.5,-10.5], 29: [11,-10.5], 30: [18.5,-10.5], 31: [-18.5,-18.5], 32: [-11,-18.5], 33: [-3.5,-18.5], 34: [3.5,-18.5], 35: [11,-18.5], 36: [18.5,-18.5]}
self.next_goal = {1: [2,7],2: [1,3,8],3: [2,4,9],4: [3,5,10],5: [4,6,11],6: [5,12],7:[1,8,13],8: [2,7,9,14],9: [3,8,10,15],10: [4,9,11,16],11: [5,10,12,17],12: [6,11,18],13: [7,14,19],14: [8,13,15,20],15: [9,14,16,21],16: [10,15,17,22],17: [11,16,18,23],18: [12,17,24],19: [13,20,25],20: [14,19,21,26], 21: [15,20,22,27], 22:[16,21,23,28], 23:[17,22,24,29], 24:[18,23,30], 25:[19,26,31], 26: [20,25,27,32], 27: [21,26,28,33], 28: [22,27,29,34],29:[23,28,30,35], 30:[24,29,36], 31:[25,32], 32:[26,31,33],33:[27,32,34],34: [28,33,35], 35: [29,34,36], 36:[30,35]}
l = len(self.goals)
self.createDict()
self.path = []
random.seed()
while len(self.path) < 6:
self.start = random.randint(1,l)
self.end = self.start
while self.end == self.start:
self.end = random.randint(1,l)
# self.start = 10
# self.end = 22
self.path = shortestPath(self.distances,self.start,self.end)
self.current_goal = self.start
print "Start: %i,\tEnd:%i" % (self.start,self.end)
print "Path: ", self.path
self.count = 1
world = open('/home/james/Dropbox/University/BEB801/PlayerStage/worlds/grid/world.world').read()
text_file = open('/home/james/Dropbox/University/BEB801/PlayerStage/worlds/grid/pos_world.world', "w")
a = self.goals[self.path[0]]
b = self.goals[self.path[1]]
text_file.write(world % tuple(self.goals[self.start] + [180*atan2((b[1]-a[1]),(b[0]-a[0]))/pi]))
text_file.close()
self.pause = True
self.msg = String()
time.sleep(2)
self.goal_callback(self.msg)
self.msg.data = str(self.path)
self.goal_num_pub.publish(self.msg)
# self.msg.data = str(self.current_goal)
# self.goal_num(self.msg)
print 'Ready, start Stage and all_node.py and hit enter.'
def __init__(self):
self.goal_pub = rospy.Publisher("/goal", String, queue_size=10)
self.goal_num_pub = rospy.Publisher("/goal_number",
String,
queue_size=10)
self.subscriber = rospy.Subscriber("/goal_reached", String,
self.goal_callback)
self.goals = {
1: [-18.5, 18.5],
2: [-11, 18.5],
3: [-3.5, 18.5],
4: [3.5, 18.5],
5: [11, 18.5],
6: [18.5, 18.5],
7: [-18.5, 10.5],
8: [-11, 10.5],
9: [-3.5, 10.5],
10: [3.5, 10.5],
11: [11, 10.5],
12: [18.5, 10.5],
13: [-18.5, 3.5],
14: [-11, 3.5],
15: [-3.5, 3.5],
16: [3.5, 3.5],
17: [11, 3.5],
18: [18.5, 3.5],
19: [-18.5, -3.5],
20: [-11, -3.5],
21: [-3.5, -3.5],
22: [3.5, -3.5],
23: [11, -3.5],
24: [18.5, -3.5],
25: [-18.5, -10.5],
26: [-11, -10.5],
27: [-3.5, -10.5],
28: [3.5, -10.5],
29: [11, -10.5],
30: [18.5, -10.5],
31: [-18.5, -18.5],
32: [-11, -18.5],
33: [-3.5, -18.5],
34: [3.5, -18.5],
35: [11, -18.5],
36: [18.5, -18.5]
}
self.next_goal = {
1: [2, 7],
2: [1, 3, 8],
3: [2, 4, 9],
4: [3, 5, 10],
5: [4, 6, 11],
6: [5, 12],
7: [1, 8, 13],
8: [2, 7, 9, 14],
9: [3, 8, 10, 15],
10: [4, 9, 11, 16],
11: [5, 10, 12, 17],
12: [6, 11, 18],
13: [7, 14, 19],
14: [8, 13, 15, 20],
15: [9, 14, 16, 21],
16: [10, 15, 17, 22],
17: [11, 16, 18, 23],
18: [12, 17, 24],
19: [13, 20, 25],
20: [14, 19, 21, 26],
21: [15, 20, 22, 27],
22: [16, 21, 23, 28],
23: [17, 22, 24, 29],
24: [18, 23, 30],
25: [19, 26, 31],
26: [20, 25, 27, 32],
27: [21, 26, 28, 33],
28: [22, 27, 29, 34],
29: [23, 28, 30, 35],
30: [24, 29, 36],
31: [25, 32],
32: [26, 31, 33],
33: [27, 32, 34],
34: [28, 33, 35],
35: [29, 34, 36],
36: [30, 35]
}
l = len(self.goals)
self.createDict()
self.path = []
random.seed()
while len(self.path) < 6:
self.start = random.randint(1, l)
self.end = self.start
while self.end == self.start:
self.end = random.randint(1, l)
# self.start = 10
# self.end = 22
self.path = shortestPath(self.distances, self.start, self.end)
self.current_goal = self.start
print "Start: %i,\tEnd:%i" % (self.start, self.end)
print "Path: ", self.path
self.count = 1
world = open(
'/home/james/Dropbox/University/BEB801/PlayerStage/worlds/grid/world.world'
).read()
text_file = open(
'/home/james/Dropbox/University/BEB801/PlayerStage/worlds/grid/pos_world.world',
"w")
a = self.goals[self.path[0]]
b = self.goals[self.path[1]]
text_file.write(
world % tuple(self.goals[self.start] +
[180 * atan2((b[1] - a[1]), (b[0] - a[0])) / pi]))
text_file.close()
self.pause = True
self.msg = String()
time.sleep(2)
self.goal_callback(self.msg)
self.msg.data = str(self.path)
self.goal_num_pub.publish(self.msg)
# self.msg.data = str(self.current_goal)
# self.goal_num(self.msg)
print 'Ready, start Stage and all_node.py and hit enter.'
