roby 0.7

data/plugins/fault_injection/Rakefile

@@ -0,0 +1,18 @@
+rdoc_base_dir = Dir.pwd
+if ENV['ROBY_ROOT_DIR']
+    $LOAD_PATH << File.join(ENV['ROBY_ROOT_DIR'], 'lib')
+    rdoc_base_dir = ENV['ROBY_ROOT_DIR']
+end
+require 'rake/rdoctask'
+require 'roby/app/rake'
+
+Rake::RDocTask.new("docs") do |rdoc|
+  rdoc.options << "--inline-source" << "--accessor" << "attribute" << "--accessor" << "attr_predicate"
+  rdoc.rdoc_dir = "#{rdoc_base_dir}/doc/rdoc/plugins/fault_injection"
+  rdoc.title    = "Fault injection plugin for Roby"
+  rdoc.template = Roby::Rake.rdoc_template
+  rdoc.options << '--main' << 'README.txt'
+  rdoc.rdoc_files.include('README.txt', 'TODO.txt', 'History.txt')
+  rdoc.rdoc_files.include('fault_injection.rb', 'app.rb')
+end
+

data/plugins/fault_injection/app.rb

@@ -0,0 +1,52 @@
+
+module Roby
+    # Fault injection plugin for Roby
+    #
+    # Roby::FaultInjection::Application#fault_models defines a set of
+    # probabilistic models of emission for task events. It maps task model
+    # events to emission models, and during execution will randomly emit
+    # events.
+    #
+    # Fault injection applies only on tasks which are running and interruptible.
+    module FaultInjection
+	# This module gets included in Roby::Application when the plugin is activated
+	module Application
+	    # The set of fault models defined for this application. More specifically,
+	    # the emission model for an event +ev+ of a task model +model+ is given by
+	    #
+	    #   emission_model = Roby.app.fault_models[model][ev]
+	    #
+	    # where +ev+ is a symbol.
+	    attribute(:fault_models) do
+		Hash.new { |h, k| h[k] = Hash.new }
+	    end
+
+	    # call-seq:
+	    #   Roby.app.fault_model task_model, emission_model, ev1, ev2, ...
+	    #
+	    # Defines +emission_model+ as the emission model for the listed
+	    # events of task +task_model+. +ev1+, +ev2+ are symbols.
+	    #
+	    # Emission models are objects which must respond to #fault?(task).
+	    # If this predicate returns true for the running task +task+, then
+	    # the emission of the tested event will be simulated. The emission
+	    # models are tested every one second.
+	    #
+	    # See Roby::FaultInjection::Rate for an example.
+	    def add_fault_model(task_model, *args)
+		fault_model = args.pop
+		args.each do |ev|
+		    fault_models[task_model][ev.to_sym] = fault_model
+		end
+	    end
+	end
+    end
+
+    Application.register_plugin('fault_injection', Roby::FaultInjection::Application) do
+	require 'fault_injection'
+	Roby.every(1) do
+	    FaultInjection.apply(Roby.app.fault_models)
+	end
+    end
+end
+

data/plugins/fault_injection/fault_injection.rb

