Merge pull request #76 from ci-group/examples_fix

Examples fix

Author: portaloffreedom

experiments/examples/manager.py

@@ -1,73 +1,62 @@
 #!/usr/bin/env python3
+"""
+This script loads a robot.yaml file and inserts it into the simulator.
+"""
+
 import os
 import sys
 import asyncio
-
-# Add `..` folder in search path
-current_dir = os.path.dirname(os.path.abspath(__file__))
-newpath = os.path.join(current_dir, '..', '..')
-sys.path.append(newpath)
-
-from pygazebo.pygazebo import DisconnectError
-
-from pyrevolve import revolve_bot
-from pyrevolve import parser
 from pyrevolve.SDF.math import Vector3
+from pyrevolve import revolve_bot, parser
 from pyrevolve.tol.manage import World
+from pyrevolve.util.supervisor.supervisor_multi import DynamicSimSupervisor
+from pyrevolve.evolution import fitness
 
 
 async def run():
     """
     The main coroutine, which is started below.
     """
+    robot_file_path = "experiments/examples/yaml/spider.yaml"
+
     # Parse command line / file input arguments
     settings = parser.parse_args()
 
+    # Start Simulator
+    if settings.simulator_cmd != 'debug':
+        simulator_supervisor = DynamicSimSupervisor(
+            world_file=settings.world,
+            simulator_cmd=settings.simulator_cmd,
+            simulator_args=["--verbose"],
+            plugins_dir_path=os.path.join('.', 'build', 'lib'),
+            models_dir_path=os.path.join('.', 'models'),
+            simulator_name='gazebo'
+        )
+        await simulator_supervisor.launch_simulator(port=settings.port_start)
+        await asyncio.sleep(0.1)
+
     # Load a robot from yaml
     robot = revolve_bot.RevolveBot()
-    robot.load_file("experiments/examples/yaml/spider.yaml")
+    robot.load_file(robot_file_path)
     robot.update_substrate()
     # robot._brain = BrainRLPowerSplines()
 
     # Connect to the simulator and pause
-    world = await World.create(settings)
-    await world.pause(True)
+    connection = await World.create(settings, world_address=('127.0.0.1', settings.port_start))
+    await asyncio.sleep(1)
 
-    await world.delete_model(robot.id)
-    await asyncio.sleep(2.5)
+    # Starts the simulation
+    await connection.pause(False)
 
     # Insert the robot in the simulator
-    robot_manager = await world.insert_robot(robot, Vector3(0, 0, 0.25))
-
-    # Resume simulation
-    await world.pause(False)
+    robot_manager = await connection.insert_robot(robot, Vector3(0, 0, settings.z_start))
 
     # Start a run loop to do some stuff
     while True:
         # Print robot fitness every second
-        print("Robot fitness is {fitness}".format(
-                fitness=robot_manager.fitness()))
+        status = 'dead' if robot_manager.dead else 'alive'
+        print(f"Robot fitness ({status}) is \n"
+              f" OLD:     {fitness.online_old_revolve(robot_manager)}\n"
+              f" DISPLAC: {fitness.displacement(robot_manager, robot)}\n"
+              f" DIS_VEL: {fitness.displacement_velocity(robot_manager, robot)}")
         await asyncio.sleep(1.0)
-
-
-def main():
-    def handler(loop, context):
-        exc = context['exception']
-        if isinstance(exc, DisconnectError) \
-                or isinstance(exc, ConnectionResetError):
-            print("Got disconnect / connection reset - shutting down.")
-            sys.exit(0)
-        raise context['exception']
-
-    try:
-        loop = asyncio.get_event_loop()
-        loop.set_exception_handler(handler)
-        loop.run_until_complete(run())
-    except KeyboardInterrupt:
-        print("Got CtrlC, shutting down.")
-
-
-if __name__ == '__main__':
-    print("STARTING")
-    main()
-    print("FINISHED")