Source code for hbp_nrp_cleserver.server.PlaybackServer

# pylint: disable=too-many-lines
# 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.

Playback ROS wrapper overriding the SimulationServer implementation for playback

import logging
import rospy
from cle_ros_msgs import srv
from rosgraph_msgs.msg import Clock
from std_srvs.srv import Trigger
from gazebo_ros_replay_plugins.srv import Start

from hbp_nrp_cleserver.server.SimulationServer import SimulationServer
from hbp_nrp_cleserver.server.PlaybackServerLifecycle import PlaybackServerLifecycle
from hbp_nrp_cleserver.server.CLEGazeboSimulationAssembly import GazeboSimulationAssembly
from hbp_nrp_cleserver.bibi_config.notificator import NotificatorHandler

logger = logging.getLogger(__name__)

# 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')
notificator_handler = NotificatorHandler()

[docs]class PlaybackServer(SimulationServer): """ Playback ROS server overriding the ROSCLEServer implementation for playback """ def __init__(self, sim_id, timeout, timeout_type, gzserver, notificator, playback_path): """ Create the playback server :param sim_id: The simulation id :param timeout: The simulation timeout :param timeout_type: The type of simulation timeout :param gzserver: Gazebo simulator launch/control instance :param notificator: ROS state/error notificator interface :param playback_path: Absolute path to the playback files (where gzserver/1.log file is) """ super(PlaybackServer, self).__init__(sim_id, timeout, timeout_type, gzserver, notificator) # simulation time from playback for frontend display self.__sim_clock = 0 self.__sim_clock_subscriber = None # path to current active playback, accessible by Gazebo self.__playback_path = playback_path # services to interact with playback plugin self.__service_configure = None self.__service_start = None self.__service_pause = None self.__service_stop = None self.__service_reset = None @property def simulation_time(self): return int(self.__sim_clock) @property def playback_path(self): """ Gets the playback path """ return self.__playback_path
[docs] def prepare_simulation(self, except_hook=None): """ The CLE will be initialized within this method and ROS services for starting, pausing, stopping and resetting are setup here. :param playback_path: Recorded simulation playback path accessible by gzserver. :param except_hook: A handler method for critical exceptions """ super(PlaybackServer, self).prepare_simulation(except_hook) def clock_callback(t): """ This function is called when there is a new clock message :param t: The current simulation time """ self.__sim_clock = t.clock.secs self.__sim_clock_subscriber = rospy.Subscriber('/clock', Clock, clock_callback) # instantiate the service proxies base = '/gazebo/playback/%s' self.__service_configure = rospy.ServiceProxy(base % 'configure', srv.SimulationPlayback) self.__service_start = rospy.ServiceProxy(base % 'start', Start) self.__service_pause = rospy.ServiceProxy(base % 'pause', Trigger) self.__service_stop = rospy.ServiceProxy(base % 'stop', Trigger) self.__service_reset = rospy.ServiceProxy(base % 'reset', srv.SimulationPlayback) # configure the plugin path and check for failure resp = self.__service_configure(self.__playback_path) if not resp.success: raise Exception('Configuration of playback plugin failed: %s' % resp.message)
def _create_lifecycle(self, except_hook): """ Creates the lifecycle for the current simulation :param except_hook: An exception handler :return: The created lifecycle """ return PlaybackServerLifecycle(self.simulation_id, self, except_hook) def _create_state_message(self): """ Creates a status message :return: A dictionary with status information """ return { 'realTime': int(self.__sim_clock), 'transferFunctionsElapsedTime': {}, 'brainsimElapsedTime': 0, 'robotsimElapsedTime': 0 }
[docs] def process_lifecycle_command(self, command): """ Issue a lifecycle state change as a playback plugin command :param command: One of the supported commands [start, stop, pause] """ # ensure we have configured the plugin before issuing commands if not self.__service_start or not self.__service_stop or not self.__service_pause: raise Exception('Playback server has not been configured, cannot issue commands!') # ensure the command is valid if command not in ['start', 'stop', 'pause']: raise ValueError('Invalid playback command: %s!' % command) # issue the service command try: if command == 'start': resp = self.__service_start() elif command == 'pause': resp = self.__service_pause() elif command == 'stop': resp = self.__service_stop() # check for playback command failure if not resp.success: raise Exception('Playback command failed: %s' % resp.message) # allow stop to fail during crash/shutdown except Exception: # pylint: disable=broad-except if command != 'stop': raise
[docs] def reset_simulation(self, request): """ Reset the playback :param request: the ROS service request message (cle_ros_msgs.srv.ResetSimulation). """ # if uninitialized, we can't reset the path if not self.__playback_path: return False, 'Playback path is not configured, cannot reset!' # reset the internal status clock (probably happens anyway) self.__sim_clock = 0 # perform the reset and (re-)configure resp = self.__service_reset(self.__playback_path) return resp.success, resp.message
[docs] def shutdown(self): """ Shutdown the playback """ super(PlaybackServer, self).shutdown() if self.__sim_clock_subscriber is not None: self.__sim_clock_subscriber.unregister() self.__sim_clock_subscriber = None
[docs]class PlaybackSimulationAssembly(GazeboSimulationAssembly): """ This class is used to realize the assembly of a playback 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(PlaybackSimulationAssembly, self).__init__(sim_config) self.playback = None def _initialize(self, except_hook): """ Internally initialize the simulation :param except_hook: A method that should be called when there is a critical error """ # create the CLE server and lifecycle first to report any failures # properly"Creating Playback Server") self.playback = PlaybackServer( self.sim_config.sim_id, self.sim_config.timeout, self.sim_config.timeout_type, self.gzserver, self.ros_notificator, self.sim_config.playback_path) # prepare robot models so they can be found by gazebo self._prepare_simconfig_robot_models() # start Gazebo simulator and bridge self._start_gazebo(None) # create playback CLE server"Preparing Playback Server") self.playback.prepare_simulation(except_hook) def _shutdown(self, notifications): """ Shutdown the CLE and any hooks before shutting down Gazebo :param notifications: A flag indicating whether notifications should be attempted to send """ try: if notifications: self.ros_notificator.update_task("Shutting down Playback", update_progress=True, block_ui=False) self.robotManager.shutdown() self.playback.shutdown() #pylint: disable=broad-except except Exception as e: logger.error("The cle server could not be shut down") logger.exception(e)
[docs] def run(self): """ Runs the simulation """