@@ -0,0 +1,89 @@
+module Roby
+    class Task
+	# Returns for how many seconds this task is running.  Returns nil if
+	# the task is not running.
+	def lifetime
+	    if running?
+		Time.now - history.first.time
+	    end
+	end
+    end
+
+    module FaultInjection
+	extend Logger::Hierarchy
+	extend Logger::Forward
+	
+	# This fault model is based on a constant fault rate, defined upon the
+	# mean-time-to-failure of the task.
+	#
+	# In this model, the probability of having a fault for task t at any
+	# given instant is
+	#
+	#   p(fault) = max * (1.0 - exp(- task.lifetime / mttf))
+	#
+	class Rate
+	    attr_reader :mttf, :max
+	    def initialize(mttf, max = 1.0)
+		@mttf, @max = mttf, max
+	    end
+
+	    def fault?(task)
+		f = max * (1.0 - Math.exp(- task.lifetime / mttf))
+		rand <= f
+	    end
+	end
+
+	# Apply the given fault models to the main Roby plan
+	def self.apply(fault_models)
+	    injected_faults = Array.new
+
+	    for model, faults in fault_models
+		for ev, p in faults
+		    Roby.plan.find_tasks(model).
+			running.not_finishing.
+			interruptible.each do |task|
+			    if p.fault?(task)
+				FaultInjection.info "injecting fault #{ev} on #{task}"
+				injected_faults << [task, ev, task.inject_fault(ev)]
+			    end
+			end
+		end
+	    end
+
+	    injected_faults
+	end
+    end
+
+    class Task
+	# Makes the plan behave as if this task had failed by emitting the
+	# event +event+ with context +context+.
+	#
+	# The task must be running and interruptible, and +event+ must be
+	# terminal. The default implementation creates a duplicate of this task
+	# by using #dup and forwards the +stop+ event of the old task to
+	# +event+ on the new one.
+	#
+	# It returns a new task which replaced this one in the plan. This is
+	# the task on which +event+ will actually be emitted.
+	def inject_fault(event, context = nil)
+	    if !running?
+		raise ArgumentError, "this task is not running"
+	    elsif finishing?
+		raise ArgumentError, "this task is being stopped"
+	    elsif !interruptible?
+		raise ArgumentError, "fault injection works only on interruptible tasks"
+	    elsif !event(event).terminal?
+		raise ArgumentError, "the injected fault must be a terminal event"
+	    end
+
+	    new_task = dup
+	    old_task = self
+
+	    plan.replace_task(old_task, new_task)
+	    old_task.event(:stop).filter(context).forward new_task.event(event)
+
+	    new_task
+	end
+    end
+end
+

data/plugins/fault_injection/test/test_fault_injection.rb

@@ -0,0 +1,84 @@
+$LOAD_PATH.unshift File.expand_path( '..', File.dirname(__FILE__))
+require 'test/unit'
+require 'roby'
+require 'roby/test/common'
+require 'roby/test/tasks/simple_task'
+require 'fault_injection'
+
+class TC_FaultInjection < Test::Unit::TestCase
+    include Roby::Test
+
+    def test_inject_fault
+	Roby.control.run :detach => true
+
+	task = nil
+	Roby.execute do
+	    model = Class.new(Roby::Test::SimpleTask) do
+		event :specialized_fault
+		forward :specialized_fault => :failed
+	    end
+
+	    plan.permanent(task = model.new)
+	    assert_raises(ArgumentError) { task.inject_fault(:start) }
+
+	    task.start!
+	end
+	Roby.wait_one_cycle
+
+	fake_task = nil
+	Roby.execute do
+	    assert_raises(ArgumentError) { task.inject_fault(:updated_data) }
+	    assert_nothing_raised { task.inject_fault(:specialized_fault) }
+	    assert_equal(2, plan.known_tasks.size)
+	    fake_task = plan.known_tasks.find { |t| t != task }
+	end
+	Roby.wait_one_cycle
+
+	assert(fake_task.finished?)
+	assert(fake_task.event(:specialized_fault).happened?)
+    end
+
+    def test_apply
+	Roby.control.run :detach => true
+
+	model = Class.new(Roby::Test::SimpleTask) do
+	    event :specialized_fault
+	    forward :specialized_fault => :failed
+	end
+
+	fault_models = Hash.new { |h, k| h[k] = Hash.new }
+	fault_models[model][:specialized_fault] = FaultInjection::Rate.new(0.01, 1.0)
+	fault_models[Roby::Test::SimpleTask][:stop] = FaultInjection::Rate.new(1_000_000, 1.0)
+	
+	simple, specialized = nil
+	Roby.execute do
+	    plan.permanent(simple = Roby::Test::SimpleTask.new)
+	    plan.permanent(specialized = model.new)
+	    simple.start!
+	    specialized.start!
+	end
+	Roby.wait_one_cycle
+
+	sleep(0.5)
+	fake_specialized = nil
+	Roby.execute do
+	    result = Roby::FaultInjection.apply(fault_models)
+	    assert_equal(1, result.size)
+	    fake_specialized = result.first.last
+	end
+	Roby.wait_one_cycle
+
+	assert(simple.running?)
+	assert(specialized.finished?)
+	assert(fake_specialized.finished?)
+	assert(fake_specialized.event(:specialized_fault).happened?)
+    end
+
+    def test_task_lifetime
+	plan.discover(task = Roby::Test::SimpleTask.new)
+	task.start!
+	sleep(0.5)
+	assert(task.lifetime > 0.5)
+    end
+end
+

