diff --git a/examples/human_motion_control.py b/examples/human_motion_control.py index 9250e0e3670d814590f37b6ded6ecfa4629158f7..15aaa1ea16072761c56f13bf085a6e599029782f 100755 --- a/examples/human_motion_control.py +++ b/examples/human_motion_control.py @@ -6,7 +6,7 @@ import numpy as np def run_scenario_01(): world_config = { - 'visible_area': [[-5., 10.],[-5., 5.]], + 'visible_area': [[-10., 10.], [-10., 10.]], 'objects': [ {'type': 'Wall', 'id': 1, 'position': [6., 0], 'orientation': 0., 'length': 10.}, {'type': 'Wall', 'id': 2, 'position': [-6., 0], 'orientation': 0., 'length': 10.}, diff --git a/examples/scene_01.py b/examples/scene_01.py index 3ae2cc0103dfa6e72105d350dc53e86e3e2ebde3..a6710634eda9b28d1475e546522fb2a472984b47 100755 --- a/examples/scene_01.py +++ b/examples/scene_01.py @@ -16,7 +16,7 @@ def main(): age_min = 0 age_max = 99 world_config = { - 'visible_area': [[-5., 10.],[-5., 5.]], + 'visible_area': [[-10., 10.], [-10., 10.]], 'objects': [ {'type': 'Wall', 'id': 1, 'position': [6., 0], 'orientation': 0., 'length': 10.}, {'type': 'Wall', 'id': 2, 'position': [-6., 0], 'orientation': 0., 'length': 10.}, @@ -63,7 +63,7 @@ def main(): ari_robot = simulation.objects[50] # ari_robot.navigation.set_go_towards_human(30) # ari_robot.navigation.set_human_look_at(30) - ari_robot.navigation.set_go_towards_position([-4., 3.], 0.) + #ari_robot.navigation.set_go_towards_position([-4., 3.], 0.) group_1 = sim.scripts.GroupNavigation() simulation.add_script(group_1, id=1001) diff --git a/examples/stepwise_execution.py b/examples/stepwise_execution.py index 5f8049f0861cb2d142a6780be624a87d87aca106..9dfb1e0c09f940184c8b43c63fcc2e92897b9aeb 100755 --- a/examples/stepwise_execution.py +++ b/examples/stepwise_execution.py @@ -10,7 +10,7 @@ if __name__ == '__main__': # create an empty room simulation = mpi_sim.Simulation( - visible_area=[[-5., 10.], [-5., 5.]], + visible_area=[[-10., 10.], [-10., 10.]], objects=[ {'type': 'Wall', 'position': [6., 0], 'orientation': 0., 'length': 10.}, {'type': 'Wall', 'position': [-6., 0], 'orientation': 0., 'length': 10.}, diff --git a/examples/welcome_scenario_02.py b/examples/welcome_scenario_02.py index d2134180ceb3862f3603bb1ab834c150d2acfe3e..16d0e0881a0d2d314fdfdf5b0f16a714631a9482 100755 --- a/examples/welcome_scenario_02.py +++ b/examples/welcome_scenario_02.py @@ -65,7 +65,7 @@ class GroupDiscussionScript(mpi_sim.Script): # create an empty room simulation = mpi_sim.Simulation( - visible_area = [[-10., 10.],[-10., 10.]], + visible_area = [[0., 10.], [0., 10.]], objects = [ {'type': 'Wall', 'position': [6., 3.], 'orientation': 0., 'length': 6.}, # east {'type': 'Wall', 'position': [0., 3.], 'orientation': 0., 'length': 6.}, # west diff --git a/mpi_sim/__init__.py b/mpi_sim/__init__.py index fd67ca55293e2f1168e054bd8451f0504a509861..71cd458dd14b71a6100e453a4ee844d45b28dd5e 100644 --- a/mpi_sim/__init__.py +++ b/mpi_sim/__init__.py @@ -22,4 +22,4 @@ from mpi_sim.utils.attrdict import combine_dicts # import the module library last, so that it can find all other modules in the mpi_sim to load them import mpi_sim.core.module_library -__version__ = '0.0.8' +__version__ = '0.0.9' diff --git a/mpi_sim/core/box2d_simulation.py b/mpi_sim/core/box2d_simulation.py index 41dca9655278cb311a5d3c813f13823908098f98..224e2370b50c109102616542491306aab2b14f34 100644 --- a/mpi_sim/core/box2d_simulation.py +++ b/mpi_sim/core/box2d_simulation.py @@ -1,10 +1,27 @@ import mpi_sim as sim import Box2D -from Box2D import (b2ContactListener, b2GetPointStates) -from mpi_sim.utils.box2d_utils import fwDestructionListener +from Box2D import b2ContactListener, b2GetPointStates, b2DestructionListener, b2Fixture, b2Joint from time import time +class fwDestructionListener(b2DestructionListener): + """ + The destruction listener callback: + "SayGoodbye" is called when a joint or shape is deleted. + """ + + def __init__(self, test, **kwargs): + super(fwDestructionListener, self).__init__(**kwargs) + self.test = test + + def SayGoodbye(self, obj): + if isinstance(obj, b2Joint): + self.test.JointDestroyed(obj) + elif isinstance(obj, b2Fixture): + self.test.FixtureDestroyed(obj) + + + class Box2DSimulation(b2ContactListener): @staticmethod @@ -51,10 +68,6 @@ class Box2DSimulation(b2ContactListener): # TODO: is a ground body needed ? self.groundbody = self.world.CreateBody(userData={'name': 'ground'}) - # Keep track of the pressed keys - # TODO: remove pressed keys from the simulation and let the gui directly communicate with the agents if it wants to control them - self.pressed_keys = set() - def set_reset_point(self): pass @@ -65,9 +78,6 @@ class Box2DSimulation(b2ContactListener): self.using_contacts = False self.n_steps = 0 - # TODO: is this needed? - self.mouseJoint = None - def step(self, step_length): self.n_steps += 1 diff --git a/mpi_sim/processes/gui.py b/mpi_sim/processes/gui.py index 3f9d1fd4ad384dcaed614e2d608e086997de2407..d31efdf8144ac2ca87633bfa7a68c98481305c17 100644 --- a/mpi_sim/processes/gui.py +++ b/mpi_sim/processes/gui.py @@ -1,90 +1,125 @@ -import mpi_sim as sim +import mpi_sim +from mpi_sim.utils.attrdict import AttrDict from mpi_sim.core.process import Process -import time -import pygame import os -from pygame.locals import (QUIT, KEYDOWN, KEYUP, MOUSEBUTTONDOWN, MOUSEBUTTONUP, MOUSEMOTION, KMOD_LSHIFT) -from mpi_sim.utils.box2d_utils import (Keys, fwQueryCallback, PygameDraw, gui, fwGUI) -from Box2D import (b2Color, b2Vec2, b2DrawExtended, b2PolygonShape, b2_epsilon, b2_persistState, b2_nullState, b2_addState, b2CircleShape, b2AABB) -from Box2D.examples.top_down_car import TDTire -from Box2D.b2 import (staticBody, dynamicBody) - -# TODO: allow the gui to pause the simulation by adding a pause / unpause function to simulation (needs to take into account to not pause the gui) +import pygame +from pygame import Color +from pygame.locals import QUIT, KEYDOWN, KEYUP, MOUSEBUTTONDOWN, MOUSEBUTTONUP, MOUSEMOTION, KMOD_LSHIFT +from Box2D import b2Color, b2Vec2, b2DrawExtended, b2PolygonShape, b2_persistState, b2_nullState, b2_addState, b2CircleShape, b2AABB, b2QueryCallback +from Box2D.examples.pgu import gui + +# TODO (Feature): allow the gui to pause the simulation by adding a pause / unpause function to simulation (needs to take into account to not pause the gui) +# TODO (Feature): allow to control ari or humans, by clicking on them + +# color schemes, (R, G, B [, alpha]) +COLOR_SCHEME_LIGHT = AttrDict( + background = Color(255, 255, 255), + text = Color(0, 0, 0), + border = Color(0, 0, 0), + object = Color(100, 100, 100), + ARIRobot = Color(200, 80, 80), + Human = Color(20, 100, 100), + contact_add = Color(90, 250, 90), + contact_persist = Color(90, 90, 250), + contact_normal = Color(230, 230, 120), +) + +COLOR_SCHEME_DARK = AttrDict( + background=Color(0, 0, 0), + text=Color(255, 255, 255), + border=Color(255, 255, 255), + object=Color(150, 150, 150), + ARIRobot=Color(200, 80, 80), + Human=Color(20, 100, 100), + contact_add=Color(90, 250, 90), + contact_persist=Color(90, 90, 250), + contact_normal=Color(230, 230, 120), +) class GUI(Process): r"""GUI""" - description = "Keys: accel = w, reverse = s, left = a, right = d, pan_right = m, pan_left = n, zoom in = z, zoom out = x" - TEXTLINE_START = 30 - colors = { - 'mouse_point': b2Color(0, 1, 0), - 'bomb_center': b2Color(0, 0, 1.0), - 'bomb_line': b2Color(0, 1.0, 1.0), - 'joint_line': b2Color(0.8, 0.8, 0.8), - 'contact_add': b2Color(0.3, 0.95, 0.3), - 'contact_persist': b2Color(0.3, 0.3, 0.95), - 'contact_normal': b2Color(0.4, 0.9, 0.4), - staticBody: b2Color(0., 1., 0.), - dynamicBody: b2Color(1., 0., 0.), - 'info_window': b2Color(1., 1., 1.), - 'info_text': b2Color(1., 1., 1.), - 'black': b2Color(0., 0., 0.), - } - @staticmethod def default_config(): dc = Process.default_config() dc.name = 'gui' + dc.caption = 'Multiparty Interaction Simulation (v{})'.format(mpi_sim.__version__) + + dc.color_scheme = COLOR_SCHEME_LIGHT # 'light', 'night' + dc.screen_width = 1200 dc.screen_height = 900 dc.viewZoom = 70.0 - dc.viewCenter = [-1.5, 0.] - dc.move_world = False - dc.gui_background = False - dc.key_control = True + dc.viewCenter = None + dc.move_world = True # TODO: update move_world feature + # dc.key_control = True # maybe used later for key control dc.print_collision = True # wait_init_phase: False # sleep_init_time: 1. # Drawing - dc.drawStats = False - dc.drawShapes = True - dc.drawJoints = True - dc.drawCoreShapes = False - dc.drawAABBs = False - dc.drawOBBs = False - dc.drawPairs = False - dc.drawContactPoints = False - dc.drawContactNormals = False - dc.drawFPS = True - dc.drawMenu = False # toggle by pressing F1 - dc.drawCOMs = False # Centers of mass - dc.pointSize = 2.5 # pixel radius for drawing points - - dc.checkboxes = (("Warm Starting", "enable_warm_starting"), - ("Time of Impact", "enable_continuous"), - ("Sub-Stepping", "enable_sub_stepping"), - ("Draw", None), - ("Shapes", "drawShapes"), - ("Joints", "drawJoints"), - ("AABBs", "drawAABBs"), - ("Pairs", "drawPairs"), - ("Contact Points", "drawContactPoints"), - ("Contact Normals", "drawContactNormals"), - ("Center of Masses", "drawCOMs"), - ("Statistics", "drawStats"), - ("FPS", "drawFPS"), - ("Control", None), - ("Pause", "pause"), - ("Single Step", "singleStep")) - - dc.sliders = [ - #{'name': 'hz', 'text': 'Hertz', 'min': 5, 'max': 200}, - {'name': 'pos_iters', 'text': 'Pos Iters', 'min': 0, 'max': 100}, - {'name': 'vel_iters', 'text': 'Vel Iters', 'min': 1, 'max': 500}, - ] + dc.draw_agent_information = True # should the agent type and its id be shown? + dc.draw_object_information = False # should the agent type and its id be shown? + dc.draw_dialogue = True # should the agent type and its id be shown? + dc.draw_debug_information = False + dc.draw_stats = False + dc.draw_rtf = False + dc.draw_menu = False # toggle by pressing F1 + + dc.point_size = 2.5 # pixel radius for drawing points + dc.text_line_start = 30 + + dc.agent_information = mpi_sim.AttrDict( + x_offset = 25, # y offset to the object + y_offset = -10, # y offset to the object + x_padding = 10, # y offset to the object + y_padding = 5, # y offset to the object + border_color = None, + background_color = None, + filter = mpi_sim.AttrDict(types=[mpi_sim.objects.Human, mpi_sim.objects.ARIRobot]), # what are agents? + ) + + dc.object_information = mpi_sim.AttrDict( + x_offset = 25, # y offset to the object + y_offset = -10, # y offset to the object + x_padding = 10, # y offset to the object + y_padding = 5, # y offset to the object + border_color = None, + background_color = None, + ) + + dc.popup_information = mpi_sim.AttrDict( + x_offset = 25, # y offset to the object + y_offset = -10, # y offset to the object + x_padding = 10, # y offset to the object + y_padding = 5, # y offset to the object + border_color = 'border', + background_color = 'background', + ) + + dc.menu = mpi_sim.AttrDict( + checkboxes = [ + #('Warm Starting', 'enable_warm_starting'), + #('Time of Impact', 'enable_continuous'), + #('Sub-Stepping', 'enable_sub_stepping'), + ('Draw', None), + ('Agent Info', 'draw_agent_information'), + ('Object Info', 'draw_object_information'), + ('Dialogue', 'draw_dialogue'), + ('Debug Mode', 'draw_debug_information'), + ('Statistics', 'draw_stats'), + ('RTF', 'draw_rtf'), + #('Control', None), + #('Pause', 'pause'), + #('Single Step', 'singleStep') + ], + + sliders = [ + #{'name': 'hz', 'text': 'Hertz', 'min': 5, 'max': 200}, + ] + ) return dc @@ -92,11 +127,16 @@ class GUI(Process): def __init__(self, config=None, **kwargs): super().__init__(config=config, **kwargs) - self.n_steps = 0 + if isinstance(self.config.color_scheme, str): + if self.config.color_scheme == 'light': + self.config.color_scheme = COLOR_SCHEME_LIGHT + elif self.config.color_scheme == 'dark': + self.config.color_scheme = COLOR_SCHEME_DARK + else: + raise ValueError('Unknown color scheme \'{}\'! Available: \'light\', \'dark\'.') - self.mouseJoint = None - self.mouseWorld = None - self.query_userData = {'name' : 0, 'obj': None, 'time': 0.} + self._mouse_world = None + self._mouse_over_object = None # object over which the mouse is currently self.human_speech_history = [] self.human_speech_history_length = 12 # Screen/rendering-related @@ -104,29 +144,62 @@ class GUI(Process): self._viewCenter = None self._viewOffset = None self.screenSize = None - self.rMouseDown = False - self.textLine = 30 + self.r_mouse_down = False + + self._text_line = self.config.text_line_start + self.offsetLine = 5 self.offsetYRect = 10 self.offsetXRect = 25 + self.font = None self.fps = 0 self.time_infos_displayed = 4. # GUI-related (PGU) - self.gui_app = None - self.gui_table = None - self.setup_keys() + self._gui_app = None + self._gui_table = None - self.init = True - self.drawMenu = self.config.drawMenu + # setup keys + keys = [s for s in dir(pygame.locals) if s.startswith('K_')] + for key in keys: + value = getattr(pygame.locals, key) + setattr(Keys, key, value) - self.t_draws = [] - self.t_steps = [] - os.environ['SDL_AUDIODRIVER'] = 'dsp' - if self.config.gui_background: - os.environ["SDL_VIDEODRIVER"] = "dummy" + + # Pygame Initialization + pygame.init() + pygame.display.set_caption(self.config.caption) + + # Screen and debug draw + flags = pygame.DOUBLEBUF + self.screen = pygame.display.set_mode((self.config.screen_width, self.config.screen_height), flags) + self.screenSize = b2Vec2(*self.screen.get_size()) + + self.renderer = PygameDraw(surface=self.screen, gui=self) + + + self.font = pygame.font.Font(None, 15) + + # GUI Initialization + self._gui_app = gui.App() + self._gui_table = MenuTable(config=self.config.menu, settings=self.config) + container = gui.Container(align=1, valign=-1) + container.add(self._gui_table, 0, 0) + self._gui_app.init(container) + + self.viewCenter = self.config.viewCenter + + self.key_map = { + Keys.K_w: 'up', + Keys.K_s: 'down', + Keys.K_a: 'left', + Keys.K_d: 'right', + Keys.K_n: 'pan_left', + Keys.K_m: 'pan_right', + Keys.K_b: 'switch_raycast_mode' # use this to toggle ray casts + } def setCenter(self, value): @@ -134,6 +207,9 @@ class GUI(Process): Updates the view offset based on the center of the screen. Tells the debug draw to update its values also. """ + if value is None: + value = [0, 0] + self._viewCenter = b2Vec2(*value) self._viewCenter *= self._viewZoom self._viewOffset = self._viewCenter - self.screenSize / 2 @@ -142,321 +218,192 @@ class GUI(Process): def setZoom(self, zoom): self._viewZoom = zoom - - viewZoom = property(lambda self: self._viewZoom, setZoom, - doc='Zoom factor for the display') - viewCenter = property(lambda self: self._viewCenter / self._viewZoom, setCenter, - doc='Screen center in camera coordinates') - viewOffset = property(lambda self: self._viewOffset, - doc='The offset of the top-left corner of the screen') + # TODO: write these properties out and rename them according to the python standard + viewZoom = property(lambda self: self._viewZoom, setZoom, doc='Zoom factor for the display') + viewCenter = property(lambda self: self._viewCenter / self._viewZoom, setCenter, doc='Screen center in camera coordinates') + viewOffset = property(lambda self: self._viewOffset, doc='The offset of the top-left corner of the screen') def _add(self): - if self.init: - self.config.drawMenu = True + self.simulation.box2d_world.renderer = self.renderer - self.config = sim.combine_dicts(self.config, self.simulation.box2d_simulation.config) - - #print('Initializing Pygame Framework...') - # Pygame Initialization - pygame.init() - caption = "Python Box2D Testbed - " + self.name - pygame.display.set_caption(caption) + if self.config.viewCenter is None: + x = self.simulation.visible_area[0][0] + (self.simulation.visible_area[0][1] - self.simulation.visible_area[0][0]) / 2 + y = self.simulation.visible_area[1][0] + (self.simulation.visible_area[1][1] - self.simulation.visible_area[1][0]) / 2 + self.viewCenter = [x, y] - # Screen and debug draw - flags = pygame.DOUBLEBUF - self.screen = pygame.display.set_mode((self.config.screen_width, self.config.screen_height), flags) - self.screenSize = b2Vec2(*self.screen.get_size()) + - self.renderer = PygameDraw(surface=self.screen, test=self) - self.simulation.box2d_world.renderer = self.renderer - self.mouseJoint = self.simulation.box2d_simulation.mouseJoint - - try: - self.font = pygame.font.Font(None, 15) - except IOError: - try: - self.font = pygame.font.Font("freesansbold.ttf", 15) - except IOError: - self.logger.error("Unable to load default font or 'freesansbold.ttf'") - self.logger.error("Disabling text drawing.") - self.Print = lambda *args: 0 - self.DrawStringAt = lambda *args: 0 - - if not self.config.gui_background: - # GUI Initialization - if self.config.drawMenu: - self.gui_app = gui.App() - self.gui_table = fwGUI(self.config) - container = gui.Container(align=1, valign=-1) - container.add(self.gui_table, 0, 0) - self.gui_app.init(container) + def get_color_for_body(self, body): - self.viewCenter = self.config.viewCenter - - self.key_map = { - Keys.K_w: 'up', - Keys.K_s: 'down', - Keys.K_a: 'left', - Keys.K_d: 'right', - Keys.K_n: 'pan_left', - Keys.K_m: 'pan_right', - Keys.K_b: 'switch_raycast_mode' # use this to toggle ray casts - } - - if self.config.drawMenu and not self.config.gui_background: - self.gui_table.updateGUI(self.config) + obj = body.userData['object'] + obj_class_name = obj.__class__.__name__ + + if obj_class_name in self.config.color_scheme: + return self.config.color_scheme[obj_class_name] + else: + return self.config.color_scheme.object def _step(self): - self.screen.fill((0, 0, 0)) + self.screen.fill(self.config.color_scheme.background) # Check keys that should be checked every loop (not only on initial keydown) - if self.config.move_world and not self.config.gui_background: - self.CheckKeys() + if self.config.move_world: + self._check_keys() # Reset the text line to start the text from the top - self.textLine = self.TEXTLINE_START + self._text_line = self.config.text_line_start - # Draw the name of the test running - if self.config.drawStats: - self.Print(self.name, (127, 127, 255)) - - if self.description: - # Draw the name of the test running - for s in self.description.split('\n'): - self.Print(s, (127, 255, 127)) - - if self.config.drawMenu and not self.config.gui_background: + if self.config.draw_menu: # Update the settings based on the GUI - self.gui_table.updateSettings(self.config) - - self.n_steps += 1 - renderer = self.renderer - - if self.config.pause: - if self.config.singleStep: - self.config.singleStep = False - if self.config.drawStats: - self.Print("****PAUSED****", (200, 0, 0)) - - self.simulation.box2d_simulation.config.pause = self.config.pause - self.simulation.box2d_simulation.config.singleStep = self.config.singleStep - self.simulation.box2d_simulation.config.enable_warm_starting = self.config.enable_warm_starting - self.simulation.box2d_simulation.config.warmStarting = self.config.enable_warm_starting - self.simulation.box2d_simulation.config.continuousPhysics = self.config.enable_continuous - self.simulation.box2d_simulation.config.subStepping = self.config.enable_sub_stepping - self.simulation.box2d_simulation.config.vel_iters = self.config.vel_iters - self.simulation.box2d_simulation.config.pos_iters = self.config.pos_iters - #self.simulation.box2d_simulation.config.hz = self.config.hz - - if renderer: - # convertVertices is only applicable when using b2DrawExtended. It - # indicates that the C code should transform box2d coords to screen - # coordinates. - is_extended = isinstance(renderer, b2DrawExtended) - renderer.flags = dict( - drawShapes=self.config.drawShapes, - drawJoints=self.config.drawJoints, - drawAABBs=self.config.drawAABBs, - drawPairs=self.config.drawPairs, - drawCOMs=self.config.drawCOMs, - convertVertices=is_extended, - ) - - # Update the debug draw settings so that the vertices will be properly - # converted to screen coordinates - t_draw = time.time() + self._gui_table.updateSettings(self.config) - if renderer is not None: - renderer.StartDraw() - - self.simulation.box2d_world.DrawDebugData() + # if self.config.pause: + # if self.config.singleStep: + # self.config.singleStep = False + # if self.config.draw_stats: + # self._print("****PAUSED****") - if renderer: - if self.config.drawShapes: - for body in self.simulation.box2d_world.bodies: - for fixture in body.fixtures: - if isinstance(fixture.shape, b2PolygonShape): - vertices = [body.transform * v for v in fixture.shape.vertices] - vertices = [(self.ConvertWorldtoScreen(v[0], v[1])) for v in vertices] - renderer.DrawSolidPolygon(vertices=vertices, color=self.colors[body.type]) - if isinstance(fixture.shape, b2CircleShape): - point = body.transform * fixture.shape.pos - if fixture.body.userData['name'] == 'mobile base': - axis = body.GetWorldVector((0, -1)) - else: - axis = body.GetWorldVector((0, 1)) - renderer.DrawSolidCircle(center=self.ConvertWorldtoScreen(point[0], point[1]), radius=fixture.shape.radius, color=self.colors[body.type], axis=axis) - - # Take care of additional drawing (fps, mouse joint, - # contact points) - # If there's a mouse joint, draw the connection between the object - # and the current pointer position. - if self.mouseJoint: - p1 = renderer.to_screen(self.mouseJoint.anchorB) - p2 = renderer.to_screen(self.mouseJoint.target) - - renderer.DrawPoint(p1, self.config.pointSize, - self.colors['mouse_point']) - renderer.DrawPoint(p2, self.config.pointSize, - self.colors['mouse_point']) - renderer.DrawSegment(p1, p2, self.colors['joint_line']) - - # Draw each of the contact points in different colors. - if self.config.drawContactPoints: - for point in self.simulation.box2d_simulation.points: - if point['state'] == b2_addState: - renderer.DrawPoint(renderer.to_screen(point['position']), - self.config.pointSize, - self.colors['contact_add']) - elif point['state'] == b2_persistState: - renderer.DrawPoint(renderer.to_screen(point['position']), - self.config.pointSize, - self.colors['contact_persist']) - - if self.config.drawContactNormals: - for point in self.simulation.box2d_simulation.points: - if point['state'] != b2_nullState: - p1 = renderer.to_screen(point['position']) - p2 = renderer.axisScale * point['normal'] + p1 - renderer.DrawSegment(p1, p2, self.colors['contact_normal']) - - self.DrawHumanInfos() - self.draw_human_chat() - - renderer.EndDraw() - t_draw = time.time() - t_draw - - t_draw = max(b2_epsilon, t_draw) - self.simulation.box2d_simulation.t_step = max(b2_epsilon, self.simulation.box2d_simulation.t_step) - - # TODO: do we need the t_draw and t_steps computation? - try: - self.t_draws.append(1.0 / t_draw) - except: - pass - else: - if len(self.t_draws) > 2: - self.t_draws.pop(0) + self.renderer.StartDraw() + self._draw_bodies() - try: - self.t_steps.append(1.0 / self.simulation.box2d_simulation.t_step) - except: - pass - else: - if len(self.t_steps) > 2: - self.t_steps.pop(0) - - if self.config.drawFPS and self.config.drawMenu: - self.Print("Combined FPS %d" % self.fps) - - if self.config.drawStats: - self.Print("bodies=%d contacts=%d joints=%d proxies=%d" % - (self.simulation._box2d_world.bodyCount, self.simulation._box2d_world.contactCount, - self.simulation._box2d_world.jointCount, self.simulation._box2d_world.proxyCount)) - - # self.Print("hz %d vel/pos iterations %d/%d" % - # (self.config.hz, self.config.vel_iters, - # self.config.pos_iters)) - - if self.t_draws and self.t_steps: - self.Print("Potential draw rate: %.2f fps Step rate: %.2f Hz" - "" % (sum(self.t_draws) / len(self.t_draws), - sum(self.t_steps) / len(self.t_steps)) - ) - self.using_contacts = True - for point in self.simulation.box2d_simulation.points: - if point['state'] != b2_nullState: - self.Print('Collision between %s and %s at point (%.3f,%.3f)' % ( - point['fixtureA'].body.userData['name'], - point['fixtureB'].body.userData['name'], - point['position'][0], - point['position'][1]), - color=(229, 255, 153, 255) - ) - - if self.config.drawMenu and not self.config.gui_background: - self.gui_table.updateGUI(self.config) - - if self.init: - self.init = False - self.config.drawMenu = self.drawMenu - - if not self.config.gui_background: - self.checkEvents() - if self.config.drawMenu: - self.gui_app.paint(self.screen) - - pygame.display.flip() + if self.config.draw_debug_information: + self._draw_debug_information() + + if self.config.draw_agent_information: + self._draw_agent_information() + + if self.config.draw_object_information: + self._draw_object_information() + + self._draw_popup_information() + + if self.config.draw_dialogue: + self._draw_human_chat() + + self.renderer.EndDraw() + + if self.config.draw_rtf: + self._print('Real Time Factor : {:.2f}'.format(self.simulation.real_time_factor)) + + if self.config.draw_stats: + self._print("bodies=%d contacts=%d joints=%d proxies=%d" % + (self.simulation.box2d_world.bodyCount, self.simulation.box2d_world.contactCount, + self.simulation.box2d_world.jointCount, self.simulation.box2d_world.proxyCount)) + + self.using_contacts = True + for point in self.simulation.box2d_simulation.points: + if point['state'] != b2_nullState: + self._print('Collision between %s and %s at point (%.3f,%.3f)' % ( + point['fixtureA'].body.userData['name'], + point['fixtureB'].body.userData['name'], + point['position'][0], + point['position'][1]) + ) + + if self.config.draw_menu: + self._gui_table.updateGUI(self.config) + + self._check_events() + if self.config.draw_menu: + self._gui_app.paint(self.screen) + + pygame.display.flip() def _close(self): pass - - def setup_keys(self): - keys = [s for s in dir(pygame.locals) if s.startswith('K_')] - for key in keys: - value = getattr(pygame.locals, key) - setattr(Keys, key, value) - - def Print(self, str, color=(229, 153, 153, 255), font=None, textLine=None, offsetLine=None): + def _print(self, text, color=None, font=None, text_line=None, offset_line=None): """ Draw some text at the top status lines and advance to the next line. """ + if color is None: + color = self.config.color_scheme.text + if not font: font = self.font - if not textLine: - textLine = self.textLine - self.textLine += 15 - if not offsetLine: - offsetLine = self.offsetLine - self.screen.blit(font.render( - str, True, color), (offsetLine, textLine)) + if not text_line: + text_line = self._text_line + self._text_line += 15 + if not offset_line: + offset_line = self.offsetLine + self.screen.blit(font.render(text, True, color), (offset_line, text_line)) - def DrawStringAt(self, x, y, str, color=(229, 153, 153, 255)): - """ - Draw some text, str, at screen coordinates (x, y). - """ - self.screen.blit(self.font.render(str, True, color), (x, y)) - + def _draw_bodies(self): + for body in self.simulation.box2d_world.bodies: + for fixture in body.fixtures: + if isinstance(fixture.shape, b2PolygonShape): + vertices = [body.transform * v for v in fixture.shape.vertices] + vertices = [(self.convert_world_to_screen(v[0], v[1])) for v in vertices] + self.renderer.DrawSolidPolygon( + vertices=vertices, + color=self.get_color_for_body(body) + ) + if isinstance(fixture.shape, b2CircleShape): + point = body.transform * fixture.shape.pos + axis = body.GetWorldVector((0, 1)) + self.renderer.DrawSolidCircle( + center=self.convert_world_to_screen(point[0], point[1]), + radius=fixture.shape.radius, + color=self.get_color_for_body(body), + axis=axis + ) + + + def _draw_agent_information(self): + for obj in self.simulation.objects.values(): + if mpi_sim.utils.object_filter(obj, filter=self.config.agent_information.filter): + text_lines = ['{} : {}'.format(obj.__class__.__name__, obj.id)] + self._draw_multi_line_text(text_lines, obj.position, self.config.agent_information) + + + def _draw_object_information(self): + for obj in self.simulation.objects.values(): + if not mpi_sim.utils.object_filter(obj, filter=self.config.agent_information.filter): + text_lines = ['{} : {}'.format(obj.__class__.__name__, obj.id)] + self._draw_multi_line_text(text_lines, obj.position, self.config.object_information) + + + def _draw_popup_information(self): + + # do we point to an object ? + obj = self._mouse_over_object + if not obj: + return - def DrawHumanInfos(self): - for idx, obj in self.simulation.objects.items(): - if isinstance(obj, sim.mpi_sim.objects.Human): - text = 'Human : {}'.format(idx) - if self.query_userData['name'] == idx and self.query_userData['obj'] == obj: - if time.time() - self.query_userData['time'] <= self.time_infos_displayed: - pos = obj.position - pos = self.ConvertWorldtoScreen(pos[0], pos[1]) - pygame.draw.rect(self.renderer.surface, self.colors['black'].bytes, pygame.Rect(pos[0] + self.offsetXRect - 5, pos[1] + self.offsetYRect*1.5, 125, 50), 0) - self.DrawStringAt(pos[0] + self.offsetXRect, pos[1] + self.offsetYRect*2, 'Emotion : {}'.format(obj.config.emotion), color=self.colors['info_text'].bytes) - self.DrawStringAt(pos[0] + self.offsetXRect, pos[1] + self.offsetYRect*3 , 'Gender : {}'.format(obj.config.gender), color=self.colors['info_text'].bytes) - self.DrawStringAt(pos[0] + self.offsetXRect, pos[1] + self.offsetYRect*4 , 'Age : {}'.format(obj.config.age), color=self.colors['info_text'].bytes) - self.DrawStringAt(pos[0] + self.offsetXRect, pos[1] + self.offsetYRect*5 , 'Wearing a mask : {}'.format(obj.config.wearing_mask), color=self.colors['info_text'].bytes) - pygame.draw.rect(self.renderer.surface, self.colors['info_window'].bytes, pygame.Rect(pos[0] + self.offsetXRect - 5, pos[1] + self.offsetYRect*1.5, 125, 50), 1) - elif isinstance(obj, sim.mpi_sim.objects.ARIRobot): - text = 'ARI Robot : {}'.format(idx) - else: - continue - pos = obj.position - pos = self.ConvertWorldtoScreen(pos[0], pos[1]) - text = self.font.render(text, True, self.colors['info_text'].bytes, self.colors['black'].bytes) - textRect = text.get_rect() - textRect.topleft = (pos[0] + self.offsetXRect, pos[1]) - self.screen.blit(text, textRect) - + # get the text that should be displayed, each list entry is a new line + text_lines = [] + if isinstance(obj, mpi_sim.objects.Human): + text_lines.append('{} : {}'.format(obj.__class__.__name__, obj.id)) + # text_lines.append('Speaking : Not Implemented') # TODO: add information if the human is speaking + text_lines.append('Emotion : {}'.format(obj.config.emotion)) + text_lines.append('Gender : {}'.format(obj.config.gender)) + text_lines.append('Age : {}'.format(obj.config.age)) + text_lines.append('Wearing a mask : {}'.format(obj.config.wearing_mask)) + elif isinstance(obj, mpi_sim.objects.ARIRobot): + text_lines.append('{} : {}'.format(obj.__class__.__name__, obj.id)) + # text_lines.append('Speaking : Not Implemented') # TODO: add information if ari is speaking + # text_lines.append('Goal : Not Implemented') # TODO: add information about aris navigation goal + else: + text_lines.append('{} : {}'.format(obj.__class__.__name__, obj.id)) + + self._draw_multi_line_text(text_lines, position=obj.position, config=self.config.popup_information) - def draw_human_chat(self): + + def _draw_human_chat(self): + + # TODO (refactor): use new string drawing methods + # TODO (feature): print dialog on bottom of simulation in an area that has a filled background current_simulation_step = self.simulation.step_counter # identify humans and if they said something - for obj in self.simulation.get_objects_by_type([sim.objects.Human, sim.objects.ARIRobot]): + for obj in self.simulation.get_objects_by_type([mpi_sim.objects.Human, mpi_sim.objects.ARIRobot]): if 'speech' in obj.components: speech_comp = obj.speech @@ -466,9 +413,9 @@ class GUI(Process): if len(speech_comp.history) > 0 and speech_comp.history[-1].start_step == current_simulation_step: # identify the type of entity that said something - if isinstance(obj, sim.objects.Human): + if isinstance(obj, mpi_sim.objects.Human): type_str = 'Human' - elif isinstance(obj, sim.objects.ARIRobot): + elif isinstance(obj, mpi_sim.objects.ARIRobot): type_str = 'Robot' else: type_str = 'Unknown' @@ -484,7 +431,7 @@ class GUI(Process): pygame.font.Font("freesansbold.ttf", 15).render( '{} {}: {}'.format(type_str, h_id, content), True, - self.colors['info_text'].bytes + self.config.color_scheme.text ), (self.offsetLine, self.screenSize[1] - delta) ) @@ -492,139 +439,171 @@ class GUI(Process): # title self.screen.blit( - pygame.font.Font("freesansbold.ttf", 20).render( - 'Dialogue', - True, - self.colors['info_text'].bytes - ), + pygame.font.Font("freesansbold.ttf", 20).render('Dialogue', True, self.config.color_scheme.text), (self.offsetLine, self.screenSize[1] - (15 * self.human_speech_history_length + 30)) ) - - def ConvertWorldtoScreen(self, x, y): + + def _draw_debug_information(self): + # convertVertices is only applicable when using b2DrawExtended. It + # indicates that the C code should transform box2d coords to screen + # coordinates. + self.renderer.flags = dict( + drawShapes=False, + drawJoints=True, + drawAABBs=True, + drawPairs=True, + drawCOMs=True, + convertVertices=isinstance(self.renderer, b2DrawExtended), + ) + self.simulation.box2d_world.DrawDebugData() + + # Draw each of the contact points in different colors. + for point in self.simulation.box2d_simulation.points: + if point['state'] == b2_addState: + self.renderer.DrawPoint( + self.renderer.to_screen(point['position']), + self.config.point_size, + self.config.color_scheme.contact_add + ) + elif point['state'] == b2_persistState: + self.renderer.DrawPoint( + self.renderer.to_screen(point['position']), + self.config.point_size, + self.config.color_scheme.contact_persist + ) + + for point in self.simulation.box2d_simulation.points: + if point['state'] != b2_nullState: + p1 = self.renderer.to_screen(point['position']) + p2 = self.renderer.axisScale * point['normal'] + p1 + self.renderer.DrawSegment(p1, p2, self.config.color_scheme.contact_normal) + + + def _draw_multi_line_text(self, text_lines, position, config): + r"""Draws several (or a single) line of text.""" + # TODO (refactor): this function should be part of the renderer + + if text_lines is None or len(text_lines) == 0: + return + + default_config = mpi_sim.AttrDict( + x_offset = 0, # y offset to the position + y_offset = 0, # y offset to the position + x_padding = 0, + y_padding = 5, + text_color = 'text', + border_color = None, + background_color = None, + ) + config = mpi_sim.combine_dicts(config, default_config) + + # identify the size of the pop up window + popup_height = config.y_padding + popup_width = 0 + + rendered_text_lines = [] + for text_line in text_lines: + rendered_text = self.font.render(text_line, True, self.config.color_scheme[config.text_color]) + rendered_text_lines.append(rendered_text) + popup_width = max(popup_width, rendered_text.get_width() + 2 * config.x_padding) + popup_height += rendered_text.get_height() + config.y_padding + + # draw the popup area + pos = self.convert_world_to_screen(position[0], position[1]) + rect = pygame.Rect(pos[0] + config.x_offset, pos[1] + config.y_offset, popup_width, popup_height) + if config.background_color: + pygame.draw.rect(self.renderer.surface, self.config.color_scheme[config.background_color], rect, 0) # filling + if config.border_color: + pygame.draw.rect(self.renderer.surface, self.config.color_scheme[config.border_color], rect, 1) # border + + # draw the text + x = rect[0] + config.x_padding + y = rect[1] + config.y_padding + for rendered_text_line in rendered_text_lines: + self.screen.blit(rendered_text_line, (x, y)) + y += rendered_text_line.get_height() + config.y_padding + + + def convert_world_to_screen(self, x, y): return b2Vec2(x * self.viewZoom - self.viewOffset.x, self.screenSize.y - y * self.viewZoom + self.viewOffset.y) - def ConvertScreenToWorld(self, x, y): + def convert_screen_to_world(self, x, y): return b2Vec2((x + self.viewOffset.x) / self.viewZoom, ((self.screenSize.y - y + self.viewOffset.y) / self.viewZoom)) - def Keyboard(self, key): - key_map = self.key_map - if key in key_map: - self.simulation.box2d_simulation.pressed_keys.add(key_map[key]) - # if 'switch_raycast_mode' in self.simulation.box2d_simulation.pressed_keys: - # for ari_robot in self.simulation._permanent_objects.values(): - # if isinstance(ari_robot, sim.mpi_sim.objects.ARIRobot): - # if ari_robot.config.enable_raycast: - # for raycast in ari_robot.raycasts.values(): - # raycast.Keyboard(key) - - - def KeyboardUp(self, key): - key_map = self.key_map - if self.simulation.box2d_simulation.pressed_keys: - if key in key_map: - self.simulation.box2d_simulation.pressed_keys.remove(key_map[key]) + # maybe used later for key control of agents + # def Keyboard(self, key): + # key_map = self.key_map + # if key in key_map: + # self.simulation.box2d_simulation.pressed_keys.add(key_map[key]) + # def KeyboardUp(self, key): + # key_map = self.key_map + # if self.simulation.box2d_simulation.pressed_keys: + # if key in key_map: + # self.simulation.box2d_simulation.pressed_keys.remove(key_map[key]) - # TODO: remove this as it seems not to be used for our simulation - def handle_contact(self, contact, began): - # A contact happened -- see if a wheel hit a - # ground area - fixture_a = contact.fixtureA - fixture_b = contact.fixtureB - - body_a, body_b = fixture_a.body, fixture_b.body - ud_a, ud_b = body_a.userData, body_b.userData - if not ud_a or not ud_b: - return - - tire = None - ground_area = None - for ud in (ud_a, ud_b): - if 'obj' in ud.keys(): - obj = ud['obj'] - if isinstance(obj, TDTire): - tire = obj - elif isinstance(obj, sim.objects.GroundArea): - ground_area = obj - - if ground_area and tire: - if began: - tire.add_ground_area(ground_area) - else: - tire.remove_ground_area(ground_area) - if self.config.print_collision: - self.print_contact(contact) - - - def print_contact(self, contact): - if contact.worldManifold.points != ((0, 0), (0, 0)): - print("Collision between %s and %s at point (%.3f,%.3f)" % (contact.fixtureA.body.userData['name'], - contact.fixtureB.body.userData['name'], - contact.worldManifold.points[0][0], - contact.worldManifold.points[0][1])) - - def BeginContact(self, contact): - self.handle_contact(contact, True) - - - def EndContact(self, contact): - self.handle_contact(contact, False) - + # def _print_contact(self, contact): + # if contact.worldManifold.points != ((0, 0), (0, 0)): + # print("Collision between %s and %s at point (%.3f,%.3f)" % (contact.fixtureA.body.userData['name'], + # contact.fixtureB.body.userData['name'], + # contact.worldManifold.points[0][0], + # contact.worldManifold.points[0][1])) - def checkEvents(self): + def _check_events(self): """ Check for pygame events (mainly keyboard/mouse events). Passes the events onto the GUI also. """ for event in pygame.event.get(): - if event.type == QUIT or (event.type == KEYDOWN and event.key == Keys.K_ESCAPE): + if event.type == QUIT or (event.type == KEYDOWN and event.key == Keys.K_ESCAPE): + self.simulation.close() return False elif event.type == KEYDOWN: - self._Keyboard_Event(event.key, down=True) + self._keyboard_event(event.key, down=True) elif event.type == KEYUP: - self._Keyboard_Event(event.key, down=False) + self._keyboard_event(event.key, down=False) elif event.type == MOUSEBUTTONDOWN and self.config.move_world: - p = self.ConvertScreenToWorld(*event.pos) + p = self.convert_screen_to_world(*event.pos) if event.button == 1: # left mods = pygame.key.get_mods() if mods & KMOD_LSHIFT: - self.ShiftMouseDown(p) + self._shift_mouse_down(p) else: - self.MouseDown(p) + self._mouse_down(p) elif event.button == 2: # middle pass elif event.button == 3: # right - self.rMouseDown = True + self.r_mouse_down = True elif event.button == 4: self.viewZoom *= 1.1 elif event.button == 5: self.viewZoom /= 1.1 elif event.type == MOUSEBUTTONUP and self.config.move_world: - p = self.ConvertScreenToWorld(*event.pos) + p = self.convert_screen_to_world(*event.pos) if event.button == 3: # right - self.rMouseDown = False + self.r_mouse_down = False else: + # pass self.MouseUp(p) elif event.type == MOUSEMOTION: - p = self.ConvertScreenToWorld(*event.pos) - self.MouseMove(p) + p = self.convert_screen_to_world(*event.pos) + self._mouse_move(p) - if self.rMouseDown: - self.viewCenter -= (event.rel[0] / - 5.0, -event.rel[1] / 5.0) + if self.r_mouse_down: + self.viewCenter -= (event.rel[0] / 5.0, -event.rel[1] / 5.0) - if self.config.drawMenu and not self.config.gui_background: - self.gui_app.event(event) # Pass the event to the GUI + if self.config.draw_menu: + self._gui_app.event(event) # Pass the event to the GUI return True - def _Keyboard_Event(self, key, down=True): + def _keyboard_event(self, key, down=True): """ Internal keyboard event, don't override this. Checks for the initial keydown of the basic testbed keys. Passes the unused @@ -632,22 +611,22 @@ class GUI(Process): """ if down: if key == Keys.K_z and self.config.move_world: # Zoom in - self.viewZoom = min(1.1 * self.viewZoom, 50.0) + self.viewZoom = min(1.1 * self.viewZoom, 200.0) elif key == Keys.K_x and self.config.move_world: # Zoom out self.viewZoom = max(0.9 * self.viewZoom, 0.02) elif key == Keys.K_F1: # Toggle drawing the menu - self.config.drawMenu = not self.config.drawMenu + self.config.draw_menu = not self.config.draw_menu elif key == Keys.K_F2: # Do a single step self.config.singleStep = True - if self.config.drawMenu: - self.gui_table.updateGUI(self.config) - elif self.config.key_control: # Inform the test of the key press - self.Keyboard(key) - elif self.config.key_control: - self.KeyboardUp(key) + if self.config.draw_menu: + self._gui_table.updateGUI(self.config) + # elif self.config.key_control: # maybe used for key control + # self.Keyboard(key) + # elif self.config.key_control: + # self.KeyboardUp(key) - def CheckKeys(self): + def _check_keys(self): """ Check the keys that are evaluated on every main loop iteration. I.e., they aren't just evaluated when first pressed down @@ -670,66 +649,284 @@ class GUI(Process): self.viewCenter = (0.0, 20.0) - def MouseMove(self, p): + def _mouse_move(self, p): """ Mouse moved to point p, in world coordinates. """ - self.mouseWorld = p - if self.mouseJoint: - self.mouseJoint.target = p - + self._mouse_world = p + # Make a small box. aabb = b2AABB(lowerBound=p - (0.01, 0.01), upperBound=p + (0.01, 0.01)) # Query the world for overlapping shapes. - query = fwQueryCallback(p) + query = MouseOverQueryCallback(p) self.simulation.box2d_world.QueryAABB(query, aabb) - if query.fixture: - self.query_userData = query.fixture.body.userData - self.query_userData['time'] = time.time() + self._mouse_over_object = query.fixture.body.userData['object'] + #self.query_userData['time'] = time.time() + else: + self._mouse_over_object = None + #self.query_userData['time'] = None - def ShiftMouseDown(self, p): + def _shift_mouse_down(self, p): """ Indicates that there was a left click at point p (world coordinates) with the left shift key being held down. """ - self.mouseWorld = p + self._mouse_world = p - def MouseDown(self, p): + def _mouse_down(self, p): """ Indicates that there was a left click at point p (world coordinates) """ - if self.mouseJoint is not None: + pass + # # Create a mouse joint on the selected body (assuming it's dynamic) + # # Make a small box. + # aabb = b2AABB(lowerBound=p - (0.001, 0.001), + # upperBound=p + (0.001, 0.001)) + # + # # Query the world for overlapping shapes. + # query = fwQueryCallback(p) + # self.simulation.box2d_world.QueryAABB(query, aabb) + # if query.fixture: + # body = query.fixture.body + # body.awake = True + + +class PygameDraw(b2DrawExtended): + """ + This draw class is used for the regular drawing of shapes and the debug drawing mode of Box2D. + For the debug drawing mode it accepts callbacks from Box2D (which specifies what to draw) and handles all the rendering. + """ + surface = None + axisScale = 10.0 + + def __init__(self, gui=None, **kwargs): + b2DrawExtended.__init__(self, **kwargs) + self.flipX = False + self.flipY = True + self.convertVertices = True + self.test = gui + + + def StartDraw(self): + self.zoom = self.test.viewZoom + self.center = self.test.viewCenter + self.offset = self.test.viewOffset + self.screenSize = self.test.screenSize + + + def EndDraw(self): + pass + + + def DrawPoint(self, p, size, color): + """ + Draw a single point at point p given a pixel size and color. + """ + self.DrawCircle(p, size / self.zoom, color, drawwidth=0) + + + def DrawAABB(self, aabb, color): + """ + Draw a wireframe around the AABB with the given color. + """ + points = [(aabb.lowerBound.x, aabb.lowerBound.y), + (aabb.upperBound.x, aabb.lowerBound.y), + (aabb.upperBound.x, aabb.upperBound.y), + (aabb.lowerBound.x, aabb.upperBound.y)] + + pygame.draw.aalines(self.surface, color, True, points) + + + def DrawSegment(self, p1, p2, color): + """ + Draw the line segment from p1-p2 with the specified color. + """ + pygame.draw.aaline(self.surface, color, p1, p2) + + + def DrawTransform(self, xf): + """ + Draw the transform xf on the screen + """ + p1 = xf.position + p2 = self.to_screen(p1 + self.axisScale * xf.R.x_axis) + p3 = self.to_screen(p1 + self.axisScale * xf.R.y_axis) + p1 = self.to_screen(p1) + pygame.draw.aaline(self.surface, (255, 0, 0), p1, p2) + pygame.draw.aaline(self.surface, (0, 255, 0), p1, p3) + + + def DrawCircle(self, center, radius, color, drawwidth=1): + """ + Draw a wireframe circle given the center, radius, axis of orientation + and color. + """ + radius *= self.zoom + if radius < 1: + radius = 1 + else: + radius = int(radius) + + pygame.draw.circle(self.surface, color, center, radius, drawwidth) + + + def DrawSolidCircle(self, center, radius, axis, color): + """ + Draw a solid circle given the center, radius, axis of orientation and + color. + """ + + if isinstance(color, b2Color): + color = Color(*color.bytes) + + radius *= self.zoom + if radius < 1: + radius = 1 + else: + radius = int(radius) + + pygame.draw.circle(self.surface, color.correct_gamma(0.5), center, radius, 0) # filling + pygame.draw.circle(self.surface, color, center, radius, 1) # border + # pygame.draw.aaline(self.surface, (255, 0, 0), center, + # (center[0] - radius * axis[0], + # center[1] + radius * axis[1])) + + + def DrawPolygon(self, vertices, color): + """ + Draw a wireframe polygon given the screen vertices with the specified color. + """ + if not vertices: return - # Create a mouse joint on the selected body (assuming it's dynamic) - # Make a small box. - aabb = b2AABB(lowerBound=p - (0.001, 0.001), - upperBound=p + (0.001, 0.001)) + if len(vertices) == 2: + pygame.draw.aaline(self.surface, color, vertices[0], vertices) + else: + pygame.draw.polygon(self.surface, color, vertices, 1) - # Query the world for overlapping shapes. - query = fwQueryCallback(p) - self.simulation.box2d_world.QueryAABB(query, aabb) - if query.fixture: - body = query.fixture.body - # A body was selected, create the mouse joint - self.mouseJoint = self.simulation._box2d_world.CreateMouseJoint( - bodyA=self.simulation.box2d_simulation.groundbody, - bodyB=body, - target=p, - maxForce=1000.0 * body.mass) - body.awake = True + def DrawSolidPolygon(self, vertices, color): + """ + Draw a filled polygon given the screen vertices with the specified color. + """ + if not vertices: + return + + if isinstance(color, b2Color): + color = Color(*color.bytes) + + if len(vertices) == 2: + pygame.draw.aaline(self.surface, color, vertices[0], vertices[1]) + else: + pygame.draw.polygon(self.surface, color.correct_gamma(0.5), vertices, 0) + pygame.draw.polygon(self.surface, color, vertices, 1) + + +class MenuTable(gui.Table): + """ + Deals with the initialization and changing the settings based on the GUI + controls. Callbacks are not used, but the checkboxes and sliders are polled + by the main loop. + """ + + # TODO (Feature): fill the background behind the table for better visability + form = None - def MouseUp(self, p): + def __init__(self, config, settings, **params): + # The framework GUI is just basically a HTML-like table + # There are 2 columns right-aligned on the screen + gui.Table.__init__(self, **params) + self.form = gui.Form() + + self.config = config + + # fg = (255, 255, 255) + + # "Toggle menu" + self.tr() + self.td(gui.Label('F1: Toggle Menu', color=settings.color_scheme.text), align=1, colspan=2) + + for slider in self.config.sliders: + # "Slider title" + self.tr() + self.td(gui.Label(slider['text'], color=settings.color_scheme.text), align=1, colspan=2) + + # Create the slider + self.tr() + e = gui.HSlider(getattr(settings, slider['name']), slider['min'], slider['max'], size=20, width=100, height=16, name=slider['name']) + self.td(e, colspan=2, align=1) + + # Add each of the checkboxes. + for text, variable in self.config.checkboxes: + self.tr() + if variable is None: + # Checkboxes that have no variable (i.e., None) are just labels. + self.td(gui.Label(text, color=settings.color_scheme.text), align=1, colspan=2) + else: + # Add the label and then the switch/checkbox + self.td(gui.Label(text, color=settings.color_scheme.text), align=1) + self.td(gui.Switch(value=getattr(settings, variable), name=variable)) + + + def updateGUI(self, settings): """ - Left mouse button up. + Change all of the GUI elements based on the current settings """ - if self.mouseJoint: - self.simulation._box2d_world.DestroyJoint(self.mouseJoint) - self.mouseJoint = None + for text, variable in self.config.checkboxes: + if not variable: + continue + if hasattr(settings, variable): + self.form[variable].value = getattr(settings, variable) + + # Now do the sliders + for slider in self.config.sliders: + name = slider['name'] + self.form[name].value = getattr(settings, name) + + + def updateSettings(self, settings): + """ + Change all of the settings based on the current state of the GUI. + """ + for text, variable in self.config.checkboxes: + if variable: + setattr(settings, variable, self.form[variable].value) + + # Now do the sliders + for slider in self.config.sliders: + name = slider['name'] + setattr(settings, name, int(self.form[name].value)) + + # # If we're in single-step mode, update the GUI to reflect that. + # if settings.singleStep: + # settings.pause = True + # self.form['pause'].value = True + # self.form['singleStep'].value = False + + +class Keys(object): + pass + + +class MouseOverQueryCallback(b2QueryCallback): + + def __init__(self, p): + super(MouseOverQueryCallback, self).__init__() + self.point = p + self.fixture = None + + + def ReportFixture(self, fixture): + inside = fixture.TestPoint(self.point) + if inside: + self.fixture = fixture + # We found the object, so stop the query + return False + # Continue the query + return True diff --git a/mpi_sim/utils/box2d_utils.py b/mpi_sim/utils/box2d_utils.py index acbfb2527207959f3b304c2c48c771741c43a898..6489e5225abde5a9e109c1dc2090c32b4606c3eb 100644 --- a/mpi_sim/utils/box2d_utils.py +++ b/mpi_sim/utils/box2d_utils.py @@ -1,268 +1,8 @@ -from Box2D import (b2DestructionListener, b2QueryCallback, b2Fixture, b2Joint, b2Vec2, b2_dynamicBody, b2DrawExtended) -import pygame -try: - from Box2D.examples.pgu import gui -except: - raise ImportError('Unable to load PGU') +from Box2D import b2Vec2 import numpy as np -class fwDestructionListener(b2DestructionListener): - """ - The destruction listener callback: - "SayGoodbye" is called when a joint or shape is deleted. - """ - - def __init__(self, test, **kwargs): - super(fwDestructionListener, self).__init__(**kwargs) - self.test = test - - def SayGoodbye(self, obj): - if isinstance(obj, b2Joint): - if self.test.mouseJoint == obj: - self.test.mouseJoint = None - else: - self.test.JointDestroyed(obj) - elif isinstance(obj, b2Fixture): - self.test.FixtureDestroyed(obj) - - -class Keys(object): - pass - - -class fwQueryCallback(b2QueryCallback): - - def __init__(self, p): - super(fwQueryCallback, self).__init__() - self.point = p - self.fixture = None - - def ReportFixture(self, fixture): - body = fixture.body - if body.type == b2_dynamicBody: - inside = fixture.TestPoint(self.point) - if inside: - self.fixture = fixture - # We found the object, so stop the query - return False - # Continue the query - return True - - -class PygameDraw(b2DrawExtended): - """ - This debug draw class accepts callbacks from Box2D (which specifies what to - draw) and handles all of the rendering. - - If you are writing your own game, you likely will not want to use debug - drawing. Debug drawing, as its name implies, is for debugging. - """ - surface = None - axisScale = 10.0 - - def __init__(self, test=None, **kwargs): - b2DrawExtended.__init__(self, **kwargs) - self.flipX = False - self.flipY = True - self.convertVertices = True - self.test = test - - def StartDraw(self): - self.zoom = self.test.viewZoom - self.center = self.test.viewCenter - self.offset = self.test.viewOffset - self.screenSize = self.test.screenSize - - def EndDraw(self): - pass - - def DrawPoint(self, p, size, color): - """ - Draw a single point at point p given a pixel size and color. - """ - self.DrawCircle(p, size / self.zoom, color, drawwidth=0) - - def DrawAABB(self, aabb, color): - """ - Draw a wireframe around the AABB with the given color. - """ - points = [(aabb.lowerBound.x, aabb.lowerBound.y), - (aabb.upperBound.x, aabb.lowerBound.y), - (aabb.upperBound.x, aabb.upperBound.y), - (aabb.lowerBound.x, aabb.upperBound.y)] - - pygame.draw.aalines(self.surface, color, True, points) - - def DrawSegment(self, p1, p2, color): - """ - Draw the line segment from p1-p2 with the specified color. - """ - pygame.draw.aaline(self.surface, color.bytes, p1, p2) - - def DrawTransform(self, xf): - """ - Draw the transform xf on the screen - """ - p1 = xf.position - p2 = self.to_screen(p1 + self.axisScale * xf.R.x_axis) - p3 = self.to_screen(p1 + self.axisScale * xf.R.y_axis) - p1 = self.to_screen(p1) - pygame.draw.aaline(self.surface, (255, 0, 0), p1, p2) - pygame.draw.aaline(self.surface, (0, 255, 0), p1, p3) - - def DrawCircle(self, center, radius, color, drawwidth=1): - """ - Draw a wireframe circle given the center, radius, axis of orientation - and color. - """ - radius *= self.zoom - if radius < 1: - radius = 1 - else: - radius = int(radius) - - pygame.draw.circle(self.surface, color.bytes, - center, radius, drawwidth) - - def DrawSolidCircle(self, center, radius, axis, color): - """ - Draw a solid circle given the center, radius, axis of orientation and - color. - """ - radius *= self.zoom - if radius < 1: - radius = 1 - else: - radius = int(radius) - - pygame.draw.circle(self.surface, (color / 2).bytes + [127], - center, radius, 0) - pygame.draw.circle(self.surface, color.bytes, center, radius, 1) - # pygame.draw.aaline(self.surface, (255, 0, 0), center, - # (center[0] - radius * axis[0], - # center[1] + radius * axis[1])) - - def DrawPolygon(self, vertices, color): - """ - Draw a wireframe polygon given the screen vertices with the specified color. - """ - if not vertices: - return - - if len(vertices) == 2: - pygame.draw.aaline(self.surface, color.bytes, - vertices[0], vertices) - else: - pygame.draw.polygon(self.surface, color.bytes, vertices, 1) - - def DrawSolidPolygon(self, vertices, color): - """ - Draw a filled polygon given the screen vertices with the specified color. - """ - if not vertices: - return - - if len(vertices) == 2: - pygame.draw.aaline(self.surface, color.bytes, - vertices[0], vertices[1]) - else: - pygame.draw.polygon( - self.surface, (color / 2).bytes + [127], vertices, 0) - pygame.draw.polygon(self.surface, color.bytes, vertices, 1) - - # the to_screen conversions are done in C with b2DrawExtended, leading to - # an increase in fps. - # You can also use the base b2Draw and implement these yourself, as the - # b2DrawExtended is implemented: - # def to_screen(self, point): - # """ - # Convert from world to screen coordinates. - # In the class instance, we store a zoom factor, an offset indicating where - # the view extents start at, and the screen size (in pixels). - # """ - # x=(point.x * self.zoom)-self.offset.x - # if self.flipX: - # x = self.screenSize.x - x - # y=(point.y * self.zoom)-self.offset.y - # if self.flipY: - # y = self.screenSize.y-y - # return (x, y) - - -class fwGUI(gui.Table): - """ - Deals with the initialization and changing the settings based on the GUI - controls. Callbacks are not used, but the checkboxes and sliders are polled - by the main loop. - """ - form = None - - def __init__(self,settings, **params): - # The framework GUI is just basically a HTML-like table - # There are 2 columns right-aligned on the screen - gui.Table.__init__(self,**params) - self.form=gui.Form() - - fg = (255,255,255) - - # "Toggle menu" - self.tr() - self.td(gui.Label("F1: Toggle Menu",color=(255,0,0)),align=1,colspan=2) - - for slider in settings.sliders: - # "Slider title" - self.tr() - self.td(gui.Label(slider['text'],color=fg),align=1,colspan=2) - - # Create the slider - self.tr() - e = gui.HSlider(getattr(settings, slider['name']),slider['min'],slider['max'],size=20,width=100,height=16,name=slider['name']) - self.td(e,colspan=2,align=1) - - # Add each of the checkboxes. - for text, variable in settings.checkboxes: - self.tr() - if variable == None: - # Checkboxes that have no variable (i.e., None) are just labels. - self.td(gui.Label(text, color=fg), align=1, colspan=2) - else: - # Add the label and then the switch/checkbox - self.td(gui.Label(text, color=fg), align=1) - self.td(gui.Switch(value=getattr(settings, variable),name=variable)) - - def updateGUI(self, settings): - """ - Change all of the GUI elements based on the current settings - """ - for text, variable in settings.checkboxes: - if not variable: continue - if hasattr(settings, variable): - self.form[variable].value = getattr(settings, variable) - - # Now do the sliders - for slider in settings.sliders: - name=slider['name'] - self.form[name].value=getattr(settings, name) - - def updateSettings(self, settings): - """ - Change all of the settings based on the current state of the GUI. - """ - for text, variable in settings.checkboxes: - if variable: - setattr(settings, variable, self.form[variable].value) - - # Now do the sliders - for slider in settings.sliders: - name=slider['name'] - setattr(settings, name, int(self.form[name].value)) - - # If we're in single-step mode, update the GUI to reflect that. - if settings.singleStep: - settings.pause=True - self.form['pause'].value = True - self.form['singleStep'].value = False +# TODO: identify which of these functions can be removed, as they are not used def update_turn(keys, desired_ang_speed, max_speed): @@ -381,8 +121,6 @@ def closest_node(node, nodes): return np.argmin(dist_2) - - def calc_linear_velocity_from_forward_velocity(desired_forward_velocity, box2d_body): # find the current speed in the forward direction current_forward_normal = box2d_body.GetWorldVector((0, 1)) diff --git a/mpi_sim/utils/misc.py b/mpi_sim/utils/misc.py index 369bdbbbf3d13379b6bf53f2cad83d40b220d094..42cd558604342853930c24589de63d3d5390a438 100644 --- a/mpi_sim/utils/misc.py +++ b/mpi_sim/utils/misc.py @@ -341,55 +341,63 @@ def as_list(x): return x -def object_filter(obj, objects=None, ids=None, types=None, properties=None): +def object_filter(obj, filter=None, **kwargs): + + default_filter = mpi_sim.AttrDict( + objects = [], + ids = [], + types = [], + properties = [], + ) + filter = mpi_sim.combine_dicts(kwargs, filter, default_filter) # if no filters are given, then object is ok - if not objects and not ids and not types and not properties: + if not filter.objects and not filter.ids and not filter.types and not filter.properties: return True # make sure, that all inputs are lists - if objects is None: - objects = [] - elif isinstance(objects, mpi_sim.Object): - objects = [objects] - elif not isinstance(objects, list): + if filter.objects is None: + filter.objects = [] + elif isinstance(filter.objects, mpi_sim.Object): + filter.objects = [filter.objects] + elif not isinstance(filter.objects, list): raise ValueError('Argument objects must be either None, Object, or a list of Objects!') - if ids is None: - ids = [] - elif isinstance(ids, int): - ids = [ids] - elif not isinstance(ids, list): + if filter.ids is None: + filter.ids = [] + elif isinstance(filter.ids, int): + filter.ids = [filter.ids] + elif not isinstance(filter.ids, list): raise ValueError('Argument ids must be either None, int, or a list of ints!') - if types is None: - types = [] - elif inspect.isclass(types): - types = [types] - elif not isinstance(types, list): + if filter.types is None: + filter.types = [] + elif inspect.isclass(filter.types): + filter.types = [filter.types] + elif not isinstance(filter.types, list): raise ValueError('Argument types must be either None, class, or a list of classes!') - if properties is None: - properties = [] - elif isinstance(properties, str): - properties = [properties] - elif not isinstance(properties, list): + if filter.properties is None: + filter.properties = [] + elif isinstance(filter.properties, str): + filter.properties = [filter.properties] + elif not isinstance(filter.properties, list): raise ValueError('Argument properties must be either None, string, or a list of strings!') # check if condition holds - if objects and obj in objects: + if filter.objects and obj in filter.objects: return True - if ids and obj.id in ids: + if filter.ids and obj.id in filter.ids: return True - if types and isinstance(obj, tuple(types)): + if filter.types and isinstance(obj, tuple(filter.types)): return True - if properties: - for prop in properties: + if filter.properties: + for prop in filter.properties: if prop in obj.properties: return True diff --git a/readme.md b/readme.md index 3b5cda3a7a2b6c38a155ef5d7bee1891a495c137..3da25174263306424cfca670c57d24168e2ec8fe 100644 --- a/readme.md +++ b/readme.md @@ -1,6 +1,6 @@ # Multi-Agent Interaction Simulator -Version 0.0.8 (15.12.2022) +Version 0.0.9 (17.12.2022) Simulator for multi-agent interaction scenarios in a 2D environment. diff --git a/setup.cfg b/setup.cfg index 44d0d6b1ba52f23cabe36fe33db53c93b6da7fcf..43eb279cdee14e0da3d6250dbc5b9bd5483a14d4 100644 --- a/setup.cfg +++ b/setup.cfg @@ -1,6 +1,6 @@ [metadata] name = mpi_sim -version = 0.0.8 +version = 0.0.9 [options] packages = find: