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 __init__(self):
self.__persistent_services = []
rospy.wait_for_service('/gazebo/get_physics_properties')
self.__get_physics_properties = rospy.ServiceProxy(
'/gazebo/get_physics_properties',
GetPhysicsProperties,
persistent=True)
self.__persistent_services.append(self.__get_physics_properties)
rospy.wait_for_service('/gazebo/get_world_properties')
self.__get_world_properties = rospy.ServiceProxy(
'/gazebo/get_world_properties',
GetWorldProperties,
persistent=True)
self.__persistent_services.append(self.__get_world_properties)
rospy.wait_for_service('/gazebo/set_physics_properties')
self.__set_physics_properties = rospy.ServiceProxy(
'/gazebo/set_physics_properties',
SetPhysicsProperties,
persistent=True)
self.__persistent_services.append(self.__set_physics_properties)
rospy.wait_for_service('/gazebo/pause_physics')
self.__pause_client = rospy.ServiceProxy('/gazebo/pause_physics',
Empty,
persistent=True)
self.__persistent_services.append(self.__pause_client)
rospy.wait_for_service('/gazebo/reset_sim')
self.__reset = rospy.ServiceProxy('/gazebo/reset_sim',
Empty,
persistent=True)
self.__persistent_services.append(self.__reset)
rospy.wait_for_service('/gazebo/end_world')
self.__endWorld = rospy.ServiceProxy('/gazebo/end_world',
Empty,
persistent=True)
self.__persistent_services.append(self.__endWorld)
rospy.wait_for_service('/gazebo/advance_simulation')
self.__advance_simulation = AsynchonousRospyServiceProxy(
'/gazebo/advance_simulation', AdvanceSimulation, persistent=True)
self.__persistent_services.append(self.__advance_simulation)
self.__time_step = 0.0
self.__robots = {}
self.__is_initialized = False
self.gazebo_helper = GazeboHelper()
def _start_gazebo(self, rng_seed, playback_path, extra_models, world_file):
"""
Configures and starts the Gazebo simulator and backend services
:param rng_seed: RNG seed to spawn Gazebo with
:param playback_path: A path to playback information
:param extra_models: An additional models path or None
:param world_file: The world file that should be loaded by Gazebo
"""
# Gazebo configuration and launch
self._notify("Starting Gazebo robotic simulator")
ifaddress = netifaces.ifaddresses(
config.config.get('network', 'main-interface'))
local_ip = ifaddress[netifaces.AF_INET][0]['addr']
ros_master_uri = os.environ.get("ROS_MASTER_URI").replace(
'localhost', local_ip)
self.gzserver.gazebo_died_callback = self._handle_gazebo_shutdown
# Physics engine selection from ExDConfig; pass as -e <physics_engine>
# to gzserver
physics_engine = self.exc.physicsEngine
logger.info("Looking for physicsEngine tag value in ExDConfig")
if physics_engine is not None:
logger.info("Physics engine specified in ExDConfig: " +
str(repr(physics_engine)))
# No need to check that the physics engine is valid, pyxb already does that
else:
logger.info(
"No physics engine specified explicitly. Using default setting 'ode'"
)
physics_engine = "ode"
# experiment specific gzserver command line arguments
gzserver_args = '--seed {rng_seed} -e {engine} {world_file}'\
.format(rng_seed=rng_seed,
engine=physics_engine,
world_file=world_file)
# If playback is specified, load the first log/world file in the recording at Gazebo launch
# TODO: when storage server is available this should be updated
if playback_path:
gzserver_args += ' --play {path}/gzserver/1.log'.format(
path=playback_path)
# We use the logger hbp_nrp_cle.user_notifications in the CLE to log
# information that is useful to know for the user.
# In here, we forward any info message sent to this logger to the
# notificator
gazebo_logger = logging.getLogger('hbp_nrp_cle.user_notifications')
gazebo_logger.setLevel(logging.INFO)
gazebo_logger.handlers.append(NotificatorHandler())
# optional roslaunch support prior to Gazebo launch
if self.exc.rosLaunch is not None:
# NRRPLT-5134, only local installs are currently supported
if self.__gzserver_host != 'local':
raise Exception(
'roslaunch is currently only supported on local installs.')
self._notify(
"Launching experiment ROS nodes and configuring parameters")
self.ros_launcher = ROSLaunch(self.exc.rosLaunch.src)
try:
logger.info("gzserver arguments: " + gzserver_args)
self.gzserver.start(ros_master_uri, extra_models, gzserver_args)
except XvfbXvnError as exception:
logger.error(exception)
error = "Recoverable error occurred. Please try again. Reason: {0}".format(
exception)
raise Exception(error)
self._notify("Connecting to Gazebo robotic simulator")
self._gazebo_helper = GazeboHelper()
self._notify("Connecting to Gazebo simulation recorder")
self.gazebo_recorder = GazeboSimulationRecorder(self.sim_id)
self._notify("Starting Gazebo web client")
os.environ['GAZEBO_MASTER_URI'] = self.gzserver.gazebo_master_uri
self.__set_env_for_gzbridge()
# We do not know here in which state the previous user did let us
# gzweb.
self.gzweb = LocalGazeboBridgeInstance()
self.gzweb.restart()
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
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)
class RosControlAdapter(IRobotControlAdapter):
"""
Represents a robot simulation adapter actually using ROS
"""
def __init__(self):
self.__persistent_services = []
rospy.wait_for_service('/gazebo/get_physics_properties')
self.__get_physics_properties = rospy.ServiceProxy(
'/gazebo/get_physics_properties',
GetPhysicsProperties,
persistent=True)
self.__persistent_services.append(self.__get_physics_properties)
rospy.wait_for_service('/gazebo/get_world_properties')
self.__get_world_properties = rospy.ServiceProxy(
'/gazebo/get_world_properties',
GetWorldProperties,
persistent=True)
self.__persistent_services.append(self.__get_world_properties)
rospy.wait_for_service('/gazebo/set_physics_properties')
self.__set_physics_properties = rospy.ServiceProxy(
'/gazebo/set_physics_properties',
SetPhysicsProperties,
persistent=True)
self.__persistent_services.append(self.__set_physics_properties)
rospy.wait_for_service('/gazebo/pause_physics')
self.__pause_client = rospy.ServiceProxy('/gazebo/pause_physics',
Empty,
persistent=True)
self.__persistent_services.append(self.__pause_client)
rospy.wait_for_service('/gazebo/reset_sim')
self.__reset = rospy.ServiceProxy('/gazebo/reset_sim',
Empty,
persistent=True)
self.__persistent_services.append(self.__reset)
rospy.wait_for_service('/gazebo/end_world')
self.__endWorld = rospy.ServiceProxy('/gazebo/end_world',
Empty,
persistent=True)
self.__persistent_services.append(self.__endWorld)
rospy.wait_for_service('/gazebo/advance_simulation')
self.__advance_simulation = AsynchonousRospyServiceProxy(
'/gazebo/advance_simulation', AdvanceSimulation, persistent=True)
self.__persistent_services.append(self.__advance_simulation)
self.__time_step = 0.0
self.__robots = {}
self.__is_initialized = False
self.gazebo_helper = GazeboHelper()
def initialize(self):
"""
Initializes the world simulation control adapter
"""
if not self.__is_initialized:
physics = self.__get_physics_properties()
paused = physics.pause
if not paused:
self.__pause_client()
self.__time_step = physics.time_step
self.__is_initialized = True
logger.info("Robot control adapter initialized")
return self.__is_initialized
@property
def time_step(self):
"""
Gets the physics simulation time step in seconds
:param dt: The physics simulation time step in seconds
:return: The physics simulation time step in seconds
"""
return self.__time_step
def set_time_step(self, time_step):
"""
Sets the physics simulation time step in seconds
:param time_step: The physics simulation time step in seconds
:return: True, if the physics simulation time step is updated, otherwise False
"""
physics = self.__get_physics_properties()
success = self.__set_physics_properties(time_step,
physics.max_update_rate,
physics.gravity,
physics.ode_config)
if success:
self.__time_step = time_step
logger.info("new time step = %f", self.__time_step)
else:
logger.warn("impossible to set the new time step")
return success
@property
def is_paused(self):
"""
Queries the current status of the physics simulation
:return: True, if the physics simulation is paused, otherwise False
"""
physics = self.__get_physics_properties()
paused = physics.pause
return paused
@property
def is_alive(self):
"""
Queries the current status of the world simulation
:return: True, if the world simulation is alive, otherwise False
"""
logger.debug("Getting the world properties to check if we are alive")
world = self.__get_world_properties()
success = world.success
return success
def run_step_async(self, dt):
"""
Runs the world simulation for the given CLE time step in seconds
:param dt: The CLE time step in seconds
"""
# implement modulo to avoid floating point precision quirks in math.fmod or from direct %
# without this explicit fix, values such as 0.999 % 0.001 != 0.0 due to float accuracy
# https://en.wikipedia.org/wiki/Modulo_operation : a - (n * int(a/n))
r = dt - (self.__time_step * math.floor(dt / self.__time_step))
if r == 0.0:
steps = dt / self.__time_step
logger.debug("Advancing simulation")
return self.__advance_simulation(steps)
else:
logger.error("dt is not multiple of the physics time step")
raise ValueError("dt is not multiple of the physics time step")
def run_step(self, dt):
"""
Runs the world simulation for the given CLE time step in seconds
:param dt: The CLE time step in seconds
"""
return self.run_step_async(dt).result()
def shutdown(self):
"""
Shuts down the world simulation
"""
logger.info("Shutting down the world simulation")
for service in self.__persistent_services:
service.close()
logger.info("Robot control adapter stopped")
# Do not call endWorld here, it makes Gazebo Stop !
def reset(self):
"""
Resets the physics simulation
"""
logger.info("Resetting the world simulation")
self.__reset()
def set_robots(self, robots):
"""
Sets the list of robots
"""
self.__robots = robots
def reset_world(self, models, lights):
"""
Resets the world (robot excluded) to the state described by
models and lights
:param models: A dictionary containing pairs
(model_name: {'model_sdf': sdf, 'model_state_sdf': sdf}).
:param lights: A dictionary containing pairs (light_name: light sdf).
"""
logger.debug("Resetting the Environment")
# MODELS
# get the list of models' name currently the sim from gazebo
world_properties = self.__get_world_properties()
# robot model doesn't belong to the environment, so discard them from the active set
# we assume that the robot name contains the 'robot' substring
active_model_set = \
{m_name for m_name in world_properties.model_names
if m_name not in self.__robots.keys()}
original_model_set = frozenset(models.keys())
logger.debug("active_model_set: %s", active_model_set)
logger.debug("original_model_set: %s", original_model_set)
# LIGHTS
# get from gazebo the name of the lights in the scene
world_lights = self.gazebo_helper.get_lights_name_proxy()
# filter out sun from the world lights
active_lights_set = \
{l_name for l_name in world_lights.light_names if 'sun' not in l_name}
original_lights_set = frozenset(lights.keys())
logger.debug("active_lights_set: %s", active_lights_set)
logger.debug("original_lights_set: %s", original_lights_set)
# delete LIGHTS
for light_name in active_lights_set:
user_notifications_logger.info("Deleting: %s", light_name)
self.gazebo_helper.delete_light_proxy(light_name)
# delete MODELS
for model_name in active_model_set:
user_notifications_logger.info("Deleting: %s", model_name)
self.gazebo_helper.delete_model_proxy(model_name)
initial_pose = Pose()
initial_pose.position = Point(0, 0, 0)
initial_pose.orientation = Quaternion(0, 0, 0, 1)
# respawn LIGHTS
for light_name in original_lights_set:
user_notifications_logger.info("Loading: %s", light_name)
self.gazebo_helper.spawn_entity_proxy(light_name,
lights[light_name], "",
initial_pose, "")
# respawn MODELS
for model_name in original_model_set:
user_notifications_logger.info("Loading: %s", model_name)
self.gazebo_helper.spawn_entity_proxy(
model_name, models[model_name]['model_sdf'], "", initial_pose,
"")