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, "")