roby 0.8.0 → 3.0.0

data/lib/roby/state/open_struct_model.rb

@@ -0,0 +1,68 @@
+module Roby
+    class OpenStructModel < OpenStruct
+        # Returns the superclass, i.e. the state model this is a refinement on
+        attr_reader :superclass
+
+        def __get(name, create_substruct = true, &update)
+            if result = super(name, false, &update)
+                return result
+            elsif superclass && (result = superclass.__get(name, false, &update))
+                if result.kind_of?(OpenStructModel)
+                    return super(name, true, &update)
+                else return result
+                end
+            elsif create_substruct
+                return super
+            end
+        end
+
+        def __respond_to__(name)
+            super || (superclass.__respond_to__(name) if superclass)
+        end
+
+        def create_subfield(name)
+            if superclass
+                self.class.new(superclass.get(name), self, name)
+            else
+                self.class.new(nil, self, name)
+            end
+        end
+
+        def each_member(&block)
+            super(&block)
+            if superclass
+                superclass.each_member do |name, value|
+                    if !@members.has_key?(name)
+                        yield(name, value)
+                    end
+                end
+            end
+        end
+
+        def initialize(super_or_obj = nil, attach_to = nil, attach_name = nil)
+            @superclass = super_or_obj
+            super(nil, attach_to, attach_name)
+        end
+
+        # Base implementation for "leaf" values in an extended struct model
+        class Variable
+            # The name of this leaf in its parent field
+            attr_accessor :name
+
+            # The parent field
+            attr_accessor :field
+
+            def initialize(field = nil, name = nil)
+                @field, @name = field, name
+            end
+
+            # Returns the full path of this leaf w.r.t. the root of the state
+            # structure
+            def path
+                path = field.path.dup
+                path << name
+                path
+            end
+        end
+    end
+end

data/lib/roby/state/pos.rb

@@ -3,13 +3,13 @@ module Roby::Pos
     # A (x, y, z) vector
     class Vector3D
         # The vector coordinates
-	attr_accessor :x, :y, :z
+        attr_accessor :x, :y, :z
         # Initializes a 3D vector
-	def initialize(x = 0, y = 0, z = 0)
-	    @x, @y, @z = x, y, z
-	end
+        def initialize(x = 0, y = 0, z = 0)
+            @x, @y, @z = x, y, z
+        end
 
-	def to_s # :nodoc:
+        def to_s # :nodoc:
             "Vector3D(x=%f,y=%f,z=%f)" % [x,y,z] 
         end
         def pretty_print(pp)
@@ -17,31 +17,31 @@ def pretty_print(pp)
         end
 
         # The length of the vector
-	def length; distance(0, 0, 0) end
+        def length; distance(0, 0, 0) end
         # Returns self + v
-	def +(v); Vector3D.new(x + v.x, y + v.y, z + v.z) end
+        def +(v); Vector3D.new(x + v.x, y + v.y, z + v.z) end
         # Returns self - v
-	def -(v); Vector3D.new(x - v.x, y - v.y, z - v.z) end
+        def -(v); Vector3D.new(x - v.x, y - v.y, z - v.z) end
         # Returns the product of this vector with the scalar +a+
-	def *(a); Vector3D.new(x * a, y * a, z * a) end
+        def *(a); Vector3D.new(x * a, y * a, z * a) end
         # Returns the division of this vector with the scalar +a+
-	def /(a); Vector3D.new(x / a, y / a, z / a) end
+        def /(a); Vector3D.new(x / a, y / a, z / a) end
         # Returns the opposite of this vector
-	def -@; Vector3D.new(-x, -y, -z) end
+        def -@; Vector3D.new(-x, -y, -z) end
 
         # Returns the [x, y, z] array
-	def xyz; [x, y, z] end
+        def xyz; [x, y, z] end
         # True if +v+ is the same vector than +self+
-	def ==(v)
-	    v.kind_of?(Vector3D) &&
-		v.x == x && v.y == y && v.z == z
-	end
+        def ==(v)
+            v.kind_of?(Vector3D) &&
+                v.x == x && v.y == y && v.z == z
+        end
 
         # True if this vector is of zero length. If +tolerance+ is non-zero,
         # returns true if length <= tolerance.
-	def null?(tolerance = 0)
-	    length <= tolerance
-	end
+        def null?(tolerance = 0)
+            length <= tolerance
+        end
 
         # call-seq:
         #   v.distance2d w
