def onKeyPressed(args):
char = args["char"]
v = None
GameManager.Instance().message("Bryant pressed a button: {}".format(char),
Colors.green)
if char == "UP":
v = Vector2D(0, -1)
elif char == "LEFT":
v = Vector2D(-1, 0)
elif char == "RIGHT":
v = Vector2D(1, 0)
elif char == "DOWN":
v = Vector2D(0, 1)
elif char == "F":
EventManager.Instance().fireEvent("EVENT_FocusCameraOnEntity", player)
return
elif char == "G":
EventManager.Instance().fireEvent("EVENT_FocusCameraOnEntity", None)
return
else:
return
EventManager.Instance().fireEvent("EVENT_MoveEntity", {
"entity": player,
"vector2D": v
})
def __init__(self):
rospy.init_node("Kid", anonymous=True)
# attributes for obstacle avoidance
self.distance = [10.0 for i in range(0,16)]
self.obstacles = []
# attributes for pose management and POI robot-centric localization
self.axisRotationMatrix = self.NO_ROTATION_MATRIX
self.startOrientation = 0.0
self.translationVector = self.NO_TRANSLATION_VECTOR
self.startPoint = Vector2D()
self.theta = 0.0
self.rotationMatrix = self.NO_ROTATION_MATRIX
self.POI = Vector2D()
# releasers
self.targetFound = False
self.avoidReleaser = False
self.canRead = True
self.POIFound = False
# temporal releasers
self.lastPOIFound = -self.MAX_TIME_ELAPSED
self.lastPoseUpdateTime = -self.POSE_UPDATE_RATE
self.lastLookUpdateTime = -self.LOOK_UPDATE_RATE
self.lastSonarReadTime = -self.SONAR_UPDATE_RATE
self.lastRandomField = -self.RANDOM_FIELD_RATE
self.msg = Twist()
self.inGame = False
self.colorToTouch = ""
self.colorTouched = False
# ROS library needed for image conversion from ROS to OpenCV
self.bridge = CvBridge()
# Needed to interpretate images geometrically with camera parameters
self.camera_model = image_geometry.PinholeCameraModel()
self.frameRGBD = None
self.poseInfo = None
# publishers and subscribers
self.velPub = rospy.Publisher("/RosAria/cmd_vel", Twist, queue_size=1)
self.sonarSub = rospy.Subscriber("/RosAria/sonar", PointCloud, self.readSonar)
self.poseSub = rospy.Subscriber("/RosAria/pose", Odometry, self.updatePose)
self.RMSpeakerPub = rospy.Publisher("node_to_rolemanager", String, queue_size=10)
self.RMListenerSub = rospy.Subscriber("rolemanager_to_node", String, self.ownRoleManagerListener)
# Subscriber for computer vision module
self.imageSub = message_filters.Subscriber("/camera/rgb/image_rect_color", Image)
self.depthSub = message_filters.Subscriber("/camera/depth_registered/image", Image)
self.cameraInfoSub = message_filters.Subscriber("/camera/depth_registered/camera_info", CameraInfo)
# Topic time synchronizer
ats = message_filters.ApproximateTimeSynchronizer([self.imageSub,self.depthSub,self.cameraInfoSub], queue_size = 10, slop = 0.1)
ats.registerCallback(self.RGBDSubscriber)
# Color target
self.colorlower = None
self.colorupper = None
def getVector(dir):
if dir == "North":
return Vector2D(0, -1)
elif dir == "West":
return Vector2D(-1, 0)
elif dir == "East":
return Vector2D(1, 0)
elif dir == "South":
return Vector2D(0, 1)
else:
return Vector2D(0, 0)
def wander(self):
#B
currentTime = time.time()
if currentTime-self.lastRandomField<self.RANDOM_FIELD_RATE:
return Vector2D(0.0,1.0)
else:
intensity = 0.5 + random()*0.5
angle = random()*pi
if random()<0.5:
angle = -angle
self.lastRandomField = currentTime
return Vector2D(cos(angle)*intensity, sin(angle)*intensity)
def avoid(self, force):
#B
for p in self.obstacles:
intensity = self.repel(p.getIntensity())
vector = Vector2D(p.getVersorX()*intensity, p.getVersorY()*intensity)
force -= vector
return force
def _scan(self):
scan_results = []
GameManager.Instance().refreshCachedMap()
cached_map = list(GameManager.Instance().cached_map)
rng = self.bot._scan_range
posX = self.entity.components["Transform2D"].position.x
posY = self.entity.components["Transform2D"].position.y
lowerX = max(posX - rng, 0)
lowerY = max(posY - rng, 0)
print(GameManager.Instance().map_size_x,
GameManager.Instance().map_size_y,
self.entity.components["Transform2D"].position.y)
upperX = min(posX + rng, GameManager.Instance().map_size_x - 1)
upperY = min(posY + rng, GameManager.Instance().map_size_y - 1)
for col in xrange(lowerX, upperX + 1):
for row in xrange(lowerY, upperY + 1):
# TODO: COuld be a bug because this assumes no entity can be found on the same tile as your robot.
if len(cached_map[col][row]) > 0:
if self.entity in cached_map[col][row]:
cached_map[col][row].remove(self.entity)
continue
scan_results.append((Vector2D(row,
col), cached_map[col][row]))
return scan_results
def start(self):
# rate_start = rospy.Rate(5)
# wait until the game starts
while not self.inGame and not rospy.is_shutdown():
time.sleep(0.5)
while self.poseInfo is None or self.frameRGBD is None:
continue
self.lastLoop = -0.5
while self.inGame and not rospy.is_shutdown():
self.look()
self.feelForce()
fieldVector = self.wander()
if self.POIFound or time.time()-self.lastPOIFound<self.MAX_TIME_ELAPSED:
fieldVector = self.moveToColor(fieldVector)
if self.avoidReleaser:
fieldVector = self.avoid(fieldVector)
self.avoidReleaser = False
self.move(fieldVector)
self.move(Vector2D())
if self.colorTouched:
print("TROVATO!") # TODO faglielo DIRE
self.ownRoleManagerSpeaker(0)
def lookForPOI(self, currentTime):
frameRGBD = self.frameRGBD
RGBimage = frameRGBD[0]
depthImage = frameRGBD[1]
cameraInfo = frameRGBD[2]
centroid = self.readImageRGB(RGBimage)
self.centroid = centroid
if centroid is not None:
ray = self.extractCameraInfo(cameraInfo, centroid)
distance = self.extractDepth(depthImage, centroid)
self.distance = distance
if distance is not None and not np.isnan(distance):
candidatePOI = Vector2D(ray[0]*distance, ray[2]*distance)
if not self.POIFound and (currentTime - self.lastPOIFound) >= self.MAX_TIME_ELAPSED:
self.POI = candidatePOI
self.POIFound = True
else:
currentPOI = self.POI + self.translationVector
if candidatePOI.getIntensity() < currentPOI.getIntensity():
self.POI = candidatePOI
self.POIFound = True
else:
self.POIFound = False
else:
self.POIFound = False
def create_player():
global player
player = EntityManager.Instance().create_entity('@', z=10)
EntityManager.Instance().add_component(player,
Transform2D(Vector2D(10, 10)))
EntityManager.Instance().add_component(player, Health())
EntityManager.Instance().add_component(
player, Collider(COLLIDER_PLAYER, COLLIDER_PLAYER | COLLIDER_WALL))
GameManager.Instance().message(
"Bryant entered the strange room hesitantly.", Colors.red)
def getRepulsiveField(obsVector, repScale=1, distThresh=800):
Urep = Vector2D(0, 0)
dist = obsVector.length()
# the buffer from your self to the obstacle
dist = max(EPSILON, dist - Constants.ROBOT_DIAM / 2)
# find new obsPos with modified dist
obsVector = obsVector.normalise().multiply(dist)
if dist <= distThresh:
Urep = obsVector.multiply(MAX_REP * repScale *
(1 / distThresh - 1 / dist) / dist)
return Urep
def moveToColor(self, force):
#B
# translate and rotate POI
currentPOI = self.POI+self.translationVector
currentPOI = currentPOI.multiplyMatrix(self.rotationMatrix)
# calculate attraction vector
intensity = self.attract(currentPOI.getIntensity())
vector = Vector2D(currentPOI.getVersorX()*intensity, currentPOI.getVersorY()*intensity)
return vector
def loadMap(self, mapArr):
for row in range(0, len(mapArr)):
for col in range(0, len(mapArr[0])):
entity = EntityManager.Instance().create_entity(
mapArr[row][col])
EntityManager.Instance().add_component(
entity, Transform2D(Vector2D(col, row)))
if entity.symbol == "#":
EntityManager.Instance().add_component(
entity,
Collider(COLLIDER_WALL,
COLLIDER_PLAYER | COLLIDER_WALL))
def getRepulsiveField(obsPos, repScale=1, distThresh=1400):
Urep = Vector2D(0, 0)
myPos = Global.myPos()
dist = myPos.minus(obsPos).length()
dist = max(EPSILON, dist - Constants.ROBOT_DIAM / 2)
# find new obsPos with modified dist
direction = obsPos.minus(myPos).normalised()
obsPos = myPos.plus(direction.multiply(dist))
if dist <= distThresh:
Urep = myPos.minus(obsPos).multiply(MAX_REP * repScale *
(1 / distThresh - 1 / dist) / dist)
return Urep
def feelForce(self):
#PS
points = self.sonarReadings
del self.obstacles[:]
for i in range(1, 8):
p = points[i]
distance = sqrt(p.x**2 + p.y**2)
if distance <= self.OBSTACLE_DISTANCE:
self.obstacles.append(Vector2D(p.y, p.x))
self.avoidReleaser = True
def onKeyPressed(self, args):
char = args["char"]
v = None
GameManager.Instance().message("You pressed a button: {}".format(char), Colors.green)
if char == "UP":
v = Vector2D(0, -1)
elif char == "LEFT":
v = Vector2D(-1, 0)
elif char == "RIGHT":
v = Vector2D(1, 0)
elif char == "DOWN":
v = Vector2D(0, 1)
elif char == "S":
for e, component in EntityManager.Instance().pairs_for_type(Robot):
EventManager.Instance().fireEvent("EVENT_ShootProjectile", {"origin" : e.components["Transform2D"].position, "direction" : e.components["Robot"].bot.direction, "damage" : 20})
return
elif char == "1":
robots = []
for e, component in EntityManager.Instance().pairs_for_type(Robot):
robots.append(e)
if len(robots) >= 1:
EventManager.Instance().fireEvent("EVENT_FocusCameraOnEntity", robots[0])
return
elif char == "2":
robots = []
for e, component in EntityManager.Instance().pairs_for_type(Robot):
robots.append(e)
if len(robots) >= 2:
EventManager.Instance().fireEvent("EVENT_FocusCameraOnEntity", robots[1])
return
elif char == "G":
EventManager.Instance().fireEvent("EVENT_FocusCameraOnEntity", None)
return
else:
return
for e, component in EntityManager.Instance().pairs_for_type(Robot):
EventManager.Instance().fireEvent("EVENT_MoveEntity", {"entity" : e, "vector2D" : v})
def ballWorldVelHighConfidence(minConfidence = 0.15):
velocity = ballWorldVel()
speed = velocity.length()
uncertainty = blackboard.localisation.ballVelEigenvalue * minConfidence
if uncertainty > speed:
return Vector2D(0.0, 0.0)
velocity.normalise()
speed = max(speed - uncertainty, 0.0)
velocity.scale(speed)
return velocity
def addPanel(self, panel):
self.panels.append(panel)
# Shrink game panel to fit within upper left hand corner
if panel.side == Panel.Right:
if panel.offsetX < self.game_panel.width:
self.game_panel.width = panel.offsetX
elif panel.side == Panel.Bottom:
if panel.offsetY < self.game_panel.height:
self.game_panel.height = panel.offsetY
# Setup game window center in relation to panel[0] (main game scene)
self.console_mid = Vector2D(int(self.game_panel.width / 2), int(self.game_panel.height / 2))
self.offset_transform = Transform2D(self.console_mid)
def updatePose(self, msg):
#PS
#currentTime = time.time()
#if currentTime-self.lastPoseUpdateTime<self.POSE_UPDATE_RATE:
# return
#else:
# self.lastPoseUpdateTime = currentTime
pose = msg.pose.pose
msgQuaternion = pose.orientation
msgPoint = pose.position
self.poseInfo = (Vector2D(msgPoint.y, msgPoint.x),
euler_from_quaternion([msgQuaternion.x, msgQuaternion.y, msgQuaternion.z, msgQuaternion.w])[2])
def getRepulsiveField(obsVector, distThresh=800):
Urep = Vector2D(0, 0)
dist = obsVector.length()
# the buffer from your self to the obstacle
dist = max(EPSILON, dist - Constants.ROBOT_DIAM / 2)
repulsion = REP - REP_DECAY * dist
if repulsion < 0:
return Urep
# find new obsPos with modified dist
Urep = obsVector.normalise().multiply(-repulsion)
return Urep
def add_robot(self, type, x, y):
# Create a bot and link back the entity so that user can reference other components
bot = type()
robot = EntityManager.Instance().create_entity(bot.symbol, z=10)
bot.action = Robot(robot, bot)
EntityManager.Instance().add_component(robot, Transform2D(Vector2D(x, y)))
EntityManager.Instance().add_component(robot, Health())
EntityManager.Instance().add_component(robot, Collider(COLLIDER_PLAYER, COLLIDER_PLAYER | COLLIDER_WALL))
EntityManager.Instance().add_component(robot, bot.action)
GameManager.Instance().message("Bryant entered the strange room hesitantly.", Colors.red)
EventManager.Instance().fireEvent("EVENT_StatsUpdated", [{"HP: {0}/{1}".format(robot.components["Health"].health, robot.components["Health"].maxHealth) : {"color" : Colors.gold}},
{"MP: 5/20" : {"color" : Colors.gold}}])
return robot
def ballWorldVelHighConfidence(minConfidence=0.15):
"""
Lower values for the minConfidence means closer to the mean estimated
velocity, but also results in higher uncertainty that the velocity is
truly that fast.
"""
velocity = ballWorldVel()
speed = velocity.length()
uncertainty = blackboard.localisation.ballVelEigenvalue * minConfidence
if uncertainty > speed:
return Vector2D(0.0, 0.0)
velocity.normalise()
speed = max(speed - uncertainty, 0.0)
velocity.scale(speed)
return velocity
def loadMap(self, mapArr):
self.map_size_x = len(mapArr[0])
self.map_size_y = len(mapArr)
print("Map Size: ({0}, {1})".format(self.map_size_x, self.map_size_y))
for row in range(0, len(mapArr)):
for col in range(0, len(mapArr[0])):
if mapArr[row][col] in list(healthLookupTable.keys()):
entity = EntityManager.Instance().create_entity(
mapArr[row][col])
EntityManager.Instance().add_component(
entity, Transform2D(Vector2D(col, row)))
EntityManager.Instance().add_component(
entity, Health(healthLookupTable[mapArr[row][col]]))
EntityManager.Instance().add_component(
entity,
Collider(
COLLIDER_WALL, COLLIDER_PLAYER | COLLIDER_WALL
| COLLIDER_PROJECTILE))
def __init__(self, width, height, font='assets/terminal_8x12.png'):
System.__init__(self)
# Define screen sizes
self.screen_width = width
self.screen_height = height
# Set rendered font
tdl.set_font(font)
# Create console window
self.window = tdl.init(self.screen_width, self.screen_height, "gripe engine")
# Initial screen properties
self.console_mid = Vector2D(int(self.screen_width / 2), int(self.screen_height / 2))
self.offset_transform = Transform2D(self.console_mid)
# Add initial game panel (panel[0] is main game always)
self.game_panel = GamePanel(self.window, 0, 0, self.screen_width, self.screen_height, None, None, self)
self.panels = []
def getAttractiveField(targetVector):
return Vector2D(ATT, 0).rotate(targetVector.heading())
def __init__(self, position=Vector2D(0, 0)):
Component.__init__(self)
self.position = position
def myPos(): return Vector2D(_myPose.x, _myPose.y)
def ballRelVel(): ballVelRRC = blackboard.localisation.ballVelRRC return Vector2D(ballVelRRC.x, ballVelRRC.y)
def ballWorldVel(): ballVel = blackboard.localisation.ballVel return Vector2D(ballVel.x, ballVel.y)
def ballRelPos(): ballPosRRC = blackboard.localisation.ballPosRRC return Vector2D(ballPosRRC.x, ballPosRRC.y)
def ballWorldPos(): return Vector2D(_ballWorldPos.x, _ballWorldPos.y)