Source code for hbp_nrp_cleserver.server.GazeboSimulationAssembly

# This file is part of the Neurorobotics Platform software
# Copyright (C) 2014,2015,2016,2017 Human Brain Project
# The Human Brain Project is a European Commission funded project
# in the frame of the Horizon2020 FET Flagship plan.
# This program is free software; you can redistribute it and/or
# modify it under the terms of the GNU General Public License
# as published by the Free Software Foundation; either version 2
# of the License, or (at your option) any later version.
# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# GNU General Public License for more details.
# You should have received a copy of the GNU General Public License
# along with this program; if not, write to the Free Software
# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
This module contains the abstract base class for a simulation assembly
that uses Gazebo for world simulation

import logging
import netifaces
import os
import subprocess
import rospy
import rosnode

from hbp_nrp_cle.cle import config
from hbp_nrp_cleserver.server.SimulationAssembly import SimulationAssembly
from hbp_nrp_cleserver.server.ROSLaunch import ROSLaunch
from hbp_nrp_cleserver.server.LocalGazebo import LocalGazeboBridgeInstance, \
from hbp_nrp_cleserver.server.LuganoVizClusterGazebo import LuganoVizClusterGazebo, XvfbXvnError
from hbp_nrp_cleserver.server.GazeboSimulationRecorder import GazeboSimulationRecorder
from hbp_nrp_cle.robotsim.RobotManager import RobotManager

from hbp_nrp_backend.storage_client_api.StorageClient import StorageClient
from ._RobotCallHandler import RobotCallHandler

from hbp_nrp_commons.workspace.SimUtil import SimUtil

logger = logging.getLogger(__name__)

[docs]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." .format(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 self._storageClient = StorageClient() self._storageClient.set_sim_dir(sim_config.sim_dir) self.simAssetsDir = os.path.join(sim_config.sim_dir, 'assets') self._simResourcesDir = os.path.join(sim_config.sim_dir, 'resources') @property def storage_client(self): """ Gets the storage client handler """ return self._storageClient 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 = '{lockstep} --seed {rng_seed} -e {engine} {world_file}'.format( lockstep='--lockstep' if self.sim_config.gazebo_lockstep else '', 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:"gzserver arguments: %s", 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.sim_config.record_ros_topics) 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 _prepare_simconfig_robot_models(self): """ For each robot model in sim_config checks that the model sdf file exists in SDFFileAbsPath :return: """ if not self.sim_config.robot_models: return for robot in self.sim_config.robot_models.values(): robot_handler = RobotCallHandler(self) sdf_abs_path = SimUtil.find_file_in_paths( robot.SDFFileAbsPath, [os.path.join(self.sim_dir,] ) # By default, model assets are assumed to be in the experiment folder. # If the model attribute of bodyModel is specified, # assets must be fetched from the template Models instead. if robot.model: status, ret = robot_handler.prepare_robot(robot.model) if not status: raise Exception("Could not prepare custom robot {err}".format(err=ret)) # if no SDF is specified in the BIBI's bodyModel tag # use the SDF of the fetched model sdf_abs_path = ret if not robot.SDFFileAbsPath else sdf_abs_path # still couldn't find the SDF, abort! if not sdf_abs_path: raise Exception("Could not find robot file: {0}".format(robot.SDFFileAbsPath)) robot.SDFFileAbsPath = sdf_abs_path # Find robot specific roslaunch file in the directory where the SDF resides # Take the first one (by name) if multiple available rosLaunchRelPath = next((f for f in os.listdir(os.path.dirname(robot.SDFFileAbsPath)) if f.endswith('.launch')), None) robot.rosLaunchAbsPath = (None if rosLaunchRelPath is None else os.path.join(os.path.dirname(robot.SDFFileAbsPath), rosLaunchRelPath)) 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"
[docs] 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 as 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 as 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 as e: logger.warning("gzweb could not be stopped successfully") logger.exception(e) if self.gzserver is not None: try: if notifications: self.ros_notificator.update_task("Shutting down Gazebo robotic simulator", update_progress=True, block_ui=False) self.gzserver.stop() except Exception as e: logger.warning("gzserver could not be stopped successfully") logger.exception(e) # Stop any external robot controllers if self.sim_config.ext_robot_controller: robot_controller_filepath = SimUtil.find_file_in_paths( self.sim_config.ext_robot_controller, self.sim_config.model_paths) if robot_controller_filepath: if notifications: self.ros_notificator.update_task("Stopping external robot controllers", update_progress=True, block_ui=False) subprocess.check_call([robot_controller_filepath, 'stop']) # Stop any ROS nodes launched via roslaunch if self.sim_config.ros_launch_abs_path is not None and self.ros_launcher is not None: if notifications: self.ros_notificator.update_task("Shutting down launched ROS nodes", update_progress=True, block_ui=False) self.ros_launcher.shutdown() # try to notify for task completion, notificator should be valid until # the finally block below if notifications: self.ros_notificator.finish_task() finally: # always shut down the notificator ROS topics when done, no status for this # as there is no mechanism to deliver further updates try: if self.ros_notificator: self.ros_notificator.shutdown() except Exception as e: logger.error("The ROS notificator could not be shut down") logger.exception(e) SimUtil.delete_simulation_dir(self.sim_config.sim_dir) # Cleanup ROS core nodes, services, and topics (the command should be almost # instant and exit, but wrap it in a timeout since it's semi-officially supported)"Cleaning up ROS nodes and services") for param in rospy.get_param_names(): if param not in self._initial_ros_params: rospy.delete_param(param) for node in rosnode.get_node_names(): if node not in self._initial_ros_nodes: os.system('rosnode kill ' + str(node)) try: res = subprocess.check_output(["rosnode", "list"]) if res.find(b"/gazebo") > -1 and res.find(b"/Watchdog") > -1: os.system(b'rosnode kill /gazebo /Watchdog') elif res.find(b"/gazebo") > -1: os.system(b'rosnode kill /gazebo >/dev/null 2>&1') elif res.find(b"/Watchdog") > -1: os.system(b'rosnode kill /Watchdog >/dev/null 2>&1') except Exception as e: logger.exception(e) os.system("echo 'y' | timeout -s SIGKILL 10s rosnode cleanup >/dev/null 2>&1")
def _shutdown(self, notifications): # pragma: no cover """ Shutdown the CLE and any hooks before shutting down Gazebo :param notifications: A flag indicating whether notifications should be attempted to send """ raise NotImplementedError("This method must be overridden in inherited classes")