@@ -53,15 +53,15 @@ def null?(tolerance = 0)
         # If +w+ is a scalar, it is taken as the X coordinate and y = 0.
         #
         # In the second form, both +x+ and +y+ must be scalars.
-	def distance2d(x = 0, y = nil)
-	    if !y && x.respond_to?(:x)
-		x, y = x.x, x.y
-	    else
-		y ||= 0
-	    end
+        def distance2d(x = 0, y = nil)
+            if !y && x.respond_to?(:x)
+                x, y = x.x, x.y
+            else
+                y ||= 0
+            end
 
-	    Math.sqrt( (x - self.x) ** 2 + (y - self.y) ** 2 )
-	end
+            Math.sqrt( (x - self.x) ** 2 + (y - self.y) ** 2 )
+        end
 
         # call-seq:
         #   v.distance2d w
@@ -74,35 +74,35 @@ def distance2d(x = 0, y = nil)
         # If +w+ is a scalar, it is taken as the X coordinate and y = z = 0.
         #
         # In the second form, both +x+ and +y+ must be scalars and z == 0.
-	def distance(x = 0, y = nil, z = nil)
-	    if !y && x.respond_to?(:x)
-		x, y, z = x.x, x.y, x.z
-	    else
-		y ||= 0
-		z ||= 0
-	    end
+        def distance(x = 0, y = nil, z = nil)
+            if !y && x.respond_to?(:x)
+                x, y, z = x.x, x.y, x.z
+            else
+                y ||= 0
+                z ||= 0
+            end
 
-	    Math.sqrt( (x - self.x) ** 2 + (y - self.y) ** 2 + (z - self.z) ** 2)
-	end
+            Math.sqrt( (x - self.x) ** 2 + (y - self.y) ** 2 + (z - self.z) ** 2)
+        end
     end
 
     # This class represents both a position and an orientation
     class Euler3D < Vector3D
         # The orientation angles
-	attr_accessor :yaw, :pitch, :roll
+        attr_accessor :yaw, :pitch, :roll
 
         # Create an euler position object
-	def initialize(x = 0, y = 0, z = 0, yaw = 0, pitch = 0, roll = 0)
-	    super(x, y, z)
-	    @yaw, @pitch, @roll = yaw, pitch, roll
-	end
+        def initialize(x = 0, y = 0, z = 0, yaw = 0, pitch = 0, roll = 0)
+            super(x, y, z)
+            @yaw, @pitch, @roll = yaw, pitch, roll
+        end
 
         # Returns [yaw, pitch, roll]
-	def ypr
-	    [yaw, pitch, roll]
-	end
+        def ypr
+            [yaw, pitch, roll]
+        end
 
-	def to_s # :nodoc:
+        def to_s # :nodoc:
             "Euler3D(x=%f,y=%f,z=%f,y=%f,p=%f,r=%f)" % [x,y,z,yaw,pitch,roll]
         end
     end

data/lib/roby/state/shapes.rb

@@ -4,11 +4,11 @@ def initialize(radius, height, axis)
         @radius, @height, @axis = radius.to_f, height.to_f, axis.to_f
     end
     def diameter(axis)
-	if axis == self.axis
-	    radius * 2
-	else
-	    raise NotImplementedError
-	end
+        if axis == self.axis
+            radius * 2
+        else
+            raise NotImplementedError
+        end
     end
     alias :max_length :diameter 
     def length; diameter(:z) end 
@@ -18,15 +18,15 @@ def width; diameter(:z) end
 class Cube
     attr_accessor :length, :width, :height
     def initialize(length, width, height)
-	@length, @width, @height = length.to_f, width.to_f, height.to_f
+        @length, @width, @height = length.to_f, width.to_f, height.to_f
     end
 
     def max_length(axis)
-	if axis == :z
-	    [length, width].max
-	else
-	    height
-	end
+        if axis == :z
+            [length, width].max
+        else
+            height
+        end
     end
 end
 

data/lib/roby/state/state_model.rb

