class DeterministicClosedLoopEngine(IClosedLoopControl):
"""
Implementation of the closed loop engine that runs transfer functions sequentially
to brain simulation and world simulation. Therefore, we are to some degree sure that
the TFs work on a reproducible state
"""
# default simulation timestep in seconds (20 ms)
DEFAULT_TIMESTEP = 0.02
def __init__(self,
robot_control_adapter,
robot_comm_adapter,
brain_control_adapter,
brain_comm_adapter,
transfer_function_manager,
dt
):
"""
Create an instance of the cle.
:param robot_control_adapter: an instance of IRobotContolAdapter
:param robot_comm_adapter: an instance of IRobotCommunicationAdapter
:param brain_control_adapter: an instance of IBrainContolAdapter
:param brain_comm_adapter: an instance of IBrainCommunicationAdapter
:param transfer_function_manager: an instance of ITransferFunctionManager
:param dt: The CLE time step in seconds
"""
self.rca = robot_control_adapter
self.rca_future = None
self.rcm = robot_comm_adapter
self.bca = brain_control_adapter
self.bcm = brain_comm_adapter
self.tfm = transfer_function_manager
# default timestep
self.timestep = dt
# stop flag ask the main loop to stop
self.stop_flag = threading.Event()
self.stop_flag.clear()
# stopped flag indicate if the main loop is stopped
self.stopped_flag = threading.Event()
self.stopped_flag.set()
# global clock
cle.clock = 0.0
self.initialized = False
# time measurements
# The real time is measured this way: each time play is called
# start_time is set to the current clock time. Each time the stop
# function is called (pausing the simulation) elapsed_time is
# incremented with (stop time - start_time). Thus, the real time is
# elapsed_time if paused
# elapsed_time + (current time - start_time) if running
self.start_time = 0.0
self.elapsed_time = 0.0
self._rca_elapsed_time = 0.0
self._bca_elapsed_time = 0.0
self.__network_file = None
self.__network_configuration = None
self.__initial_robot_poses = None
# This is to be eventually removed, as communications towards gazebo should
# be done from inside the RosControlAdapter
self.gazebo_helper = GazeboHelper()
self.__start_thread = None
self.__start_future = None
self.start_cb = lambda f: None
self.initial_models = None
self.initial_lights = None
def initialize(self, brain_file=None, **configuration):
"""
Initializes the closed loop engine.
:param brain_file: A python PyNN script containing the neural network definition
:param configuration: A set of populations
"""
self.rca.initialize()
self.bca.initialize()
self.tfm.initialize('tfnode')
cle.clock = 0.0
self.start_time = 0.0
self.elapsed_time = 0.0
self.initialized = True
if brain_file:
self.__network_file = brain_file
self.__network_configuration = configuration
try:
self.load_brain(brain_file, **configuration)
except BrainTimeoutException:
logger.info(
"Timeout occurs during loading the brain:" + brain_file)
except Exception as e:
logger.exception(
"Compiling Error during loading the brain({0}): {1!r}".format(brain_file, e))
@property
def is_initialized(self):
"""
Returns True if the simulation is initialized, False otherwise.
"""
return self.initialized
def load_populations(self, **populations):
"""
load new populations into the brain
:param populations: A dictionary indexed by population names and
containing neuron indices. Neuron indices can be defined by
lists of integers or slices. Slices are either python slices or
dictionaries containing 'from', 'to' and 'step' values.
"""
if self.initialized:
if self.running:
self.stop()
if self.bca.is_alive():
self.bca.shutdown()
logger.info("Loading new populations")
self.bca.load_populations(**populations)
self.tfm.hard_reset_brain_devices()
def load_brain(self, brain_file, **brain_populations):
"""
Creates a new brain in the running simulation
:param brain_file: A python PyNN script or an h5 file
containing the neural network definition
:param brain_populations: A (optional) dictionary indexed by population names and
containing neuron indices. Neuron indices can be defined by
lists of integers or slices. Slices are either python slices or
dictionaries containing 'from', 'to' and 'step' values.
"""
if self.initialized:
if self.running:
self.stop()
if self.bca.is_alive():
self.bca.shutdown()
logger.info("Recreating brain from file " + brain_file)
self.bca.load_brain(brain_file, **brain_populations)
logger.info("Resetting TFs")
self.tfm.hard_reset_brain_devices()
def run_step(self, timestep):
"""
Runs both simulations for the given time step in seconds.
:param timestep: simulation time, in seconds
:return: Updated simulation time, otherwise -1
"""
clk = cle.clock
# robot simulation
logger.debug("Run step: Robot simulation.")
self.rca_future = self.rca.run_step_async(timestep)
self.rcm.refresh_buffers(clk)
# brain simulation
logger.debug("Run step: Brain simulation")
start = time.time()
self.bca.run_step(timestep * 1000.0)
self.bcm.refresh_buffers(clk)
self._bca_elapsed_time += time.time() - start
# wait for all thread to finish
logger.debug("Run_step: waiting on Control thread")
try:
f = self.rca_future
f.result()
self._rca_elapsed_time += f.end - f.start
except ForcedStopException:
logger.warn("Simulation was brutally stopped.")
# transfer functions
logger.debug("Run step: Transfer functions")
self.tfm.run_robot_to_neuron(clk)
self.tfm.run_neuron_to_robot(clk)
# update clock
cle.clock += timestep
logger.debug("Run_step: done !")
return cle.clock
def shutdown(self):
"""
Shuts down both simulations.
"""
self.stop_flag.set()
self.stopped_flag.wait(5)
self.rcm.shutdown()
self.bcm.shutdown()
self.rca.shutdown()
self.bca.shutdown()
def start(self):
"""
Starts the orchestrated simulations
"""
if self.__start_future is None:
self.__start_future = Future()
if self.start_cb is not None:
self.start_cb(self.__start_future)
self.__start_thread = threading.Thread(target=self.__loop)
self.__start_thread.setDaemon(True)
self.__start_future.set_running_or_notify_cancel()
self.__start_thread.start()
return True
else:
logger.warning("CLE is already running")
return False
def __loop(self):
"""
Starts the orchestrated simulations.
This function does not return (starts an infinite loop).
"""
logger.info("Simulation loop started")
try:
try:
self.stop_flag.clear()
self.stopped_flag.clear()
self.start_time = time.time()
while not self.stop_flag.isSet():
self.run_step(self.timestep)
self.__start_future.set_result(None)
finally:
logger.info("Simulation loop ended")
self.elapsed_time += time.time() - self.start_time
self.__start_thread = None
self.stopped_flag.set()
# pylint: disable=broad-except
# we change the order intentionally (first finally then catch) because
# before throwing an custom exception (brainRuntimeException), the stopped flag
# has to be set in order to finish the stop function flow.
except Exception as e:
logger.exception(e)
self.__start_future.set_exception(e)
finally:
self.__start_future = None
def stop(self, forced=False):
"""
Stops the orchestrated simulations. Also waits for the current
simulation step to end.
Must be called on a separate thread from the start one, as an example
by a threading.Timer.
:param forced: If set, the CLE instance cancels pending tasks
:except Exception: throws an exception if the CLE was forced to stop but could not be
stopped
"""
self.stop_flag.set()
if forced:
if self.rca_future is not None and self.rca_future.running():
self.stopped_flag.wait(5)
if self.rca_future.running():
self.rca_future.set_exception(ForcedStopException())
self.wait_step(timeout=self.bca.get_Timeout())
if not self.stopped_flag.isSet():
raise Exception("The simulation loop could not be completed")
else:
self.wait_step()
def reset(self):
"""
Reset the orchestrated simulations (stops them before resetting).
"""
self.stop()
self.rca.reset()
self.bca.reset()
self.tfm.reset()
cle.clock = 0.0
self.start_time = 0.0
self.elapsed_time = 0.0
self._rca_elapsed_time = 0.0
self._bca_elapsed_time = 0.0
logger.info("CLE reset")
def reset_world(self, sdf_world_string=""):
"""
Reset the world to a given configuration, or to the initial one if none is passed.
:param sdf_world_string: the new environment to be set in the world simulator
(default value is the empty string for compatibility with the ROS message).
"""
if len(sdf_world_string) > 0:
models, lights = self.gazebo_helper.parse_world_string(sdf_world_string)
else:
# backward compatibility
# when working without collab, reset to the initial state
models = self.initial_models
lights = self.initial_lights
self.stop()
self.rca.reset_world(models, lights)
logger.info("CLE world reset")
def reset_brain(self, brain_file=None, populations=None):
"""
Reloads the brain and resets the transfer function.
If no parameter is specified, it reloads the initial brain.
:param brain_file: A python PyNN script containing the neural network definition
:param populations: A set of populations
"""
if brain_file is not None and populations is not None:
self.load_brain(brain_file, **populations)
else:
self.load_brain(self.__network_file, *self.__network_configuration)
logger.info("CLE Brain reset")
@property
def simulation_time(self): # -> float64
"""
Get the current simulation time.
"""
return cle.clock
@property
def running(self):
"""
Gets a flag indicating whether the simulation is running
"""
return self.__start_future is not None and self.__start_future.running()
@property
def real_time(self): # -> float64
"""
Get the total execution time.
"""
if self.running:
return self.elapsed_time + time.time() - self.start_time
return self.elapsed_time
@property
def initial_robot_poses(self):
"""
Get the robot pose at the beginning of the simulation.
"""
return self.__initial_robot_poses
@initial_robot_poses.setter
def initial_robot_poses(self, value):
"""
Set the robot pose at the beginning of the simulation.
Usually performed after CLE initialization.
:param value: The new value for the initial robot pose.
"""
self.__initial_robot_poses = value
def reset_robot_pose(self):
"""
Set the robot in the simulation to its initial pose.
"""
for rid, pose in self.__initial_robot_poses.iteritems():
self.gazebo_helper.set_model_pose(str(rid), pose)
self.tfm.reset()
def tf_elapsed_time(self):
"""
Gets the time share of the Transfer Functions
"""
return get_tf_elapsed_times(self.tfm)
def brainsim_elapsed_time(self):
"""
Gets the time share of the brain simulation
"""
return self._bca_elapsed_time
def robotsim_elapsed_time(self):
"""
Gets the time share of the robot simulation
"""
return self._rca_elapsed_time
def wait_step(self, timeout=None):
"""
Wait for the currently running simulation step to end.
:param timeout: The maximum amount of time (in seconds) to wait for the end of this step
"""
self.stopped_flag.wait(timeout)
class TestGazeboHelper(unittest.TestCase):
def setUp(self):
self.mock_env = patch(
'hbp_nrp_cle.robotsim.GazeboHelper.os.environ').start()
self.mock_env.get = MagicMock(return_value=None)
self.mock_wait_for_service = patch(
'hbp_nrp_cle.robotsim.GazeboHelper.rospy.wait_for_service').start(
)
self.mock_service_proxy = patch(
'hbp_nrp_cle.robotsim.GazeboHelper.rospy.ServiceProxy').start()
self.gazebo_helper = GazeboHelper()
def tearDown(self):
self.mock_env.stop()
self.mock_wait_for_service.stop()
self.mock_service_proxy.stop()
def test_gazebo_helper_init(self):
waited = sorted([
self.mock_wait_for_service.call_args_list[x][0][0]
for x in xrange(len(self.mock_wait_for_service.call_args_list))
])
proxied = sorted([
self.mock_service_proxy.call_args_list[x][0][0]
for x in xrange(len(self.mock_service_proxy.call_args_list))
])
services = sorted([
GZROS_S_SPAWN_SDF_ENTITY, GZROS_S_GET_WORLD_PROPERTIES,
GZROS_S_SET_MODEL_STATE, GZROS_S_DELETE_MODEL,
GZROS_S_DELETE_LIGHT, GZROS_S_DELETE_LIGHTS,
GZROS_S_GET_LIGHTS_NAME, GZROS_S_WAIT_FOR_RENDERING
])
self.assertEquals(services, waited)
self.assertEquals(services, proxied)
def test_load_gazebo_model_file(self):
self.gazebo_helper.load_sdf_entity = MagicMock()
with LogCapture('hbp_nrp_cle.user_notifications') as logcapture:
test_pose = Pose()
test_pose.position = Point(1, 2, 3)
test_pose.orientation = Quaternion(4, 5, 6, 7)
wpath = os.path.join(os.path.abspath(os.path.dirname(__file__)),
"sample_model.sdf")
self.gazebo_helper.load_gazebo_model_file("toto", wpath, test_pose)
self.assertEqual(
self.gazebo_helper.load_sdf_entity.call_args_list[0][0][0],
"toto")
actual_XML = objectify.fromstring(
self.gazebo_helper.load_sdf_entity.call_args_list[0][0][1])
actual_normalized_string = etree.tostring(actual_XML)
expected_XML = objectify.fromstring("""<?xml version="1.0" ?>
<sdf version="1.5">
<model name='vr_poster'>
<pose>0 0 0 0 0 0</pose>
<static>1</static>
<link name='body'>
<visual name='vr_poster'>
<cast_shadows>1</cast_shadows>
<geometry>
<mesh>
<uri>model://viz_poster/meshes/viz_poster.dae</uri>
</mesh>
</geometry>
</visual>
</link>
</model>
</sdf>
""")
expected_normalized_string = etree.tostring(expected_XML)
self.assertEqual(actual_normalized_string,
expected_normalized_string)
self.assertEqual(self.gazebo_helper.load_sdf_entity.call_count, 1)
self.assertEqual(
self.gazebo_helper.load_sdf_entity.call_args_list[0][0][2],
test_pose)
logcapture.check(('hbp_nrp_cle.user_notifications', 'DEBUG',
'%s successfully loaded in Gazebo' % wpath))
def test_load_gazebo_model_file_with_retina(self):
self.gazebo_helper.load_sdf_entity = MagicMock()
wpath = os.path.join(os.path.abspath(os.path.dirname(__file__)),
"sample_model_with_retina.sdf")
test_retina_config_path = os.path.join(
os.path.abspath(os.path.dirname(__file__)),
"sample_retina_script.py")
self.gazebo_helper.load_gazebo_model_file("toto", wpath, None,
test_retina_config_path)
self.assertEqual(
self.gazebo_helper.load_sdf_entity.call_args_list[0][0][0], "toto")
actual_XML = etree.fromstring(
self.gazebo_helper.load_sdf_entity.call_args_list[0][0][1])
retina_script_path_elem = actual_XML.xpath(
"//sensor/plugin[@name='RetinaCameraPlugin']/retinaScriptPath")[-1]
self.assertEqual(retina_script_path_elem.text, test_retina_config_path)
def test_parse_gazebo_world_file(self):
def abs_path(file_name):
return os.path.join(os.path.abspath(os.path.dirname(__file__)),
file_name)
def file_to_normalized_xml_string(file_name):
file_path = abs_path(file_name)
with open(file_path, 'r') as file:
sdf_string = file.read()
return normalize_xml(sdf_string)
def normalize_xml(xml_string):
sdf_XML = objectify.fromstring(xml_string)
return etree.tostring(sdf_XML) # normalised sdf
sample_world_sdf_filename = abs_path("sample_world.sdf")
# call target function
models, lights = self.gazebo_helper.parse_gazebo_world_file(
sample_world_sdf_filename)
normalised_sun1_sdf = file_to_normalized_xml_string('sun1.sdf')
normalised_sun2_sdf = file_to_normalized_xml_string('sun2.sdf')
normalised_ground_plane_sdf = file_to_normalized_xml_string(
'ground_plane.sdf')
self.assertEquals(normalize_xml(models['ground_plane']['model_sdf']),
normalised_ground_plane_sdf)
self.assertEquals(normalize_xml(lights['sun1']), normalised_sun1_sdf)
self.assertEquals(normalize_xml(lights['sun2']), normalised_sun2_sdf)
def test_load_gazebo_world(self):
self.gazebo_helper.load_sdf_entity = MagicMock()
fake_sdf = '<sdf/>'
fake_models = {
'ground_plane': {
'model_sdf': fake_sdf,
'model_state_sdf': fake_sdf
}
}
fake_lights = {'sun1': fake_sdf, 'sun2': fake_sdf}
self.gazebo_helper.load_gazebo_world(fake_models, fake_lights)
self.assertEqual(
len(self.gazebo_helper.load_sdf_entity.call_args_list), 3)
self.gazebo_helper.load_sdf_entity.assert_any_call("sun1", fake_sdf)
self.gazebo_helper.load_sdf_entity.assert_any_call("sun2", fake_sdf)
self.gazebo_helper.load_sdf_entity.assert_any_call(
"ground_plane", fake_sdf)
def test_load_gazebo_world_file(self):
self.gazebo_helper.load_gazebo_world = MagicMock()
wpath = os.path.join(os.path.abspath(os.path.dirname(__file__)),
"sample_world.sdf")
# models = {'ground_plane': {'model_sdf': sdfGP, 'model_state_sdf': None} }
# lights = {'sun1': sdf1, 'sun2': sdf1}
models, lights = self.gazebo_helper.load_gazebo_world_file(wpath)
self.gazebo_helper.load_gazebo_world.assert_called_with(models, lights)
def test_load_sdf_entity(self):
instance = self.gazebo_helper.spawn_entity_proxy
sdf_xml = """<?xml version="1.0" ?>
<sdf version="1.5">
<model name='vr_poster'>
<pose>0 0 0 0 0 0</pose>
<static>1</static>
<link name='body'>
<visual name='vr_poster'>
<cast_shadows>1</cast_shadows>
<geometry>
<mesh>
<uri>model://viz_poster/meshes/viz_poster.dae</uri>
</mesh>
</geometry>
</visual>
</link>
</model>
</sdf>"""
self.gazebo_helper.load_sdf_entity("toto", sdf_xml)
arg_initial_pose = self.gazebo_helper.spawn_entity_proxy.call_args_list[
0][0][3]
ptn = arg_initial_pose.position
orn = arg_initial_pose.orientation
self.assertEquals((ptn.x, ptn.y, ptn.z), (0, 0, 0))
self.assertEquals((orn.x, orn.y, orn.z, orn.w), (0, 0, 0, 1))
self.assertEquals(instance.call_args_list[0][0][0], "toto")
# Testing with given pose
test_pose = Pose()
test_pose.position = Point(5, 3, 5)
test_pose.orientation = Quaternion(1, 2, 3, 4)
self.gazebo_helper.load_sdf_entity("toto", sdf_xml, test_pose)
arg_initial_pose = self.gazebo_helper.spawn_entity_proxy.call_args_list[
1][0][3]
self.assertEquals(arg_initial_pose, test_pose)
# Testing with invalid XML
self.assertRaises(etree.XMLSyntaxError,
self.gazebo_helper.load_sdf_entity, "toto",
"invalid XML string")
sdf_xml = """<?xml version="1.0" ?>
<sdf version="1.5">
<light name='sun2' type='directional'>
<cast_shadows>0</cast_shadows>
<pose>0 0 10 0 -0 0</pose>
<diffuse>0.4 0.4 0.4 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<direction>0.8 0 0.6</direction>
<attenuation>
<range>20</range>
<constant>0.5</constant>
<linear>0.01</linear>
<quadratic>0.001</quadratic>
</attenuation>
</light>
</sdf>"""
self.gazebo_helper.load_sdf_entity("light", sdf_xml)
arg_initial_pose = self.gazebo_helper.spawn_entity_proxy.call_args_list[
2][0][3]
ptn = arg_initial_pose.position
orn = arg_initial_pose.orientation
self.assertEquals((ptn.x, ptn.y, ptn.z), (0, 0, 0))
self.assertEquals((orn.x, orn.y, orn.z, orn.w), (0, 0, 0, 1))
self.assertEquals(instance.call_args_list[2][0][0], "light")
# Testing with given pose
test_pose = Pose()
test_pose.position = Point(5, 3, 5)
test_pose.orientation = Quaternion(1, 2, 3, 4)
self.gazebo_helper.load_sdf_entity("light", sdf_xml, test_pose)
arg_initial_pose = self.gazebo_helper.spawn_entity_proxy.call_args_list[
3][0][3]
self.assertEquals(arg_initial_pose, test_pose)
# Testing with invalid XML
self.assertRaises(etree.XMLSyntaxError,
self.gazebo_helper.load_sdf_entity, "light",
"invalid XML string")
def test_empty_gazebo_world(self):
self.gazebo_helper.empty_gazebo_world()
self.assertGreater(self.mock_service_proxy.call_count, 0)
self.assertGreater(self.mock_wait_for_service.call_count, 0)
def test_set_model_pose(self):
none_pose = Pose()
none_pose.position = Point(0, 0, 0)
none_pose.orientation = Quaternion(0, 0, 0, 1)
self.gazebo_helper.set_model_pose('robot', None)
arg_model_state = self.gazebo_helper.set_model_state_proxy.call_args_list[
0][0][0]
self.assertEquals(arg_model_state.model_name, 'robot')
self.assertEquals(arg_model_state.pose, none_pose)
custom_pose = Pose()
custom_pose.position = Point(0, 7, 0)
custom_pose.orientation = Quaternion(0, 0, 0, 1)
self.gazebo_helper.set_model_pose('robot', custom_pose)
arg_model_state = self.gazebo_helper.set_model_state_proxy.call_args_list[
1][0][0]
self.assertEquals(arg_model_state.model_name, 'robot')
self.assertEquals(arg_model_state.pose, custom_pose)
def test_load_textures(self):
# empty textures list -> noop
self.gazebo_helper.load_textures([])
self.assertFalse(self.gazebo_helper.spawn_entity_proxy.called)
self.assertFalse(self.gazebo_helper.delete_model_proxy.called)
# valid textures -> check service calls
dummy_textures = [{'name': 'dummy_texture_name'}]
self.gazebo_helper.load_textures(dummy_textures)
self.assertTrue(self.gazebo_helper.spawn_entity_proxy.called)
self.assertTrue(self.gazebo_helper.delete_model_proxy.called)