def test_file_missing(self):
mock_running = MockValue(False)
mock_error = MockValue(None)
ROSLaunch._ROSLaunch__launch('foo.launch', mock_running, mock_error)
self.assertEquals(mock_error.value,
'ROS launch file does not exist: foo.launch')
self.assertEquals(mock_running.value, False)
def test_file_unreadable(self, mock_isfile):
mock_running = MockValue(False)
mock_error = MockValue(None)
ROSLaunch._ROSLaunch__launch('foo.launch', mock_running, mock_error)
self.assertEquals(mock_error.value,
'ROS launch file is not readable: foo.launch')
self.assertEquals(mock_running.value, False)
def test_launch_logic(self, mock_roslaunch, mock_access, mock_isfile):
mock_running = MockValue(False)
mock_error = MockValue(None)
def mock_sleep(time):
mock_running.value = False
with patch('time.sleep', side_effect=mock_sleep):
ROSLaunch._ROSLaunch__launch('foo.launch', mock_running,
mock_error)
self.assertEquals(mock_error.value, None)
self.assertEquals(mock_running.value, False)
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_launch_successful(self):
def mock_launch(that, launch_file, running, error):
running.value = True
time.sleep(1)
with patch.object(ROSLaunch, '_ROSLaunch__launch', mock_launch):
foo = ROSLaunch('foo.launch')
self.assertEquals(foo._running.value, True)
self.assertEquals(foo._error.value, None)
def initialize(self, robot):
"""
Initialize a robot into the scene
:param robot: robot to be initialize
"""
if robot.rosLaunchAbsPath:
robot.ROSLaunch = ROSLaunch(robot.rosLaunchAbsPath)
try:
logger.info("Adding robot {id} from {path}: ".format(
id=robot.id, path=robot.SDFFileAbsPath))
self.scene_handler().load_gazebo_model_file(
str(robot.id), robot.SDFFileAbsPath, robot.pose,
self.retina_config)
except Exception as e:
del self.robots[robot.id]
raise Exception(
"Error loading {robot} into the scene. {err}".format(
robot=robot.id, err=str(e)))
def test_shutdown(self):
mock_process = Mock()
def mock_launch(that, launch_file, running, error):
running.value = True
time.sleep(1)
def mock_sleep(time):
mock_process.is_alive = Mock(return_value=False)
with patch.object(ROSLaunch, '_ROSLaunch__launch', mock_launch),\
patch('time.sleep', side_effect=mock_sleep) as mock_time:
foo = ROSLaunch('foo.launch')
# should abort if already shut down
foo._process = mock_process
foo._process.is_alive = Mock(return_value=False)
foo.shutdown()
mock_time.assert_not_called()
# clean shutdown
foo._process.is_alive = Mock(return_value=True)
foo.shutdown()
self.assertEquals(foo._running.value, False)
# failed shutdown
foo._process.is_alive = Mock(return_value=True)
foo._running.value = True
def mock_sleep_fail(time):
foo._error.value = 'shutdown fail'
mock_time.side_effect = mock_sleep_fail
self.assertRaises(Exception, foo.shutdown)
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()