@@ -0,0 +1,303 @@
+class Class
+    # Implementation of a conversion to StateVariableModel
+    #
+    # It makes it possible to use a class to initialize a state leaf model with
+    #
+    #   state.model.path.to.value = MyClass
+    def to_state_variable_model(field, name)
+        model = Roby::StateVariableModel.new(field, name)
+        model.type = self
+        return model
+    end
+end
+
+module Roby
+    # Representation of a leaf in the state model
+    class StateVariableModel < OpenStructModel::Variable
+        # Returns the type of this field. nil means any type. It is matched
+        # against values using #===
+        attr_accessor :type
+
+        # Returns a data source for that field
+        #
+        # At this level, the format data source is unspecified. It is meant to
+        # be used by other mechanisms to fill the data_source tree in the state
+        # object (this is the state model)
+        attr_accessor :data_source
+
+        def to_state_variable_model(field, name)
+            result = dup
+            result.field = field
+            result.name = name
+            result
+        end
+    end
+
+    # Representation of a level in the state model
+    class StateModel < OpenStructModel
+        # Returns the superclass, i.e. the state model this is a refinement on
+        attr_reader :superclass
+
+        def to_s
+            "#<StateModel:#{object_id} path=#{path.join("/")} fields=#{@members.keys.sort.join(",")}>"
+        end
+
+        def initialize(super_or_obj = nil, attach_to = nil, attach_name = nil)
+            super(super_or_obj, attach_to, attach_name)
+            global_filter do |name, value|
+                if value.respond_to?(:to_state_variable_model)
+                    value.to_state_variable_model(self, name)
+                else
+                    raise ArgumentError, "cannot set #{value} on #{name} in a state model. Only allowed values are StateVariableModel, and values that respond to #to_state_variable_model"
+                end
+            end
+        end
+
+        # This methods iterates over the state model, and for each state
+        # variable for which a data source model is provided, create the
+        # corresponding data source by calling #resolve
+        def resolve_data_sources(object, state)
+            each_member do |name, field|
+                if field.respond_to?(:data_source)
+                    state.data_sources.set(name, field.data_source.resolve(object))
+                else
+                    field.resolve_data_sources(object, state.__get(name, true))
+                end
+            end
+        end
+    end
+
+    # Representation of the last known state
+    class StateLastValueField < OpenStruct
+        def method_missing(name, *args)
+            if name =~ /=$/
+                raise ArgumentError, "cannot write to a StateLastValueField object"
+            end
+            return super
+        end
+    end
+
+    # Representation of the data sources in the state
+    class StateDataSourceField < OpenStruct
+    end
+
+    # Representation of a level in the current state
+    class StateField < OpenStruct
+        def to_s
+            "#<StateField:#{object_id} path=#{path.join("/")} fields=#{@members.keys.sort.join(",")}>"
+        end
+
+        # Returns a structure that gives access to the models of the
+        # members of this struct. I.e.
+        #
+        #   state.pose.model.position
+        #   
+        # is the model for state.pose.position
+        #
+        # Due to the use of open structures, one should always check for the
+        # presence of the field first with
+        #
+        #   if state.pose.model.position?
+        #      # do something with state.pose.model.position
+        #   end
+        #
+        # Note that the models are accessible from any level, i.e.
+        # state.model.pose.position is an equivalent of the above example.
+        def model
+            @model
+        end
+
+        # Returns a structure that gives access to the last known values for the
+        # members of this struct. I.e.
+        #
+        #   state.pose.last_known.position
+        #   
+        # is the last value known for state.pose.position
+        #
+        # Note that the last known values are accessible from any level, i.e.
+        # state.last_known.pose.position is an equivalent of the above example.
+        def last_known
+            @last_known
+        end
+
+
+        # Returns a structure that gives access to the data sources for the
+        # members of this struct. I.e.
+        #
+        #   state.pose.data_sources.position
+        #   
+        # will give the data source for state.pose.position if there is one.
+        #
+        # Due to the use of open structures, one should always check for the
+        # presence of the field first with
+        #
+        #   if state.pose.data_sources.position?
+        #      # do something with state.pose.data_sources.position
+        #   end
+        #
+        # Note that the models are accessible from any level, i.e.
+        # state.model.pose.position is an equivalent of the above example.
+        def data_sources
+            @data_sources
+        end
+
+        def initialize(model = nil, attach_to = nil, attach_name = nil)
+            if !attach_to
+                # We are root, initialize last_known and data_sources
+                @last_known = StateLastValueField.new
+                @data_sources = StateDataSourceField.new
+            end
+
+            super(model, attach_to, attach_name)
+
+            if model
+                # If we do have a model, verify that the assigned values match
+                # the model's type
+                global_filter do |name, value|
+                    if (field_model = model.get(name)) && (field_type = field_model.type)
+                        if !(field_type === value)
+                            raise ArgumentError, "field #{name} is expected to have values of type #{field_type.name}, #{value} is of type #{value.class}"
+                        end
+                    end
+                    value
+                end
+            end
+        end
+
+        def link_to(parent, name)
+            super
+            @last_known = parent.last_known.get(name) ||
+                StateLastValueField.new(nil, parent.last_known, name)
+            @data_sources = parent.data_sources.get(name) ||
+                StateDataSourceField.new(nil, parent.data_sources, name)
+        end
+
+        def attach
+            super
+            @last_known.attach
+            @data_sources.attach
+        end
+
+        # Reimplemented from OpenStruct
+        def method_missing(name, *args)
+            if name =~ /(.*)=$/
+                if data_source = data_sources.get($1)
+                    raise ArgumentError, "cannot explicitely set a field for which a data source exists"
+                end
+            end
+            super
+        end
+
+        # Reimplemented from OpenStruct
+        #
+        # It disables automatic substruct creation for state variables for which
+        # a data source exists
+        def __get(name, create_substruct = true)
+            if (source = data_sources.get(name)) && !source.kind_of?(StateDataSourceField)
+                # Don't create a substruct, we know that this subfield should be
+                # populated by the data source
+                create_substruct = false
+            end
+            return super(name, create_substruct)
+        end
+
+        # Read each subfield that have a source, and update both their
+        # last_known and current value.
+        def read
+            data_sources.each_member do |field_name, field_source|
+                new_value = field_source.read
+                set(field_name, new_value)
+                if new_value
+                    last_known.set(field_name, new_value)
+                end
+            end
+        end
+
+        def create_model
+            StateModel.new
+        end
+    end
+
+    # Implementation of the state representation at runtime.
+    #
+    # It gives access to three views to the state:
+    #  * the current values are directly accessible from this state object
+    #  * the last known value is stored in last_known.path.to.value
+    #  * the state model is stored in model.path.to.value
+    #  * the current data source for a state variable is stored in
+    #    data_sources.path.to.value
+    class StateSpace < StateField
+        def initialize(model = nil)
+            @exported_fields = nil
+            super(model)
+        end
+
+        # Declares that no state fields should be marshalled. The default is to
+        # export everything
+        #
+        # It cancels any list of fields exported with #export
+        #
+        # See also #export_all and #export
+        def export_none
+            @exported_fields = Set.new
+        end
+
+        # Declares that all the state fields should be marshalled. This is the
+        # default
+        #
+        # It cancels any list of fields exported with #export
+        #
+        # See also #export_none and #export
+        def export_all
+            @exported_fields = nil
+        end
+
+        # Declares that only the given names should be marshalled, instead of
+        # marshalling every field. It is cumulative, i.e. if multiple calls to
+        # #export follow each other then the fields get added to the list of
+        # exported fields instead of replacing it.
+        #
+        # If #export_all has been called, a call to #export cancels it.
+        #
+        # See also #export_none and #export_all
+        def export(*names)
+            @exported_fields ||= Set.new
+            @exported_fields.merge names.map { |n| n.to_s }.to_set
+        end
+
+        def create_subfield(name)
+            model = if self.model then self.model.get(name) end
+            StateField.new(model, self, name)
+        end
+
+        # Implementation of marshalling with Ruby's Marshal
+        #
+        # Only the fields that can be marshalled will be saved. Any other field
+        # will silently be ignored.
+        #
+        # Which fields get marshalled can be controlled with #export_all,
+        # #export_none and #export. The default is to marshal all fields.
+        def _dump(lvl = -1)
+            if !@exported_fields
+                super
+            else
+                marshalled_members = @exported_fields.map do |name|
+                    value = @members[name]
+                    [name, Marshal.dump(value)] rescue nil
+                end
+                marshalled_members.compact!
+                Marshal.dump([marshalled_members, @aliases])
+            end
+        end
+
+        def deep_copy
+            exported_fields, @exported_fields = @exported_fields, Set.new
+            Marshal.load(Marshal.dump(self))
+        ensure
+            @exported_fields = exported_fields
+        end
+
+        def testing?; Roby.app.testing? end
+        def simulation?; Roby.app.simulation? end
+    end
+end