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()
def test_shutdown(self):
# reset mocks to reset call counts
rospy.Service.reset_mock()
rospy.ServiceProxy.reset_mock()
recorder = GazeboSimulationRecorder(0)
recorder.shutdown()
# ensure the shutdowns are properly called
recorder._GazeboSimulationRecorder__service_simulation_recorder.shutdown.assert_called_once(
)
recorder._GazeboSimulationRecorder__recorder_cleanup.assert_called_once(
)
def test_init(self):
# reset mocks to reset call counts
rospy.Service.reset_mock()
rospy.ServiceProxy.reset_mock()
# ensure the constructor is calling our mocks properly
recorder = GazeboSimulationRecorder(0)
rospy.Service.assert_called_with(
'/ros_cle_simulation/0/simulation_recorder',
srv.SimulationRecorder,
recorder._GazeboSimulationRecorder__command)
rospy.ServiceProxy.assert_has_calls([
call('/gazebo/recording/start', Trigger),
call('/gazebo/recording/stop', Trigger),
call('/gazebo/recording/cancel', Trigger),
call('/gazebo/recording/cleanup', Trigger),
call('/gazebo/recording/get_recording', Trigger)
])
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 _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 test_command(self):
# reset mocks to reset call counts
rospy.Service.reset_mock()
rospy.ServiceProxy.reset_mock()
recorder = GazeboSimulationRecorder(0)
# valid calls
recorder._GazeboSimulationRecorder__command(
srv.SimulationRecorderRequest(srv.SimulationRecorderRequest.STATE))
recorder._GazeboSimulationRecorder__recorder_state.assert_called_once()
recorder._GazeboSimulationRecorder__command(
srv.SimulationRecorderRequest(srv.SimulationRecorderRequest.START))
recorder._GazeboSimulationRecorder__recorder_start.assert_called_once()
recorder._GazeboSimulationRecorder__command(
srv.SimulationRecorderRequest(srv.SimulationRecorderRequest.STOP))
recorder._GazeboSimulationRecorder__recorder_stop.assert_called_once()
recorder._GazeboSimulationRecorder__command(
srv.SimulationRecorderRequest(
srv.SimulationRecorderRequest.CANCEL))
recorder._GazeboSimulationRecorder__recorder_cancel.assert_called_once(
)
recorder._GazeboSimulationRecorder__command(
srv.SimulationRecorderRequest(srv.SimulationRecorderRequest.RESET))
recorder._GazeboSimulationRecorder__recorder_cleanup.assert_called_once(
)
# invalid call
resp = recorder._GazeboSimulationRecorder__command(
srv.SimulationRecorderRequest('foo'))
self.assertEqual(False, resp.value)
self.assertEqual('Invalid Simulation Recorder command: foo',
resp.message)
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)