rclrb 1.1.0

data/lib/rclrb/fields.rb

@@ -0,0 +1,224 @@
+module Rclrb
+  ##
+  # This module contains classes used to handle ROS messages.
+  # The content of Fields is internal API to rclrb and should not be directly called and is subject
+  # to change.
+  module Fields
+    # This function is used for building generic parse_ros_message_array function, that will read array elements one-by-one and call block to parse them
+    def Fields.__generic_parse_array data, length, field_size, &block
+      array = []
+      for i in 0...length
+        ptr = data + i * field_size
+        array.append block.call(ptr)
+      end
+      return array
+    end
+    # This function is used for building generic fill_ros_message_array function, that will write array elements one-by-one and call block to add them
+    def Fields.__generic_fill_ros_message_array array_pointer, value, field_size, &block
+      for i in 0...value.length
+        block.call array_pointer + i * field_size, value[i]
+      end
+    end
+
+
+    class AtomicField
+      attr_reader :init_value, :field_name, :ffi_type
+      def initialize(init_value, field_name, ffi_type, array_reader, array_writter)
+        @init_value = init_value
+        @field_name    = "Rclrb::Fields::#{field_name}"
+        @ffi_type      = ffi_type
+        @memory_size   = FFI::type_size @ffi_type
+        @array_reader  = array_reader
+        @array_writter = array_writter
+      end
+      def array_field()
+        @array_field = ArrayField.new(self) unless defined?(@array_field)
+        return @array_field
+      end
+      def parse_ros_message(data)
+        return data
+      end
+      def parse_ros_message_array(data, length)
+        if @array_reader
+          return data.send @array_reader, 0, length
+        else
+          return Fields.__generic_parse_array(data, length, self.ffi_size) { |ptr| ptr.get(@ffi_type, 0) }
+        end
+      end
+      def fill_ros_message(data, index, value)
+        data[index] = value
+      end
+      def fill_ros_message_array(array_pointer, value)
+        if @array_writter
+          return array_pointer.send @array_writter, 0, value
+        else
+          Fields.__generic_fill_ros_message_array(array_pointer, value, self.ffi_size) { |ptr, value| ptr.put @ffi_type, 0, value }
+        end
+      end
+      def destroy_ros_message(data)
+      end
+      def ffi_size()
+        return @memory_size
+      end
+      def is_atomic
+        return true
+      end
+    end
+    class StringField
+      def StringField.init_value()
+        return "\"\""
+      end
+      def StringField.field_name()
+        return "Rclrb::Fields::StringField"
+      end
+      def StringField.parse_ros_message(data)
+        length =  data[:length]
+        return data[:string].get_string(0, length)
+      end
+      def StringField.parse_ros_message_array(data, length)
+        return Fields.__generic_parse_array(data, length, StringField.ffi_size) { |ptr| StringField.parse_ros_message FFIType.new(ptr) }
+      end
+      def StringField.fill_ros_message(data, index, value)
+        st = data[index]
+        str_pointer = Rclrb.rcl_allocate value.length + 1
+        st[:string] = str_pointer
+        str_pointer.put_string(0, value)
+        str_pointer.put_uint8(value.length, 0)
+        st[:length] = value.length
+        st[:capacity] = value.length + 1
+      end
+      def StringField.destroy_ros_message(data)
+        if data.kind_of? FFIType
+          Rclrb.rcl_deallocate data[:string]
+        else
+          return StringField.destroy_ros_message FFIType.new(data)
+        end
+      end
+      def StringField.array_field()
+        @@array_field = ArrayField.new(StringField) unless defined?(@@array_field)
+        return @@array_field
+      end
+      def StringField.ffi_type
+        return FFIType
+      end
+      def StringField.ffi_size
+        return FFIType.size
+      end
+      def StringField.is_atomic
+        return false
+      end
+      class FFIType < FFI::Struct
+        layout :string, :pointer, :length, :size_t, :capacity, :size_t
+      end
+    end
+    class TimeField
+      def TimeField.init_value()
+        return "Rclrb::Time.new()"
+      end
+      def TimeField.field_name()
+        return "Rclrb::Fields::TimeField"
+      end
+      def TimeField.parse_ros_message(data)
+        return Time.from_sec_nsec data[:sec], data[:nsec]
+      end
+      def TimeField.fill_ros_message(data, index, value)
+        st = data[index]
+        s, ns = value.to_sec_nsec
+        st[:sec] = s
+        st[:nsec] = ns
+      end
+      def TimeField.destroy_ros_message(data)
+      end
+      def TimeField.array_field()
+        @@array_field = ArrayField.new(TimeField) unless defined?(@@array_field)
+        return @@array_field
+      end
+      def TimeField.ffi_type
+        return FFIType
+      end
+      def TimeField.ffi_size
+        return FFIType.size
+      end
+      def TimeField.is_atomic
+        return false
+      end
+      class FFIType < FFI::Struct
+        layout :sec, :int32, :nsec, :uint32
+      end
+    end
+    class ArrayField
+      def initialize(field)
+        @field = field
+      end
+      def init_value()
+        return "[]"
+      end
+      def field_name()
+        return "#{@field.field_name}.array_field()"
+      end
+      def parse_ros_message(data)
+        return @field.parse_ros_message_array data[:array], data[:length]
+      end
+      def fill_ros_message(data, index, value)
+        array_pointer = Rclrb.rcl_allocate value.length * @field.ffi_size
+        @field.fill_ros_message_array array_pointer, value
+        arr = data[index]
+        arr[:array] = array_pointer
+        arr[:length] = value.length
+        arr[:capacity] = value.length
+      end
+      SIZE = CApi::POINTER_SIZE + 2 * CApi::SIZE_T_SIZE
+      def ros_size()
+        return SIZE
+      end
+      def destroy_ros_message(data)
+        length =  data[:length]
+        unless @field.is_atomic
+          for i in 0...length
+            @field.destroy_ros_message data[:array] + i * @field.ffi_size
+          end
+        end
+        Rclrb.rcl_deallocate data[:array]
+      end
+      def is_atomic
+        return false
+      end
+      def ffi_type
+        return FFIType
+      end
+      class FFIType < FFI::Struct
+        layout :array, :pointer, :length, :size_t, :capacity, :size_t
+      end
+    end
+    Float64Field = AtomicField.new "0.0", "Float64Field", :double, :get_array_of_float64, :put_array_of_float64
+    Float32Field = AtomicField.new "0.0", "Float32Field", :float, :get_array_of_float32, :put_array_of_float32
+    ByteField = AtomicField.new "0", "ByteField", :uint8, :get_array_of_uint8, :put_array_of_uint8
+    CharField = AtomicField.new "0", "CharField", :uint8, :get_array_of_uint8, :put_array_of_uint8
+    BoolField = AtomicField.new "false", "BoolField", :bool, nil, nil
+    Int8Field = AtomicField.new "0", "Int8Field", :int8, :get_array_of_int8, :put_array_of_int8
+    Int16Field = AtomicField.new "0", "Int16Field", :int16, :get_array_of_int16, :put_array_of_int16
+    Int32Field = AtomicField.new "0", "Int32Field", :int32, :get_array_of_int32, :put_array_of_int32
+    Int64Field = AtomicField.new "0", "Int64Field", :int64, :get_array_of_int64, :put_array_of_int64
+    Uint8Field = AtomicField.new "0", "Uint8Field", :uint8, :get_array_of_uint8, :put_array_of_uint8
+    Uint16Field = AtomicField.new "0", "Uint16Field", :uint16, :get_array_of_uint16, :put_array_of_uint16
+    Uint32Field = AtomicField.new "0", "Uint32Field", :uint32, :get_array_of_uint32, :put_array_of_uint32
+    Uint64Field = AtomicField.new "0", "Uint64Field", :uint64, :get_array_of_uint64, :put_array_of_uint64
+
+    AtomicFields = { "float64" => Float64Field,
+        "float32" => Float32Field,
+        "char" => CharField,
+        "byte" => ByteField,
+        "bool" => BoolField,
+        "int8" => Int8Field,
+        "int16" => Int16Field,
+        "int32" => Int32Field,
+        "int64" => Int64Field,
+        "uint8" => Uint8Field,
+        "uint16" => Uint16Field,
+        "uint32" => Uint32Field,
+        "uint64" => Uint64Field,
+        "string" => StringField,
+        "builtin_interfaces/Time"   => TimeField }
+
+  end
+end