data/plugins/subsystems/README.txt

@@ -0,0 +1,40 @@
+
+http://rubyforge.org/projects/roby
+
+= Subsystems plugin for Roby
+
+This is a plugin for the Roby robotic control framework. It allows to generate
+an initial plan in which a set of common subsystems (sensors, localization,
+...) are already initialized, easing later the writing and management of the
+planning methods for other, more dynamical actions.
+
+== Usage
+
+A set of services can be defined on <tt>State.services</tt> like this:
+
+  State.services.localization = 'pom'
+  State.services.camera = 'cam'
+
+Then, at controller initialization and before the controller file is actually
+loaded, the corresponding planning methods are called to generate a plan in which those
+subsystems are present:
+
+  planner.localization(:id => 'pom')
+  planner.camera(:id => 'cam')
+
+The returned tasks are also started, in order. The controller file gets loaded
+only when the subsystems are ready (i.e. their start events are emitted).
+
+== Enabling the plugin
+
+The plugin must be enabled in the configuration files (i.e. either in
+<tt>config/init.rb</tt> or in one of the robot-specific files) through a call
+to Roby.app.using:
+
+  Roby.app.using 'subsystems'
+
+== License:
+
+This work is licensed under the CeCiLL-B license, which is a free software
+license similar to the BSD license. See License.txt for details.
+

data/plugins/subsystems/Rakefile

@@ -0,0 +1,18 @@
+rdoc_base_dir = Dir.pwd
+if ENV['ROBY_ROOT_DIR']
+    $LOAD_PATH << File.join(ENV['ROBY_ROOT_DIR'], 'lib')
+    rdoc_base_dir = ENV['ROBY_ROOT_DIR']
+end
+require 'rake/rdoctask'
+require 'roby/app/rake'
+
+Rake::RDocTask.new("docs") do |rdoc|
+  rdoc.options << "--inline-source" << "--accessor" << "attribute" << "--accessor" << "attr_predicate"
+  rdoc.rdoc_dir = "#{rdoc_base_dir}/doc/rdoc/plugins/subsystems"
+  rdoc.title    = "Subsystems plugin for Roby"
+  rdoc.template = Roby::Rake.rdoc_template
+  rdoc.options << '--main' << 'README.txt'
+  rdoc.rdoc_files.include('README.txt')
+  rdoc.rdoc_files.include('app.rb')
+end
+

data/plugins/subsystems/app.rb

