def test_spawn_vglconnect(self, mock_spawn): # failure: __node is None self.instance._node = None self.assertRaises(Exception, self.instance._spawn_vglconnect) # failure: __allocation_process is None self.instance = LuganoVizClusterGazebo() self.instance._allocation_process = None self.assertRaises(Exception, self.instance._spawn_vglconnect) self.instance._node = 'something that won\'t be used' self.instance._allocation_process = mock_spawn() mock_spawn().sendline = Mock() mock_spawn().expect = Mock(return_value=0) mock_spawn.call_count = 0 # successful call self.instance._spawn_vglconnect() self.assertEqual(mock_spawn.call_count, 1) self.assertNotEqual(mock_spawn().sendline.call_count, 0) # password missing mock_spawn().expect = Mock(return_value=1) self.assertRaises(Exception, self.instance._spawn_vglconnect) # generic error mock_spawn().expect = Mock(return_value=2) self.assertRaises(Exception, self.instance._spawn_vglconnect) self.instance = LuganoVizClusterGazebo()
def test_start_xvnc(self, mock_spawn): self.instance._spawn_vglconnect = mock_spawn mock_spawn().sendline = Mock() # failure: __node is None self.instance._node = None self.assertRaises(Exception, self.instance._start_xvnc) # failure: __allocation_process is None self.instance = LuganoVizClusterGazebo() self.instance._allocation_process = None self.assertRaises(Exception, self.instance._start_xvnc) self.instance._node = 'something that won\'t be used' self.instance._allocation_process = mock_spawn() mock_spawn().sendline = Mock() # error creating vnc mock_spawn().expect = Mock(return_value=1) self.assertRaises(Exception, self.instance._start_xvnc) # successful call mock_spawn().expect = Mock(return_value=0) mock_spawn.call_count = 0 self.instance._start_xvnc() self.assertEqual(mock_spawn.call_count, 1) self.assertNotEqual(mock_spawn().sendline.call_count, 0) self.assertNotEqual(mock_spawn().expect.call_count, 0) self.instance = LuganoVizClusterGazebo()
def test_start_gazebo(self, mock_os_environ, mock_spawn): # failure: __node is None self.instance = LuganoVizClusterGazebo() self.instance._node = None self.assertRaises(Exception, self.instance._start_gazebo, 'random_master_uri') # failure: __allocation_process is None self.instance = LuganoVizClusterGazebo() self.instance._allocation_process = None self.assertRaises(Exception, self.instance._start_gazebo, 'random_master_uri') # failure: __remote_display_port is -1 self.instance = LuganoVizClusterGazebo() self.instance._remote_display_port = -1 self.assertRaises(Exception, self.instance._start_gazebo, 'random_master_uri') self.instance = LuganoVizClusterGazebo() self.instance._node = 'not none' self.instance._allocation_process = 'this neither' self.instance._remote_display_port = 1 self.instance._spawn_vglconnect = mock_spawn mock_spawn().sendline = Mock() mock_spawn().expect = Mock(return_value=1) self.assertRaises(Exception, self.instance._start_gazebo, 'random_master_uri') self.instance = LuganoVizClusterGazebo()
def test_start(self): self.instance._allocate_job = Mock() self.instance._start_fake_X = Mock() self.instance._start_xvnc = Mock() self.instance._start_gazebo = Mock() self.instance.start('random_master_uri') self.assertEqual(self.instance._allocate_job.call_count, 1) self.assertEqual(self.instance._start_fake_X.call_count, 1) self.assertEqual(self.instance._start_xvnc.call_count, 1) self.assertEqual(self.instance._start_gazebo.call_count, 1) self.instance = LuganoVizClusterGazebo()
def test_stop(self, mock_spawn): self.instance._clean_remote_files = Mock() self.instance._deallocate_job = Mock() self.instance._remote_xvnc_process = Mock() self.instance._gazebo_remote_process = Mock() self.instance._x_server_process = Mock() self.instance._allocation_process = mock_spawn() mock_spawn().sendline = Mock() self.instance.stop() self.assertEqual(self.instance._clean_remote_files.call_count, 1) self.assertEqual(self.instance._deallocate_job.call_count, 1) self.instance = LuganoVizClusterGazebo()
def __init__(self, sim_config): """ Creates a new simulation assembly to simulate an experiment using the CLE and Gazebo :param sim_config: config of the simulation to be managed """ super(GazeboSimulationAssembly, self).__init__(sim_config) self.robotManager = RobotManager() if self.sim_config.gzserver_host == 'local': self.gzserver = LocalGazeboServerInstance() elif self.sim_config.gzserver_host == 'lugano': self.gzserver = LuganoVizClusterGazebo(self.sim_config.timeout.tzinfo if self.sim_config.timeout is not None else None, self.sim_config.reservation) else: raise Exception("The gzserver location '{0}' is not supported.", self.sim_config.gzserver_host) self.gzweb = None self._initial_ros_params = None self._initial_ros_nodes = None self.ros_launcher = None self.gazebo_recorder = None
def __init__(self, sim_id, exc, bibi, **par): """ Creates a new simulation assembly to simulate an experiment using the CLE and Gazebo :param sim_id: The simulation id :param exc: The experiment configuration :param bibi: The BIBI configuration :param gzserver_host: The gazebo host :param reservation: The reservation number :param timeout: The timeout for the simulation """ super(GazeboSimulationAssembly, self).__init__(sim_id, exc, bibi, par) if models_path is None: raise Exception("Server Error. NRP_MODELS_DIRECTORY not defined.") # determine the Gazebo simulator target, due to dependencies this must # happen here gzserver_host = par.get('gzserver_host', 'local') timeout = par.get('timeout', None) reservation = par.get('reservation', None) if gzserver_host == 'local': self.gzserver = LocalGazeboServerInstance() elif gzserver_host == 'lugano': self.gzserver = LuganoVizClusterGazebo( timeout.tzinfo if timeout is not None else None, reservation) else: raise Exception("The gzserver location '{0}' is not supported.", gzserver_host) self._timeout = timeout self.__gzserver_host = gzserver_host self._gazebo_helper = None self.gzweb = None self.ros_launcher = None self.gazebo_recorder = None
class GazeboSimulationAssembly(SimulationAssembly): """ The abstract base class for a simulation assembly that uses Gazebo for world simulation """ def __init__(self, sim_id, exc, bibi, **par): """ Creates a new simulation assembly to simulate an experiment using the CLE and Gazebo :param sim_id: The simulation id :param exc: The experiment configuration :param bibi: The BIBI configuration :param gzserver_host: The gazebo host :param reservation: The reservation number :param timeout: The timeout for the simulation """ super(GazeboSimulationAssembly, self).__init__(sim_id, exc, bibi, par) if models_path is None: raise Exception("Server Error. NRP_MODELS_DIRECTORY not defined.") # determine the Gazebo simulator target, due to dependencies this must # happen here gzserver_host = par.get('gzserver_host', 'local') timeout = par.get('timeout', None) reservation = par.get('reservation', None) if gzserver_host == 'local': self.gzserver = LocalGazeboServerInstance() elif gzserver_host == 'lugano': self.gzserver = LuganoVizClusterGazebo( timeout.tzinfo if timeout is not None else None, reservation) else: raise Exception("The gzserver location '{0}' is not supported.", gzserver_host) self._timeout = timeout self.__gzserver_host = gzserver_host self._gazebo_helper = None self.gzweb = None self.ros_launcher = None self.gazebo_recorder = None 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() # pylint: disable=missing-docstring # pylint: disable=broad-except def __set_env_for_gzbridge(self): def get_gzbridge_setting(name, default): """ Obtain parameter from pyxb bindings of experiment schema. If something goes wrong return default value. The default also defines the type that the parameter should have. :param name: Name of the parameter :param default_: default value :return: a string """ try: s = self.exc.gzbridgesettings val = getattr(s, name) val = type(default)(val) # pylint: disable=broad-except except Exception: val = default return repr(val) os.environ['GZBRIDGE_POSE_FILTER_DELTA_TRANSLATION'] \ = get_gzbridge_setting('pose_update_delta_translation', 1.e-5) os.environ[ 'GZBRIDGE_POSE_FILTER_DELTA_ROTATION'] = get_gzbridge_setting( 'pose_update_delta_rotation', 1.e-4) os.environ['GZBRIDGE_UPDATE_EARLY_THRESHOLD'] = get_gzbridge_setting( 'pose_update_early_threshold', 0.02) def _handle_gazebo_shutdown(self): # pragma: no cover """ Handles the case that gazebo died unexpectedly """ logger.exception("Gazebo died unexpectedly") # Avoid further notice self.gzserver.gazebo_died_callback = None # in case the simulation is still being started, we abort the # initialization self._abort_initialization = "Gazebo died unexpectedly" def shutdown(self): """ Shutdown CLE """ # Once we do reach this point, the simulation is stopped # and we can clean after ourselves. # pylint: disable=broad-except, too-many-branches,too-many-statements # Clean up gazebo after ourselves number_of_subtasks = 4 if self.exc.rosLaunch is not None: number_of_subtasks += 1 if self.bibi.extRobotController is not None: number_of_subtasks += 1 try: # Check if notifications to clients are currently working try: self.ros_notificator.start_task( "Stopping simulation", "Shutting down simulation recorder", number_of_subtasks=number_of_subtasks, block_ui=False) notifications = True except Exception, e: logger.error("Could not send notifications") logger.exception(e) notifications = False # Call the recorder plugin to shutdown before shutting down Gazebo if self.gazebo_recorder is not None: try: self.gazebo_recorder.shutdown() except Exception, e: logger.warning( "Gazebo recorder could not be shutdown successfully") logger.exception(e) self._shutdown(notifications) # Shutdown gzweb before shutting down Gazebo if self.gzweb is not None: try: if notifications: self.ros_notificator.update_task( "Shutting down Gazebo web client", update_progress=True, block_ui=False) self.gzweb.stop() except Exception, e: logger.warning("gzweb could not be stopped successfully") logger.exception(e)
def setUp(self): self.instance = LuganoVizClusterGazebo()
class TestLuganoVizClusterGazebo(unittest.TestCase): def setUp(self): self.instance = LuganoVizClusterGazebo() @patch('pexpect.spawn') def test_start_fake_x(self, mock_spawn): mock_spawn().expect = Mock(return_value=1) mock_spawn.call_count = 0 self.instance._start_fake_X() self.assertEqual(mock_spawn.call_count, 1) self.assertNotEqual(mock_spawn().expect.call_count, 0) mock_spawn().expect = Mock(return_value=3) self.assertRaises(Exception, self.instance._start_fake_X) @patch('pexpect.spawn') def test_spawn_vglconnect(self, mock_spawn): # failure: __node is None self.instance._node = None self.assertRaises(Exception, self.instance._spawn_vglconnect) # failure: __allocation_process is None self.instance = LuganoVizClusterGazebo() self.instance._allocation_process = None self.assertRaises(Exception, self.instance._spawn_vglconnect) self.instance._node = 'something that won\'t be used' self.instance._allocation_process = mock_spawn() mock_spawn().sendline = Mock() mock_spawn().expect = Mock(return_value=0) mock_spawn.call_count = 0 # successful call self.instance._spawn_vglconnect() self.assertEqual(mock_spawn.call_count, 1) self.assertNotEqual(mock_spawn().sendline.call_count, 0) # password missing mock_spawn().expect = Mock(return_value=1) self.assertRaises(Exception, self.instance._spawn_vglconnect) # generic error mock_spawn().expect = Mock(return_value=2) self.assertRaises(Exception, self.instance._spawn_vglconnect) self.instance = LuganoVizClusterGazebo() @patch('pexpect.spawn') def test_start_xvnc(self, mock_spawn): self.instance._spawn_vglconnect = mock_spawn mock_spawn().sendline = Mock() # failure: __node is None self.instance._node = None self.assertRaises(Exception, self.instance._start_xvnc) # failure: __allocation_process is None self.instance = LuganoVizClusterGazebo() self.instance._allocation_process = None self.assertRaises(Exception, self.instance._start_xvnc) self.instance._node = 'something that won\'t be used' self.instance._allocation_process = mock_spawn() mock_spawn().sendline = Mock() # error creating vnc mock_spawn().expect = Mock(return_value=1) self.assertRaises(Exception, self.instance._start_xvnc) # successful call mock_spawn().expect = Mock(return_value=0) mock_spawn.call_count = 0 self.instance._start_xvnc() self.assertEqual(mock_spawn.call_count, 1) self.assertNotEqual(mock_spawn().sendline.call_count, 0) self.assertNotEqual(mock_spawn().expect.call_count, 0) self.instance = LuganoVizClusterGazebo() @patch('pexpect.spawn') @patch('hbp_nrp_cleserver.server.LuganoVizClusterGazebo.os.environ') def test_start_gazebo(self, mock_os_environ, mock_spawn): # failure: __node is None self.instance = LuganoVizClusterGazebo() self.instance._node = None self.assertRaises(Exception, self.instance._start_gazebo, 'random_master_uri') # failure: __allocation_process is None self.instance = LuganoVizClusterGazebo() self.instance._allocation_process = None self.assertRaises(Exception, self.instance._start_gazebo, 'random_master_uri') # failure: __remote_display_port is -1 self.instance = LuganoVizClusterGazebo() self.instance._remote_display_port = -1 self.assertRaises(Exception, self.instance._start_gazebo, 'random_master_uri') self.instance = LuganoVizClusterGazebo() self.instance._node = 'not none' self.instance._allocation_process = 'this neither' self.instance._remote_display_port = 1 self.instance._spawn_vglconnect = mock_spawn mock_spawn().sendline = Mock() mock_spawn().expect = Mock(return_value=1) self.assertRaises(Exception, self.instance._start_gazebo, 'random_master_uri') self.instance = LuganoVizClusterGazebo() def test_start(self): self.instance._allocate_job = Mock() self.instance._start_fake_X = Mock() self.instance._start_xvnc = Mock() self.instance._start_gazebo = Mock() self.instance.start('random_master_uri') self.assertEqual(self.instance._allocate_job.call_count, 1) self.assertEqual(self.instance._start_fake_X.call_count, 1) self.assertEqual(self.instance._start_xvnc.call_count, 1) self.assertEqual(self.instance._start_gazebo.call_count, 1) self.instance = LuganoVizClusterGazebo() @patch('pexpect.spawn') def test_stop(self, mock_spawn): self.instance._clean_remote_files = Mock() self.instance._deallocate_job = Mock() self.instance._remote_xvnc_process = Mock() self.instance._gazebo_remote_process = Mock() self.instance._x_server_process = Mock() self.instance._allocation_process = mock_spawn() mock_spawn().sendline = Mock() self.instance.stop() self.assertEqual(self.instance._clean_remote_files.call_count, 1) self.assertEqual(self.instance._deallocate_job.call_count, 1) self.instance = LuganoVizClusterGazebo() def test_restart(self): with patch( "hbp_nrp_cleserver.server.LuganoVizClusterGazebo.LuganoVizClusterGazebo.start" ): with patch( "hbp_nrp_cleserver.server.LuganoVizClusterGazebo.LuganoVizClusterGazebo.stop" ): self.instance.restart('') self.assertEqual(self.instance.start.call_count, 1) self.assertEqual(self.instance.stop.call_count, 1)
class GazeboSimulationAssembly(SimulationAssembly): # pragma: no cover """ The abstract base class for a simulation assembly that uses Gazebo for world simulation """ def __init__(self, sim_config): """ Creates a new simulation assembly to simulate an experiment using the CLE and Gazebo :param sim_config: config of the simulation to be managed """ super(GazeboSimulationAssembly, self).__init__(sim_config) self.robotManager = RobotManager() if self.sim_config.gzserver_host == 'local': self.gzserver = LocalGazeboServerInstance() elif self.sim_config.gzserver_host == 'lugano': self.gzserver = LuganoVizClusterGazebo(self.sim_config.timeout.tzinfo if self.sim_config.timeout is not None else None, self.sim_config.reservation) else: raise Exception("The gzserver location '{0}' is not supported.", self.sim_config.gzserver_host) self.gzweb = None self._initial_ros_params = None self._initial_ros_nodes = None self.ros_launcher = None self.gazebo_recorder = None def _start_gazebo(self, extra_models): """ 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 """ self._initial_ros_params = rospy.get_param_names() self._initial_ros_nodes = rosnode.get_node_names() # 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 # experiment specific gzserver command line arguments gzserver_args = '--seed {rng_seed} -e {engine} {world_file}'.format( rng_seed=self.rng_seed, engine=self.sim_config.physics_engine, world_file=self.sim_config.world_model.resource_path.abs_path) # If playback is specified, load the first log/world file in the recording at Gazebo launch if self.sim_config.playback_path: gzserver_args += ' --play {path}'.format( path=os.path.join(self.sim_config.playback_path, 'gzserver/1.log')) else: # optional roslaunch support prior to Gazebo launch for non-playback simulations if self.sim_config.ros_launch_abs_path is not None: if self.sim_config.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.sim_config.ros_launch_abs_path) 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.robotManager.init_scene_handler() self._notify("Connecting to Gazebo simulation recorder") self.gazebo_recorder = GazeboSimulationRecorder(self.sim_config.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() # pylint: disable=missing-docstring def __set_env_for_gzbridge(self): os.environ['GZBRIDGE_POSE_FILTER_DELTA_TRANSLATION'] = self.sim_config.gzbridge_setting( 'pose_update_delta_translation', 1.e-5) os.environ['GZBRIDGE_POSE_FILTER_DELTA_ROTATION'] = self.sim_config.gzbridge_setting( 'pose_update_delta_rotation', 1.e-4) os.environ['GZBRIDGE_UPDATE_EARLY_THRESHOLD'] = self.sim_config.gzbridge_setting( 'pose_update_early_threshold', 0.02) def _handle_gazebo_shutdown(self): # pragma: no cover """ Handles the case that gazebo died unexpectedly """ logger.exception("Gazebo died unexpectedly") # Avoid further notice self.gzserver.gazebo_died_callback = None # in case the simulation is still being started, we abort the initialization self._abort_initialization = "Gazebo died unexpectedly" def shutdown(self): """ Shutdown CLE """ # Once we do reach this point, the simulation is stopped # and we can clean after ourselves. # pylint: disable=broad-except, too-many-branches,too-many-statements # Clean up gazebo after ourselves number_of_subtasks = 4 if self.sim_config.ros_launch_abs_path is not None: number_of_subtasks += 1 if self.sim_config.ext_robot_controller is not None: number_of_subtasks += 1 try: # Check if notifications to clients are currently working try: self.ros_notificator.start_task("Stopping simulation", "Shutting down simulation recorder", number_of_subtasks=number_of_subtasks, block_ui=False) notifications = True except Exception, e: logger.error("Could not send notifications") logger.exception(e) notifications = False # Call the recorder plugin to shutdown before shutting down Gazebo if self.gazebo_recorder is not None: try: self.gazebo_recorder.shutdown() except Exception, e: logger.warning("Gazebo recorder could not be shutdown successfully") logger.exception(e) self._shutdown(notifications) # Shutdown gzweb before shutting down Gazebo if self.gzweb is not None: try: if notifications: self.ros_notificator.update_task("Shutting down Gazebo web client", update_progress=True, block_ui=False) self.gzweb.stop() except Exception, e: logger.warning("gzweb could not be stopped successfully") logger.exception(e)