data/lib/rclrb/future.rb

@@ -0,0 +1,82 @@
+module Rclrb
+  #
+  # Define a future
+  #
+  # 
+  class Future
+    ##
+    # Create a future with a nil result
+    def initialize
+      @result = nil
+      @observers = []
+      @mutex = Mutex.new
+      @resource = ConditionVariable.new
+      @exception = nil
+    end
+    ##
+    # Execute the block in a thread and return a Future.
+    # The block is expected to return a value which is set using set_value.
+    def Future.execute(&block)
+      f = Future.new
+      Thread.new do
+        begin
+          f.set_result block.call
+        rescue => ex
+          f.set_exception ex
+        end
+      end
+      return f
+    end
+    ##
+    # Add an observer, which is executed once the result is set.
+    def add_observer(&block)
+      @mutex.synchronize do
+        if @result.nil?
+          @observers.push block
+        else
+          block.call @result
+        end
+      end
+    end
+    ##
+    # Wait undefinitely for a result.
+    def wait_for_result()
+      @mutex.synchronize do
+        while @result.nil?
+          raise @exception unless @exception.nil?
+          @resource.wait(@mutex)
+        end
+      end
+    end
+    ##
+    # Returns true if it already has a result.
+    def has_result?
+      return not(@result.nil?)
+    end
+    ##
+    # Set that an exception occurs
+    def set_exception(ex)
+      @mutex.synchronize do
+        @exception = ex
+        @resource.signal
+      end
+    end
+    ##
+    # Set the result of the future
+    def set_result(result)
+      raise RclError.new "Result is already set" unless @result.nil?
+      @mutex.synchronize do
+        @result = result
+        @resource.signal
+      end
+      @observers.each() { |o| o.call Time.now, @result }
+      @observers = []
+    end
+    ##
+    # Wait for the result and return it
+    def result()
+      wait_for_result()
+      return @result
+    end
+  end
+end