@@ -0,0 +1,171 @@
+
+module Roby
+    # Subsystem plugin for Roby
+    #
+    # This plugin manages a set of services that should be available at all
+    # times (for instance: localization). For now, it makes Roby start them at
+    # initialization time, allowing to consider that they are always available
+    # during plan generation afterwards.
+    #
+    # == Subsystems at initialization
+    # The set of subsystems that should be considered is to be set through the State.services
+    # configuration object:
+    #
+    #   State.services do |sys|
+    #	  sys.localization = 'pom'
+    #	  sys.laser_ranges = 'sick'
+    #	end
+    #
+    # The subsystem plugin will then use <tt>MainPlanner#subsystem_name(:id =>
+    # 'subsystem_selected')</tt> to generate the subsystems' plans. In the
+    # example above, the initial plan will be made using
+    #
+    #   planner.localization(:id => 'pom')
+    #   planner.laser_ranges(:id => 'sick')
+    #
+    # The resulting plan is then started one subsystem after the other
+    # <b>before</b> the controller is loaded
+    #
+    # == Subsystems and state update
+    #
+    # If the main tasks representing the subsystem -- the one returned by the
+    # planning methods, respond to update_<sysname>(state), that method is
+    # called, provided that the task is running, at every cycle with the
+    # system's state representation as an argument.
+    #
+    # For instance, the Localization task which represents the localization
+    # subsystems can have a #update_localization method which takes an
+    # argument. This method would update at each cycle the system's position in
+    # the state representation.
+    module Subsystems
+	module Application
+	    def self.setup_main_planner
+		MainPlanner.class_eval do
+		    Roby::State.services.each_member do |name, value|
+			filter(name) do |options, method|
+			    options[:id] || method.id.to_s == value.to_s
+			end
+		    end
+		end
+	    end
+
+	    # This method generates the initial plan built upon the set of
+	    # required subsystems.
+	    def self.initialize_plan
+		setup_main_planner
+		plan = Transaction.new(Roby.plan)
+
+		# Event which is emitted when all subsystems are properly
+		# initialized
+		ready = AndGenerator.new
+
+		# Create the one task for each subsystem. +ready+ is an event
+		# which is fired when all subsystems are properly initialized
+		planner = MainPlanner.new(plan)
+
+		tasks = Array.new
+		task_objects = ValueSet.new
+		State.services.each_member do |name, value|
+		    next if name == 'tasks'
+		    new_task = begin
+				   planner.send(name)
+			       rescue Planning::NotFound => e
+				   raise RuntimeError, e.full_message
+			       end
+
+		    State.services.tasks.send("#{name}=", new_task)
+
+		    plan.permanent(new_task)
+		    started_with = if new_task.has_event?(:ready) then :ready
+				   else :start
+				   end
+
+		    ready << new_task.event(started_with)
+
+		    new_task.on(started_with) do
+			Robot.info "#{name} subsystem started (#{value})"
+		    end
+		    new_task.on(:stop) do
+			Robot.info "#{name} subsystem stopped (#{value})"
+		    end
+
+		    tasks << [name, value, new_task]
+		    task_objects << new_task
+		end
+
+		# We make sure each subsystem is started only when all the
+		# other subsystems it depends on are started and ready.
+		# +start_with+ is the event which should be called to start
+		# everything
+		starting_point = EventGenerator.new(true)
+		tasks.each do |name, type, task|
+		    children = task.generated_subgraph(TaskStructure::Hierarchy) & task_objects
+		    children.delete(task)
+
+		    ev = if children.empty?
+			     starting_point
+			 else
+			     children.inject(AndGenerator.new) do |ev, child|
+				 started_with = if child.has_event?(:ready) then :ready
+						else :start
+						end
+				 ev << child.event(started_with)
+			     end
+			 end
+
+		    ev.on { Robot.info "starting subsystem #{name} (#{type})" }
+		    ev.on task.event(:start)
+		end
+
+		Roby.execute do
+		    plan.commit_transaction
+		end
+		[starting_point, ready]
+	    end
+
+	    # Hook to generate the initial plan and start subsystems when the
+	    # application starts.
+	    def self.run(config, &block)
+		unless Roby::State.services? && !Roby::State.services.empty?
+		    Robot.info "No subsystems defined"
+		    return yield
+		end
+
+		Robot.info "Starting subsystems ..."
+
+		starting_point, ready = initialize_plan
+		# Start the deepest tasks. The signalling order will do the rest.
+		# The 'ready' event is emitted when all the subsystem tasks are
+		Roby.wait_until(ready) do
+		    Roby.execute do
+			starting_point.call
+		    end
+		end
+
+		yield
+	    end
+	end
+    end
+
+    Application.register_plugin('subsystems', Roby::Subsystems::Application) do
+	# Call the #update_ methods for the subsystems tasks which define it.
+	Roby.each_cycle do
+	    srv = Roby::State.services
+	    srv.each_member do |name, value|
+		task = srv.tasks.send(name)
+		next unless task.running?
+		if task.has_event?(:ready)
+		    next unless task.event(:ready).happened?
+		end
+
+		if task.respond_to?("update_#{name}")
+		    begin
+			task.send("update_#{name}", State)
+		    rescue Exception => e
+			Roby.warn "update_#{name} failed on #{task} with #{e}"
+		    end
+		end
+	    end
+	end
+    end
+end