reintroduced robot collision analyzer

there is an example script that can be run from the param
revolve.py --test-robot-collision path/to/robot_phenotype.yaml

Author: portaloffreedom

cpprevolve/revolve/gazebo/plugin/BodyAnalyzer.cpp

@@ -202,7 +202,9 @@ void BodyAnalyzer::OnContacts(ConstContactsPtr &msg)
 
 void BodyAnalyzer::AnalyzeRequest(ConstRequestPtr &request)
 {
-    if (request->request() not_eq " ")
+    auto request_title = request->request();
+
+    if (request_title not_eq "analyze_body")
     {
         // Request is not meant for us
         return;

pyrevolve/config.py

@@ -393,7 +393,13 @@ def str_to_address(v):
 parser.add_argument(
         '--test-robot',
         default=None, type=str,
-        help="Start a simulation with a single robot instead of running evolution. Loads yaml robots."
+        help="Start a simulation with a single robot instead of running evolution. Loads a yaml robot."
+)
+
+parser.add_argument(
+        '--test-robot-collision',
+        default=None, type=str,
+        help="Tests the collision of a single robot. Loads a yaml robot."
 )
 
 parser.add_argument(

pyrevolve/data_analisys/check_robot_collision.py

@@ -0,0 +1,70 @@
+import asyncio
+import logging
+import os
+
+import sys
+import time
+
+from pyrevolve import parser
+from pyrevolve.custom_logging import logger
+from pyrevolve.gazebo.analyze import BodyAnalyzer
+from pyrevolve.revolve_bot import RevolveBot
+from pyrevolve.util.supervisor.supervisor_collision import CollisionSimSupervisor
+
+
+async def test_collision_robot(robot_file_path: str):
+    log = logger.create_logger('experiment', handlers=[
+        logging.StreamHandler(sys.stdout),
+    ])
+
+    # Set debug level to DEBUG
+    log.setLevel(logging.DEBUG)
+
+    # Parse command line / file input arguments
+    settings = parser.parse_args()
+
+    assert (settings.test_robot_collision is not None)
+    robot = RevolveBot(_id=settings.test_robot_collision)
+    robot.load_file(robot_file_path, conf_type='yaml')
+    robot.save_file(f'{robot_file_path}.sdf', conf_type='sdf')
+
+    def simulator_died_callback(_process, _ret_code):
+        pass
+
+    # Start Simulator
+    if settings.simulator_cmd != 'debug':
+        simulator_supervisor = CollisionSimSupervisor(
+            world_file=os.path.join('tools', 'analyzer', 'analyzer-world.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',
+            process_terminated_callback=simulator_died_callback,
+        )
+        await simulator_supervisor.launch_simulator(port=settings.port_start)
+        await asyncio.sleep(0.1)
+
+    log.debug("simulator ready")
+
+    # Connect to the simulator and pause
+    analyzer = await BodyAnalyzer.create('127.0.0.1', settings.port_start)
+    log.debug("connection ready")
+    await asyncio.sleep(1)
+
+    log.info("Sending robot to the analyzer simulator")
+    start = time.time()
+    result = await analyzer.analyze_robot(robot)
+    end = time.time()
+
+    log.debug(f'Analyzer finished in {end-start}')
+    log.debug('result:')
+    log.debug(result)
+
+    await analyzer.disconnect()
+    log.debug("disconnected")
+
+    if settings.simulator_cmd != 'debug':
+        await simulator_supervisor.stop()
+
+    log.debug("simulator killed")

pyrevolve/data_analisys/visualize_robot.py

@@ -46,8 +46,8 @@ async def test_robot_run(robot_file_path: str):
     robot.load_file(robot_file_path, conf_type='yaml')
     robot.save_file(f'{robot_file_path}.sdf', conf_type='sdf')
 
+    await connection.pause(True)
     robot_manager = await connection.insert_robot(robot, Vector3(0, 0, 0.25), life_timeout=None)
-    await connection.pause(False)
     await asyncio.sleep(1.0)
 
     # Start the main life loop

pyrevolve/gazebo/analyze.py

@@ -1,14 +1,6 @@
-from __future__ import absolute_import
-from __future__ import print_function
-
-import asyncio
-import logging
-import sys
-
-from pyrevolve.sdfbuilder import SDF
-from pyrevolve.sdfbuilder.sensor import Sensor
-
-from ..spec import BodyAnalysisResponse
+from pyrevolve.custom_logging.logger import logger
+from pyrevolve.SDF.math import Vector3
+from pyrevolve.spec import BodyAnalysisResponse
 from .connect import connect, RequestHandler
 
 # Message ID sequencer, start at some high value to prevent ID clashes
@@ -23,68 +15,13 @@ def _msg_id():
     return r
 
 
-# Prevent the trollius logging warning
-kl = logging.getLogger("trollius")
-kl.addHandler(logging.StreamHandler(sys.stdout))
-
-
-def get_analysis_robot(robot, builder):
-    """
-    Creates an SDF model suitable for analysis from a robot object
-    and a builder.
-    :param robot:
-    :type robot: Robot
-    :param builder:
-    :type builder: BodyBuilder
-    :return:
-    """
-    model = builder.sdf_robot(
-            robot=robot,
-            controller_plugin=None,
-            name="analyze_bot",
-            analyzer_mode=True)
-    model.remove_elements_of_type(Sensor, recursive=True)
-    sdf = SDF()
-    sdf.add_element(model)
-    return sdf
-
-
-def analyze_body(sdf, address=("127.0.0.1", 11346)):
-    """
-    Single body analyzer. Opens a new connection, analyzes the
-    body, and returns the result. If you already have a manager
-    running doing other things, create an instance of `BodyAnalyzer`
-    instead.
-
-    :param sdf: SDF object consisting of BodyPart
-                instances.
-    :type sdf: SDF
-    :param address: Tuple of the hostname and port where the analyzer
-                    resides. Note that the default is one up from the default
-                    Gazebo port, since it is meant to be used with the
-                    `run-analyzer.sh` tool.
-    :type address: (str, int)
-    :return:
-    :rtype: (bool, (float, float, float))
-    """
-    response_obj = [None]
-
-    async def internal_analyze():
-        analyzer = await (BodyAnalyzer.create(address))
-        response_obj[0] = await (analyzer.analyze_sdf(sdf))
-
-    loop = asyncio.get_event_loop()
-    loop.run_until_complete(internal_analyze())
-    return response_obj[0]
-
-
 class BodyAnalyzer(object):
     """
     Class used to make body analysis requests.
     """
     _PRIVATE = object()
 
-    def __init__(self, _private, address):
+    def __init__(self, _private, address, port):
         """
         Private constructor - use the `create` coroutine instead.
 
@@ -97,43 +34,46 @@ def __init__(self, _private, address):
                              "the `create` coroutine.")
 
         self.address = address
+        self.port = port
         self.manager = None
         self.request_handler = None
 
     @classmethod
-    async def create(cls, address=("127.0.0.1", 11346)):
+    async def create(cls, address, port):
         """
         Instantiates a new body analyzer at the given address.
 
-        :param address: host, port tuple.
-        :type address: (str, int)
+        :param address: hostname.
+        :param port: host port.
         :return:
         """
-        self = cls(cls._PRIVATE, address)
-        await (self._init())
+        self = cls(cls._PRIVATE, address, port)
+        await self._init()
         return self
 
     async def _init(self):
         """
         BodyAnalyzer initialization coroutine
         :return:
         """
-        self.manager = await (connect(self.address))
+        self.manager = await connect(self.address)
         self.request_handler = await (
             RequestHandler.create(self.manager, msg_id_base=_msg_id()))
 
-    async def analyze_robot(self, robot, builder, max_attempts=5):
+    async def disconnect(self):
+        await self.manager.stop()
+        await self.request_handler.stop()
+
+    async def analyze_robot(self, robot, max_attempts=5):
         """
         Performs body analysis of a given Robot object.
         :param robot:
         :type robot: Robot
-        :param builder:
-        :type builder: Builder
         :param max_attempts:
         :return:
         """
-        sdf = get_analysis_robot(robot, builder)
-        ret = await (self.analyze_sdf(sdf, max_attempts=max_attempts))
+        sdf = robot.to_sdf(pose=Vector3(0, 0, 0))
+        ret = await self.analyze_sdf(sdf, max_attempts=max_attempts)
         return ret
 
     async def analyze_sdf(self, sdf, max_attempts=5):
@@ -156,7 +96,7 @@ async def analyze_sdf(self, sdf, max_attempts=5):
                 break
 
         if not msg:
-            # Error return
+            logger.error("analyze_sdf returned not valid message")
             return None
 
         if msg.HasField("boundingBox"):

pyrevolve/util/supervisor/supervisor_collision.py

@@ -0,0 +1,35 @@
+from .supervisor_multi import DynamicSimSupervisor
+
+
+class CollisionSimSupervisor(DynamicSimSupervisor):
+    def __init__(self,
+                 world_file,
+                 output_directory=None,
+                 simulator_cmd="gzserver",
+                 simulator_args=None,
+                 restore_arg="--restore-directory",
+                 snapshot_world_file="snapshot.world",
+                 restore_directory=None,
+                 plugins_dir_path=None,
+                 models_dir_path=None,
+                 simulator_name='collision',
+                 process_terminated_callback=None,
+                 ):
+        super().__init__(world_file,
+                         output_directory,
+                         simulator_cmd,
+                         simulator_args,
+                         restore_arg,
+                         snapshot_world_file,
+                         restore_directory,
+                         plugins_dir_path,
+                         models_dir_path,
+                         simulator_name,
+                         process_terminated_callback)
+
+    async def _launch_simulator(self,
+                                ready_str="Body analyzer ready",
+                                output_tag="simulator",
+                                address='localhost',
+                                port=11345):
+        return await super()._launch_simulator(ready_str, output_tag, address, port)

revolve.py

@@ -5,6 +5,7 @@
 import importlib
 
 from pyrevolve.data_analisys.visualize_robot import test_robot_run
+from pyrevolve.data_analisys.check_robot_collision import test_collision_robot
 from pyrevolve import parser
 from experiments.examples import only_gazebo
 
@@ -17,6 +18,9 @@ def run(loop, arguments):
     if arguments.test_robot is not None:
         return loop.run_until_complete(test_robot_run(arguments.test_robot))
 
+    if arguments.test_robot_collision is not None:
+        return loop.run_until_complete(test_collision_robot(arguments.test_robot_collision))
+
     if arguments.manager is not None:
         # this split will give errors on windows
         manager_lib = os.path.splitext(arguments.manager)[0]