data/lib/rclrb/guard_condition.rb

@@ -0,0 +1,18 @@
+module Rclrb
+  class GuardCondition
+    attr_reader :guard_condition_handle
+    def initialize
+      @guard_condition_handle = CApi.rcl_get_zero_initialized_guard_condition
+      CApi.handle_result CApi.rcl_guard_condition_init(@guard_condition_handle, Rclrb.rcl_context, CApi.rcl_guard_condition_get_default_options)
+    end
+    rclrb_finalize_with :@guard_condition_handle do |guard_condition_handle|
+      CApi.handle_result CApi.rcl_guard_condition_fini(guard_condition_handle)
+    end
+    def trigger()
+      CApi.handle_result CApi.rcl_trigger_guard_condition(@guard_condition_handle)
+    end
+    def spin(wait_set = nil)
+      wait_set.add self if wait_set
+    end
+  end
+end

data/lib/rclrb/init.rb

@@ -0,0 +1,59 @@
+module Rclrb
+
+  ##
+  # Initialise Rclrb with the given arguments
+  def Rclrb.init(arguments: [])
+    @@initOptions = CApi.rcl_get_zero_initialized_init_options()
+    @@context = CApi.rcl_get_zero_initialized_context()
+    @@allocator = CApi.rcutils_get_default_allocator
+    CApi.handle_result(CApi.rcl_init_options_init(@@initOptions, @@allocator))
+
+    # Prepare arguments
+    p_values = nil
+    if arguments.length > 0
+      strings = arguments.map { |k| FFI::MemoryPointer.from_string(k.to_s) }
+      p_values = FFI::MemoryPointer.new(:pointer, arguments.size + 1)
+      p_values.write_array_of_pointer(strings)
+    end
+
+    # Call init
+    CApi.handle_result(CApi.rcl_init(arguments.length, p_values, @@initOptions, @@context))
+
+    # Guard condition to shutdown execution when shutdown is called
+    @@signal_guard_condition = GuardCondition.new
+    @@shutdown_requested = false
+    
+    # Setup signal to catch ctr+c
+    Rclrb.setup_signal
+  end
+  def Rclrb.setup_signal
+    Signal.trap("INT") {
+      puts "Interrupts called..."
+      Rclrb.shutdown()
+    }
+  end
+  ##
+  # Terminate Rclrb
+  def Rclrb.shutdown
+    @@shutdown_requested = true
+    @@signal_guard_condition.trigger
+  end
+  def Rclrb.rcl_context()
+    return @@context
+  end
+  def Rclrb.rcl_signal_guard_condition
+    return @@signal_guard_condition
+  end
+  def Rclrb.rcl_shutdown_requested?
+    return @@shutdown_requested
+  end
+  def Rclrb.rcl_allocator
+    return @@allocator
+  end
+  def Rclrb.rcl_allocate(size)
+    return @@allocator[:allocate].call size, @@allocator[:state]
+  end
+  def Rclrb.rcl_deallocate(ptr)
+    @@allocator[:deallocate].call ptr, @@allocator[:state]
+  end
+end

