00001
00002
00003 import roslib; roslib.load_manifest('pacman_controller')
00004 from visualization_msgs.msg import Marker
00005 from visualization_msgs.msg import MarkerArray
00006 from geometry_msgs.msg import PoseStamped,Point
00007 from std_msgs.msg import String
00008 from collvoid_msgs.msg import PoseTwistWithCovariance
00009
00010 import commands
00011
00012 import rospy
00013 import math
00014
00015 THRESHOLD = 0.20
00016
00017 EAT_POINTS = 10
00018 EAT_SPECIAL = 50
00019 EAT_CHERRY = 100
00020 EAT_GHOST = 200
00021
00022 TIME_FLEEING = 20.0
00023
00024 def dist(a, b):
00025 return math.sqrt(math.pow(a['x'] - b['x'], 2) + math.pow(a['y'] - b['y'], 2))
00026
00027 class State():
00028 SETUP = -1
00029 INIT = 0
00030 RUNNING = 1
00031 FLEEING = 2
00032 PAUSED = 3
00033 GAME_OVER = 4
00034 STOPPED = 5
00035 WON = 6
00036
00037 class GameEngine():
00038 def __init__(self):
00039
00040 self.points = []
00041 self.edges = []
00042 self.initialized = False
00043 self.pacman = {}
00044 self.ghosts = {}
00045
00046 self.scoreLoc = rospy.get_param('/score_loc')
00047 self.infoLoc = rospy.get_param('/info_loc')
00048 self.power_ups = rospy.get_param('/power_ups')
00049
00050 self.readMap()
00051
00052
00053 self.state = State.STOPPED
00054
00055 self.score = 0;
00056 self.sendStart = False
00057
00058 topic = 'pacman_map'
00059
00060 self.pubMap = rospy.Publisher(topic, MarkerArray)
00061 self.subCommandsRobot= rospy.Subscriber("/commands_robot", String, self.commandsRobotCallback)
00062 self.subCommonPositions = rospy.Subscriber("/position_share", PoseTwistWithCovariance, self.positionShareCallback)
00063
00064 self.pubPositions = rospy.Publisher('/game_positions', MarkerArray)
00065
00066 self.pubScore = rospy.Publisher('/score', Marker)
00067
00068 self.pubInfo = rospy.Publisher('/info', Marker)
00069 self.pubState = rospy.Publisher('/state', String)
00070
00071
00072
00073
00074 def update(self):
00075 self.publishMap();
00076 self.publishState()
00077 self.publishPositions();
00078 self.publishScore()
00079
00080 if self.state in [State.GAME_OVER, State.STOPPED, State.PAUSED, State.INIT, State.WON, State.SETUP]:
00081 return;
00082
00083 self.eatMapPoints();
00084 self.checkGhosts()
00085 if (self.gameWon):
00086 self.state == State.WON
00087
00088 def gameWon(self):
00089 for point in self.mapPoints:
00090 if not point['eaten']:
00091 return False
00092 return True
00093
00094 def publishState(self):
00095 self.pubState.publish(str(self.state));
00096 marker = Marker()
00097 marker.header.frame_id = "/map"
00098 marker.header.stamp = rospy.get_rostime()
00099 marker.type = marker.TEXT_VIEW_FACING
00100 marker.action = marker.ADD
00101 marker.scale.x = 1.0
00102 marker.scale.y = 0.2
00103 marker.scale.z = 0.5
00104 marker.color.a = 1.0
00105 marker.color.r = 1.0
00106 marker.color.g = 0.0
00107 marker.color.b = 0.0
00108 marker.pose.orientation.w = 1.0
00109 marker.pose.position.x = self.infoLoc['x']
00110 marker.pose.position.y = self.infoLoc['y']
00111 marker.pose.position.z = 0.0
00112 marker.id = 0
00113 if (self.state in [State.RUNNING]):
00114 if not self.sendStart:
00115 marker.text = 'Start'
00116 self.sendStart = True
00117 self.timeSend = rospy.get_rostime()
00118 elif (rospy.get_rostime()-self.timeSend).to_sec() > 0.2:
00119
00120 marker.action = marker.DELETE
00121 else:
00122 marker.text = 'Start'
00123
00124 if (self.state == State.FLEEING):
00125 timeFleeing = TIME_FLEEING-(rospy.get_rostime()-self.timeStartFleeing).to_sec()
00126 if timeFleeing > 0:
00127 marker.text = str(timeFleeing)
00128 else:
00129 marker.action = marker.DELETE
00130 self.state = State.RUNNING
00131
00132 if (self.state in [State.GAME_OVER, State.STOPPED, State.SETUP]):
00133 marker.text = 'GAME OVER!'
00134
00135 if (self.state in [State.INIT]):
00136 marker.text = 'Initializing Game'
00137
00138 if (self.state in [State.PAUSED]):
00139 marker.text = 'Game Paused'
00140
00141 if (self.state in [State.WON]):
00142 marker.text = 'Congratulations, you have won the Game! Now get back to you real job!'
00143
00144 self.pubInfo.publish(marker)
00145
00146 def publishScore(self):
00147 marker = Marker()
00148 marker.header.frame_id = "/map"
00149 marker.type = marker.TEXT_VIEW_FACING
00150 marker.action = marker.ADD
00151 marker.scale.x = 0.2
00152 marker.scale.y = 0.2
00153 marker.scale.z = 0.5
00154 marker.color.a = 1.0
00155 marker.color.r = 1.0
00156 marker.color.g = 0.0
00157 marker.color.b = 0.0
00158 marker.pose.orientation.w = 1.0
00159 marker.pose.position.x = self.scoreLoc['x']
00160 marker.pose.position.y = self.scoreLoc['y']
00161 marker.pose.position.z = 0.0
00162 marker.id = 0
00163 marker.text = 'Score: ' + str(self.score)
00164
00165 self.pubScore.publish(marker)
00166
00167 def reset(self):
00168 self.score = 0
00169 self.sendStart = False
00170 for point in self.mapPoints:
00171 point['eaten'] = False
00172
00173 def checkGhosts(self):
00174 for ghost in self.ghosts:
00175 if (dist(self.pacman, self.ghosts[ghost]) < self.pacman['radius'] + self.ghosts[ghost]['radius'] + 1.5*THRESHOLD):
00176 if self.state == State.RUNNING and not self.ghosts[ghost]['eaten']:
00177 self.state = State.GAME_OVER
00178 return
00179 elif self.state == State.FLEEING:
00180 if not self.ghosts[ghost]['eaten']:
00181 self.score += EAT_GHOST
00182 self.ghosts[ghost]['eaten'] = True
00183
00184 def eatMapPoints(self):
00185 for point in self.mapPoints:
00186 if dist(self.pacman, point) < self.pacman['radius'] + THRESHOLD:
00187 if (not point['eaten']):
00188 self.score += EAT_POINTS
00189 point['eaten'] = True
00190 if (point['powerup']):
00191 self.state = State.FLEEING
00192 self.timeStartFleeing = rospy.get_rostime()
00193
00194
00195 def publishPositions(self):
00196 if not self.initialized:
00197 return
00198 nr = 0
00199 markerArray = MarkerArray()
00200 marker = Marker()
00201 marker.header.frame_id = "/map"
00202 marker.type = marker.MESH_RESOURCE
00203 marker.action = marker.ADD
00204 marker.scale.x = 0.2
00205 marker.scale.y = 0.2
00206 marker.scale.z = 0.2
00207
00208 marker.mesh_use_embedded_materials = True
00209 marker.mesh_resource = "package://pacman_controller/meshes/pacman.dae"
00210
00211
00212 marker.color.a = 1.0
00213 marker.color.r = 0.0
00214 marker.color.g = 1.0
00215 marker.color.b = 0.0
00216 marker.pose.orientation = self.pacman['orientation']
00217 marker.pose.position.x = self.pacman['x']
00218 marker.pose.position.y = self.pacman['y']
00219 marker.pose.position.z = 0.0
00220 marker.id = nr
00221 markerArray.markers.append(marker)
00222
00223 for ghost in self.ghosts:
00224 curGhost = self.ghosts[ghost]
00225 if not curGhost["initialized"]:
00226 continue
00227 nr += 1
00228 marker = Marker()
00229 marker.header.frame_id = "/map"
00230 marker.type = marker.MESH_RESOURCE
00231 marker.action = marker.ADD
00232 marker.scale.x = 0.3
00233 marker.scale.y = 0.3
00234 marker.scale.z = 0.3
00235
00236 marker.mesh_use_embedded_materials = True
00237 if curGhost["eaten"]:
00238 marker.mesh_resource = "package://pacman_controller/meshes/dead.dae"
00239 elif self.state == State.FLEEING:
00240 marker.mesh_resource = "package://pacman_controller/meshes/ghost_catchable.dae"
00241 else:
00242 marker.mesh_resource = "package://pacman_controller/meshes/%s.dae"%ghost
00243 marker.color.a = 1.0
00244 marker.color.r = 0.0
00245 marker.color.g = 1.0
00246 marker.color.b = 0.0
00247 marker.pose.orientation = curGhost['orientation']
00248 marker.pose.position.x = curGhost['x']
00249 marker.pose.position.y = curGhost['y']
00250 marker.pose.position.z = 0.0
00251 marker.id = nr
00252 markerArray.markers.append(marker)
00253
00254
00255 self.pubPositions.publish(markerArray)
00256 return
00257
00258 def positionBlocked(self, position, agent):
00259 if not agent == self.pacman:
00260 if (dist(position, self.pacman) < self.pacman['radius'] + THRESHOLD):
00261 return True
00262 for ghost in self.ghosts:
00263 if self.ghosts[ghost] == agent:
00264 continue
00265 if (dist(position, self.ghosts[ghost]) < self.ghosts[ghost]['radius'] + THRESHOLD):
00266 return True
00267
00268
00269 return False
00270
00271 def readMap(self):
00272 vertices = rospy.get_param("/vertices");
00273 edgesRead = rospy.get_param("/edges");
00274 self.points = []
00275 self.edges = []
00276 for i in range(len(vertices)):
00277
00278 point = {}
00279 point['x'] = vertices['point_%d'%i]['x']
00280 point['y'] = vertices['point_%d'%i]['y']
00281 point['id'] = vertices['point_%d'%i]['id']
00282 point['neighbors'] = []
00283 self.points.append(point)
00284
00285 for i in range(len(edgesRead)):
00286
00287 self.addEdge(edgesRead['edge_%d'%i])
00288 self.edges.append(edgesRead['edge_%d'%i])
00289
00290 self.createMapPoints();
00291
00292
00293 def addEdge(self, edge):
00294 if not edge['a'] in self.points[edge['b']]['neighbors']:
00295 self.points[edge['b']]['neighbors'].append(edge['a'])
00296 if not edge['b'] in self.points[edge['a']]['neighbors']:
00297 self.points[edge['a']]['neighbors'].append(edge['b'])
00298
00299 def commandsRobotCallback(self, msg):
00300 if (msg.data == "Start") and self.state == State.INIT:
00301 self.state = State.RUNNING
00302
00303 if (msg.data == "Setup"):
00304 self.state = State.SETUP
00305
00306
00307 if (msg.data == "Stop"):
00308 self.state = State.STOPPED
00309
00310
00311 if (msg.data == "Init") and self.state in [State.STOPPED, State.GAME_OVER, State.SETUP]:
00312 self.state = State.INIT
00313 self.reset()
00314
00315 if self.state in [State.STOPPED, State.GAME_OVER]:
00316 return
00317
00318 if (msg.data == "Pause") and self.state in [State.RUNNING, State.FLEEING]:
00319 self.old_state = self.state
00320 self.state = State.PAUSED
00321
00322 if (msg.data == "Resume") and self.state == State.PAUSED:
00323 self.state = self.old_state
00324
00325
00326 def positionShareCallback(self, msg):
00327 if (msg.robot_id == "pacman"):
00328 self.pacman['x'] = msg.pose.pose.position.x
00329 self.pacman['y'] = msg.pose.pose.position.y
00330 self.pacman['orientation'] = msg.pose.pose.orientation
00331 self.pacman['radius'] = msg.radius
00332 self.initialized = True
00333
00334 else:
00335 if not msg.robot_id in self.ghosts:
00336 self.ghosts[msg.robot_id] = {}
00337 self.ghosts[msg.robot_id]['initialized'] = False
00338
00339 self.ghosts[msg.robot_id]['eaten'] = False
00340 self.ghosts[msg.robot_id]['home'] = self.points[rospy.get_param('/%s/home/'%msg.robot_id)]
00341 self.ghosts[msg.robot_id]['x'] = msg.pose.pose.position.x
00342 self.ghosts[msg.robot_id]['y'] = msg.pose.pose.position.y
00343 self.ghosts[msg.robot_id]['orientation'] = msg.pose.pose.orientation
00344
00345 self.ghosts[msg.robot_id]['radius'] = msg.radius
00346 if self.ghosts[msg.robot_id]['eaten'] and dist(self.ghosts[msg.robot_id]['home'], self.ghosts[msg.robot_id]) < self.ghosts[msg.robot_id]['radius'] + THRESHOLD:
00347 self.ghosts[msg.robot_id]['eaten'] = False
00348 self.ghosts[msg.robot_id]['initialized'] = True
00349
00350 def createMapPoints(self):
00351 self.mapPoints = [];
00352 for edge in self.edges:
00353
00354 point_a = self.points[edge['a']]
00355 point_b = self.points[edge['b']]
00356 cur_point = {}
00357 cur_point['x'] = point_a['x']
00358 cur_point['y'] = point_a['y']
00359 direction = self.get_dir(point_a, point_b)
00360 while (dist(cur_point, point_b) > 0.2):
00361 point = {}
00362 point['x'] = cur_point['x'] + 0.2 * direction[0]
00363 point['y'] = cur_point['y'] + 0.2 * direction[1]
00364 point['neighbors'] = [point_a['id'], point_b['id']]
00365 point['eaten'] = False
00366 point['powerup'] = False
00367 cur_point = point
00368 self.mapPoints.append(point)
00369 for node in self.power_ups['nodes']:
00370 point = self.points[node]
00371 point['eaten'] = False
00372 point['powerup'] = True
00373 self.mapPoints.append(point)
00374
00375
00376 def findClosestMapPoint(self,position):
00377 minDist = dist(self.mapPoints[0], position)
00378 minPoint = self.mapPoints[0]
00379 for p in self.mapPoints:
00380 if p["powerup"]:
00381 continue;
00382 distNew = dist(p, position)
00383 if distNew < minDist:
00384 minDist = distNew
00385 minPoint = p
00386 return minPoint
00387
00388
00389 def findClosestWP(self, position):
00390 minPoint = self.findClosestMapPoint(position)
00391 distA = dist(minPoint, self.points[minPoint['neighbors'][0]])
00392 distB = dist(minPoint, self.points[minPoint['neighbors'][1]])
00393 if (distA > distB):
00394 return minPoint['neighbors'][1]
00395 else:
00396 return minPoint['neighbors'][0]
00397
00398 def publishMap(self):
00399 markerArray = MarkerArray()
00400 marker = Marker()
00401 marker.header.frame_id = "/map"
00402 marker.type = marker.SPHERE_LIST
00403 marker.action = marker.ADD
00404 marker.scale.x = 0.05
00405 marker.scale.y = 0.05
00406 marker.scale.z = 0.05
00407 marker.color.a = 1.0
00408 marker.color.r = 1.0
00409 marker.color.g = 1.0
00410 marker.color.b = 0.0
00411 marker.pose.orientation.w = 1.0
00412 marker.pose.position.x = 0.0
00413 marker.pose.position.y = 0.0
00414 marker.pose.position.z = 0.0
00415 marker.id = 0
00416 for p in self.mapPoints:
00417 if p['eaten']:
00418 continue
00419 if p['powerup']:
00420 continue
00421 point = Point()
00422 point.x = p['x']
00423 point.y = p['y']
00424 point.z = 0.15
00425 marker.points.append(point)
00426 markerArray.markers.append(marker)
00427
00428 marker = Marker()
00429 marker.header.frame_id = "/map"
00430 marker.type = marker.SPHERE_LIST
00431 marker.action = marker.ADD
00432 marker.scale.x = 0.3
00433 marker.scale.y = 0.3
00434 marker.scale.z = 0.3
00435 marker.color.a = 1.0
00436 marker.color.r = 1.0
00437 marker.color.g = 1.0
00438 marker.color.b = 0.0
00439 marker.pose.orientation.w = 1.0
00440 marker.pose.position.x = 0.0
00441 marker.pose.position.y = 0.0
00442 marker.pose.position.z = 0.0
00443 marker.id = 1
00444 for p in self.mapPoints:
00445 if p['eaten']:
00446 continue
00447 if not p['powerup']:
00448 continue
00449 point = Point()
00450 point.x = p['x']
00451 point.y = p['y']
00452 point.z = 0.2
00453 marker.points.append(point)
00454 markerArray.markers.append(marker)
00455
00456 self.pubMap.publish(markerArray)
00457
00458
00459
00460 def publishMarkers(self):
00461 markerArray = MarkerArray()
00462 marker = Marker()
00463 marker.header.frame_id = "/map"
00464 marker.type = marker.SPHERE_LIST
00465 marker.action = marker.ADD
00466 marker.scale.x = 0.2
00467 marker.scale.y = 0.2
00468 marker.scale.z = 0.2
00469 marker.color.a = 1.0
00470 marker.color.r = 0.0
00471 marker.color.g = 1.0
00472 marker.color.b = 0.0
00473 marker.pose.orientation.w = 1.0
00474 marker.pose.position.x = 0.0
00475 marker.pose.position.y = 0.0
00476 marker.pose.position.z = 0.0
00477 marker.id = 0
00478 for p in self.points:
00479 point = Point()
00480 point.x = p['x']
00481 point.y = p['y']
00482 marker.points.append(point)
00483
00484
00485 for p in self.points:
00486 marker = Marker()
00487 marker.header.frame_id = "/map"
00488 marker.type = marker.TEXT_VIEW_FACING
00489 marker.action = marker.ADD
00490 marker.scale.x = 0.2
00491 marker.scale.y = 0.2
00492 marker.scale.z = 0.2
00493 marker.color.a = 1.0
00494 marker.color.r = 1.0
00495 marker.color.g = 0.0
00496 marker.color.b = 0.0
00497 marker.pose.orientation.w = 1.0
00498 marker.pose.position.x = p['x']
00499 marker.pose.position.y = p['y']
00500 marker.pose.position.z = 0.0
00501 marker.id = p['id']+1
00502 marker.text = 'Node: ' + str(p['id'])
00503 markerArray.markers.append(marker)
00504
00505
00506
00507
00508 nr = len(self.points)+1
00509 for edge in self.edges:
00510
00511 point_a = self.points[edge['a']]
00512 point_b = self.points[edge['b']]
00513 marker = Marker()
00514 marker.header.frame_id = "/map"
00515 marker.type = marker.SPHERE_LIST
00516 marker.action = marker.ADD
00517 marker.scale.x = 0.05
00518 marker.scale.y = 0.05
00519 marker.scale.z = 0.05
00520 marker.color.a = 1.0
00521 marker.color.r = 0.0
00522 marker.color.g = 1.0
00523 marker.color.b = 0.0
00524 marker.pose.orientation.w = 1.0
00525 marker.pose.position.x = 0.0
00526 marker.pose.position.y = 0.0
00527 marker.pose.position.z = 0.0
00528 marker.id = nr
00529 cur_point = {}
00530 cur_point['x'] = point_a['x']
00531 cur_point['y'] = point_a['y']
00532 direction = self.get_dir(point_a, point_b)
00533 while (dist(cur_point, point_b) > 0.2):
00534 point = Point()
00535 point.x = cur_point['x'] + 0.2 * direction[0]
00536 point.y = cur_point['y'] + 0.2 * direction[1]
00537 cur_point['x'] = point.x
00538 cur_point['y'] = point.y
00539 marker.points.append(point)
00540 markerArray.markers.append(marker)
00541 nr +=1
00542 self.pubMap.publish(markerArray)
00543
00544 def get_dir(self, point_a, point_b):
00545 result_x = point_b['x'] - point_a['x']
00546 result_y = point_b['y'] - point_a['y']
00547 dis = dist(point_a,point_b)
00548 return [result_x / dis, result_y / dis]
00549
00550
00551 def find_shortest_path(self,start, end, path = [], pathIds = []):
00552 if pathIds == []:
00553 pathIds = pathIds + [-1]
00554 else:
00555 pathIds = pathIds + [start['id']]
00556 path = path + [start]
00557
00558 if start == end:
00559 return path
00560 shortest = None
00561 for node in start['neighbors']:
00562
00563 if node not in pathIds:
00564 newpath = self.find_shortest_path(self.points[node], end, path, pathIds)
00565 if newpath:
00566 if not shortest or len(newpath) < len(shortest):
00567 shortest = newpath
00568 return shortest
00569
00570
00571
00572 if __name__ == '__main__':
00573 rospy.init_node('game_engine')
00574 gameEngine = GameEngine()
00575 while not rospy.is_shutdown():
00576 gameEngine.update()
00577 rospy.sleep(0.05)
00578
00579