data/lib/rclrb/interfaces.rb

@@ -0,0 +1,270 @@
+module Rclrb
+  ##
+  # This module contains classes used to generate package to handle ROS messages.
+  # The content of Interfaces is internal API to rclrb and should not be directly called and is subject
+  # to change.
+  module Interfaces
+    def Interfaces.__parse_msg_srv_act_file package_name, path, filename, interface_type, interface_type_camel
+      definition = {}
+      definition[:name] = filename[0, filename.length - 4]
+      definition[:class_name] = "#{Rclrb.camelize(package_name)}::#{interface_type_camel}::#{definition[:name]}"
+      line_num = 0
+      fieldss = []
+      fields = []
+      constantss = []
+      constants = []
+      depends = []
+      ext_depends = []
+      File.open("#{path}/#{interface_type}/#{filename}").each do |line|
+        line_num += 1
+        # Remove comment
+        comment_index = line.index('#')
+        line = line[0, comment_index] unless comment_index.nil?
+        if line.index('=') != nil
+          l = line.split('=')
+          if l.length == 2
+            l2 = l[0].split(' ')
+            if l2.length == 2
+              constants.append "#{l2[1].upcase} = #{l[1]}"
+            else
+              raise ParseError.new "Cannot parse '#{path}/#{interface_type}/#{filename}', invalid constant definition on line number #{line_num}"
+            end
+          else
+            raise ParseError.new "Cannot parse '#{path}/#{interface_type}/#{filename}', invalid constant definition on line number #{line_num}"
+          end
+        else
+          l = line.split(' ', ).reject { |c| c.empty?}
+          if l.length == 2 or l.length == 3
+            type = l[0]
+            name = l[1]
+            array_index = type.index(/\[\d*\]/)
+            if array_index.nil?
+              base_type = type
+              is_array = false
+            else
+              base_type = type[0, array_index]
+              is_array = true
+            end
+
+            if Fields::AtomicFields.include? base_type
+              field_def = Fields::AtomicFields[base_type].field_name
+              if l.length == 3
+                field_init_value = l[2]
+              else
+                field_init_value = Fields::AtomicFields[base_type].init_value
+              end
+            else
+              if base_type == "Header"
+                field_def = "StdMsgs::#{interface_type_camel}::Header"
+                field_init_value = "StdMsgs::Header.new"
+                if package_name != "std_msgs"
+                  ext_depends.append "std_msgs/msg"
+                end
+              elsif base_type.index("/").nil?
+                field_def = "#{Rclrb.camelize(package_name)}::Msg::#{base_type}"
+                field_init_value = "#{field_def}.new"
+                if interface_type != 'msg'
+                  ext_depends.append "#{package_name}/msg"
+                end
+              else
+                base_type_split = base_type.split "/"
+                if base_type_split.length == 2
+                  ext_depends.append "#{base_type_split[0]}/msg" unless base_type_split[0] == package_name
+                  field_def = "#{Rclrb.camelize(base_type_split[0])}::Msg::#{base_type_split[1]}"
+                  field_init_value = "#{field_def}.new"
+                else
+                  raise ParseError.new "Cannot parse '#{path}/#{interface_type}/#{filename}', invalid type #{base_type} at line number #{line_num}"
+                end
+              end
+              depends.append field_def
+            end
+            if is_array
+              field_def += ".array_field"
+              field_init_value = []
+            end
+            fields.append({ :name => name, :field_def_name => field_def, :field_init_value => field_init_value })
+          elsif l.length == 1 and l[0] = "---"
+            fieldss.append(fields)
+            constantss.append(constants)
+            fields = []
+            constants = []
+          elsif l.length != 0
+            raise ParseError.new "Cannot parse '#{path}/#{interface_type}/#{filename}', invalid line number #{line_num}"
+          end
+        end
+      end
+      fieldss.append(fields)
+      constantss.append(constants)
+      definition[:fields] = fieldss
+      definition[:constants] = constantss
+      definition[:depends] = depends
+      definition[:ext_depends] = ext_depends.uniq
+      return definition
+    end
+    def Interfaces.__fields_body(index, klass_name)
+      str = <<-EOF
+      attr_accessor <%= mdef[:fields][#{index}].map { |f| ":" + f[:name] } .join(" ,") %>
+      <% mdef[:constants][#{index}].each do |constant| %>
+      <%= constant %>
+      <% end %>
+      def initialize()
+        <% mdef[:fields][#{index}].each do |field| %>
+        @<%= field[:name] %> = <%= field[:field_init_value] %>
+        <% end %>
+      end
+      class FFIType < FFI::Struct
+        layout <%= mdef[:fields][#{index}].map { |f| ":" + f[:name] + ", " + f[:field_def_name] + ".ffi_type" }.join(" ,") %>
+      end
+
+      def #{klass_name}.get_ffi_struct(data)
+        if data.kind_of? FFIType
+          return data
+        else
+          return FFIType.new data
+        end
+      end
+      def #{klass_name}.parse_ros_message(data)
+        msg = #{klass_name}.new
+        ffi_struct = #{klass_name}.get_ffi_struct(data)
+        <% mdef[:fields][#{index}].each do |field| %>
+        msg.<%= field[:name] %> = <%= field[:field_def_name] %>.parse_ros_message ffi_struct[:<%= field[:name] %>]
+        <% end %>
+        return msg
+      end
+      def #{klass_name}.parse_ros_message_array(data, length)
+        return Fields.__generic_parse_array(data, length, #{klass_name}.ffi_size) { |ptr| #{klass_name}.parse_ros_message(ptr) }
+      end
+      def #{klass_name}.destroy_ros_message(data)
+        ffi_struct = #{klass_name}.get_ffi_struct(data)
+        <% mdef[:fields][#{index}].each do |field| %>
+        <%= field[:field_def_name] %>.destroy_ros_message(ffi_struct[:<%= field[:name] %>])
+        <% end %>
+      end
+      def #{klass_name}.get_ros_message(msg)
+        ffi_struct = FFIType.new
+        #{klass_name}.__fill_ros_message(ffi_struct, msg)
+        return ffi_struct
+      end
+      def #{klass_name}.fill_ros_message(ffi_struct, member, msg)
+        #{klass_name}.__fill_ros_message(ffi_struct[member], msg)
+      end
+      def #{klass_name}.__fill_ros_message(ffi_struct, msg)
+        <% mdef[:fields][#{index}].each do |field| %>
+        <%= field[:field_def_name] %>.fill_ros_message(ffi_struct, :<%= field[:name] %>,  msg.<%= field[:name] %>)
+        <% end %>
+      end
+      def #{klass_name}.fill_ros_message_array(array_pointer, value)
+        Fields.__generic_fill_ros_message_array(array_pointer, value, #{klass_name}.ffi_size) { |ptr, value| #{klass_name}.__fill_ros_message(ffi_type.new(ptr), value) }
+      end
+      def #{klass_name}.ffi_type
+        return FFIType
+      end
+      def #{klass_name}.ffi_size
+        return FFIType.size
+      end
+      def #{klass_name}.is_atomic
+        return false
+      end
+EOF
+    end
+    def Interfaces.load_definitions package_name
+      is_msg_package = package_name.end_with? "/msg"
+      is_srv_package = package_name.end_with? "/srv"
+      if is_msg_package or is_srv_package
+        ros_package_name = package_name[0, package_name.size - 4]
+      else
+        return false
+      end
+      module_name = Rclrb.camelize ros_package_name
+      p = "#{`ros2 pkg prefix #{ros_package_name}`.chop}/share/#{ros_package_name}"
+      return false unless File.directory?(p)
+      if is_msg_package
+        msg_defs_unsorted = Dir.glob("*.msg", base: p + "/msg").map { |name| Interfaces.__parse_msg_srv_act_file(ros_package_name, p, name, "msg", "Msg") }
+        # msg_defs.sort! { |a| a[:depends].length }
+        msg_defs = []
+        current_index = 0
+        while msg_defs_unsorted.length > 0
+          if current_index >= msg_defs_unsorted.length
+            current_index = 0
+          end 
+          add_to_msg_defs = true
+          msg_defs_unsorted[current_index][:depends].each() { |x|
+            if x.start_with? module_name
+              found = false
+              msg_defs.each() { |y|
+                if y[:class_name] == x
+                  found = true
+                  break
+                end
+              }
+              unless found
+                add_to_msg_defs = false
+                break
+              end
+            end
+          }
+          if add_to_msg_defs
+            msg_defs.push msg_defs_unsorted[current_index]
+            msg_defs_unsorted.delete_at current_index
+          else
+            current_index += 1
+          end
+        end
+
+        msg_defs.each() do |mdef|
+          mdef[:ext_depends].each() { |d| require d }
+        end
+        CApi.create_type_support_library(module_name, ros_package_name, "message", "msg", "RosidlMessageTypeSupportT", msg_defs.map {|mdef| mdef[:name]})
+        template = ERB.new <<-EOF
+module <%= module_name %>
+  module Msg
+    <% msg_defs.each do |mdef| %>
+    class <%= mdef[:name] %>
+      def <%= mdef[:name] %>.type_support()
+        TypeSupportCApi::TypeSupport_msg_<%= mdef[:name] %>
+      end
+      #{Interfaces.__fields_body(0, "<%= mdef[:name] %>")}
+      def <%= mdef[:name] %>.array_field()
+        @@array_field = Fields::ArrayField.new(<%= mdef[:name] %>) unless defined?(@@array_field)
+        return @@array_field
+      end
+    end
+    <% end %>
+  end
+end
+EOF
+        # puts template.result(binding)
+        Object.class_eval template.result(binding)
+        return true
+      elsif is_srv_package
+        srv_defs = Dir.glob("*.srv", base: p + "/srv").map { |name| Interfaces.__parse_msg_srv_act_file(ros_package_name, p, name, "srv", "Srv") }
+        srv_defs.each() do |mdef|
+          mdef[:ext_depends].each() { |d| require d }
+        end
+        CApi.create_type_support_library module_name, ros_package_name, "service", "srv", "RosidlServiceTypeSupportT", srv_defs.map {|mdef| mdef[:name]}
+        template = ERB.new <<-EOF
+module <%= module_name %>
+  module Srv
+  <% srv_defs.each do |mdef| %>
+    module <%= mdef[:name] %>
+      def <%= mdef[:name] %>.type_support()
+        TypeSupportCApi::TypeSupport_srv_<%= mdef[:name] %>
+      end
+      class Request
+        #{Interfaces.__fields_body(0, "Request")}
+      end
+      class Response
+        #{Interfaces.__fields_body(1, "Response")}
+      end
+    end
+  <% end %>
+  end
+end
+EOF
+        Object.class_eval template.result(binding)
+        return true
+      end
+      return false
+    end
+  end
+end

data/lib/rclrb/internal.rb

@@ -0,0 +1,16 @@
+module Rclrb
+  ##
+  # Uncamelize a string, for instance transform "ThisIsAnExample" to "this_is_an_example" 
+  def Rclrb.uncamelize str
+    return str.gsub(/::/, '/')
+              .gsub(/([A-Z]+)([A-Z][a-z])/,'\1_\2')
+              .gsub(/([a-z\d])([A-Z])/,'\1_\2')
+              .tr("-", "_")
+              .downcase
+  end
+  ##
+  # Camelize a string, for instance transform "this_is_an_example" to "ThisIsAnExample" 
+  def Rclrb.camelize str
+    return str.split("_").map(&:capitalize).join
+  end
+end