rclrb 1.1.0

data/lib/rclrb/capi.rb

@@ -0,0 +1,369 @@
+require 'ffi'
+
+module Rclrb
+  ##
+  # This module contains the function and data structures used to interface with the RCL C library
+  # The content of CApi is internal API to rclrb and should not be directly called and is subject
+  # to change.
+  module CApi
+    extend FFI::Library
+    ffi_lib 'rcl'
+
+    RCL_RET_OK = 0
+    RCL_RET_TIMEOUT = 2
+    RCL_RET_SUBSCRIPTION_TAKE_FAILED = 401
+    RCL_RET_CLIENT_TAKE_FAILED = 501
+    RCL_RET_SERVICE_TAKE_FAILED = 601
+
+    RCUTILS_ERROR_MESSAGE_MAX_LENGTH = 1024
+    RMW_GID_STORAGE_SIZE = 24
+
+    POINTER_SIZE = FFI::Pointer::SIZE
+    SIZE_T_SIZE = FFI::type_size(:size_t)
+    INT32_SIZE = FFI::type_size(:int32)
+    UINT32_SIZE = FFI::type_size(:uint32)
+
+    # Rcutils structs
+    class RcutilsErrorStringT < FFI::Struct
+      layout :str, [:uint8, RCUTILS_ERROR_MESSAGE_MAX_LENGTH]
+    end
+
+    # Rmw structs
+    enum :RmwQosReliabilityPolicyE, [
+      :RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT,
+      :RMW_QOS_POLICY_RELIABILITY_RELIABLE,
+      :RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT,
+      :RMW_QOS_POLICY_RELIABILITY_UNKNOWN
+    ]
+    enum :RmwQosDdurabilityPolicy, [
+      :RMW_QOS_POLICY_DURABILITY_SYSTEM_DEFAULT,
+      :RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL,
+      :RMW_QOS_POLICY_DURABILITY_VOLATILE,
+      :RMW_QOS_POLICY_DURABILITY_UNKNOWN
+    ]
+    enum :RmwQosLivelinessPolicy, [
+      :RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT,
+      :RMW_QOS_POLICY_LIVELINESS_AUTOMATIC,
+      :RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_NODE_DEPRECATED,
+      :RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC,
+      :RMW_QOS_POLICY_LIVELINESS_UNKNOWN
+    ]
+    enum :RmwQosGistoryPolicyE, [
+      :RMW_QOS_POLICY_HISTORY_SYSTEM_DEFAULT,
+      :RMW_QOS_POLICY_HISTORY_KEEP_LAST,
+      :RMW_QOS_POLICY_HISTORY_KEEP_ALL,
+      :RMW_QOS_POLICY_HISTORY_UNKNOWN
+    ]
+  
+    enum :rmw_unique_network_flow_endpoints_requirement_e, []
+
+    class RmwTimeS < FFI::Struct
+      layout :sec, :uint64, :nsec, :uint64
+    end
+
+    class RmwQoSProfileT < FFI::Struct
+      layout :history, :RmwQosGistoryPolicyE,
+            :depth, :size_t,
+            :reliability, :RmwQosReliabilityPolicyE,
+            :durability, :RmwQosDdurabilityPolicy,
+            :deadline, RmwTimeS,
+            :lifespan, RmwTimeS,
+            :liveliness, :RmwQosLivelinessPolicy,
+            :liveliness_lease_duration, RmwTimeS,
+            :avoid_ros_namespace_conventions, :bool
+    end
+
+    class RmwSubscriptionOptionsT < FFI::Struct
+      layout :rmw_specific_subscription_payload, :pointer,
+             :ignore_local_publications, :bool,
+             :require_unique_network_flow_endpoints, :rmw_unique_network_flow_endpoints_requirement_e,
+             :content_filter_options, :pointer
+    end
+
+    class RmwPublisherOptionsT < FFI::Struct
+      layout :rmw_specific_publisher_payload, :pointer,
+             :require_unique_network_flow_endpoints, :rmw_unique_network_flow_endpoints_requirement_e
+    end
+
+    class RmwGidT < FFI::Struct
+      layout :implementation_identifier, :string,
+             :data, [:uint8, RMW_GID_STORAGE_SIZE]
+    end
+
+    RmwTimePointValueT = :int64
+
+    class RmwMessageInfoT < FFI::Struct
+      layout :source_timestamp, RmwTimePointValueT,
+             :received_timestamp, RmwTimePointValueT,
+             :publication_sequence_number, :uint64,
+             :reception_sequence_number, :uint64,
+             :publisher_gid, RmwGidT,
+             :from_intra_process, :bool
+    end
+
+    class RmwSubscriptionAllocationT < FFI::Struct
+      layout :implementation_identifier, :string,
+             :data, :pointer
+    end
+
+    class RmwPublisherAllocationT < FFI::Struct
+      layout :implementation_identifier, :string,
+             :data, :pointer
+    end
+
+    class RmwRequestIdT < FFI::Struct
+      layout :writer_guid, [:int16, 16],
+            :sequence_number, :int64
+    end
+
+    class RmwServiceInfoT < FFI::Struct
+      layout :source_timestamp, RmwTimePointValueT,
+             :received_timestamp, RmwTimePointValueT,
+             :request_id, RmwRequestIdT
+    end
+    
+    # Rosidl
+    
+    class RosidlMessageTypeSupportT < FFI::Struct
+      layout :typesuport_identifier, :string,
+             :data, :pointer,
+             :func, :pointer
+    end
+
+    class RosidlServiceTypeSupportT < FFI::Struct
+      layout :typesuport_identifier, :string,
+             :data, :pointer,
+             :func, :pointer
+    end
+
+    # Rcl
+    callback :allocate_callback, [:size_t, :pointer], :pointer
+    callback :deallocate_callback, [:pointer, :pointer], :void
+
+    class RclAllocatorT < FFI::Struct
+      layout :allocate, :allocate_callback, :deallocate, :deallocate_callback,
+            :reallocate, :pointer, :zero_allocate, :pointer,
+            :state, :pointer
+    end
+
+    class RclArgumentsT < FFI::Struct
+      layout :impl, :pointer
+    end
+
+    class RclContextT < FFI::Struct
+      layout :global_arguments, RclArgumentsT,
+            :impl, :pointer,
+            :instance_id_storage,  [:uint8, 8] 
+    end
+
+    class RclInitOptionsT < FFI::Struct
+      layout :impl, :pointer
+    end
+
+    enum :RclClockTypeT, [:RCL_CLOCK_UNINITIALIZED, :RCL_ROS_TIME, :RCL_SYSTEM_TIME, :RCL_STEADY_TIME]
+
+    class RclClockT < FFI::Struct
+      layout :type, :RclClockTypeT,
+             :jump_callbacks, :pointer,
+             :num_jump_callbacks, :size_t,
+             :now, :pointer,
+             :data, :pointer,
+             :allocator, RclAllocatorT
+    end
+
+    class RclNodeT < FFI::Struct
+      layout :context, :pointer, :impl, :pointer
+    end
+
+    class RclSubscriptionOptionsT < FFI::Struct
+      layout :qos, RmwQoSProfileT, :allocator, RclAllocatorT, :rmw_subscription_options, RmwSubscriptionOptionsT
+    end
+
+    class RclPublisherOptionsT < FFI::Struct
+      layout :qos, RmwQoSProfileT, :allocator, RclAllocatorT, :rmw_subscription_options, RmwPublisherOptionsT
+    end
+
+    class RclServiceOptionsT < FFI::Struct
+      layout :qos, RmwQoSProfileT, :allocator, RclAllocatorT
+    end
+
+    class RclClientOptionsT < FFI::Struct
+      layout :qos, RmwQoSProfileT, :allocator, RclAllocatorT
+    end
+
+    class RclGuardConditionOptionsT < FFI::Struct
+      layout :allocator, RclAllocatorT
+    end
+
+    class RclSubscriptionT < FFI::Struct
+      layout :impl, :pointer
+    end
+    
+    class RclPublisherT < FFI::Struct
+      layout :impl, :pointer
+    end
+    
+    class RclServiceT < FFI::Struct
+      layout :impl, :pointer
+    end
+    
+    class RclClientT < FFI::Struct
+      layout :impl, :pointer
+    end
+
+    class RclGuardConditionT < FFI::Struct
+      layout :context, RclContextT.by_ref,
+             :impl, :pointer
+    end
+
+    class RclTimerT < FFI::Struct
+      layout :impl, :pointer
+    end
+
+    class RclNodeOptionsT < FFI::Struct
+      layout :allocator, RclAllocatorT, :use_global_arguments, :bool,
+            :arguments, RclArgumentsT, :enable_rosout, :bool,
+            :rosout_qos, RmwQoSProfileT
+    end
+
+    class RclWaitSetT < FFI::Struct
+      layout :subscriptions, :pointer,
+             :size_of_subscriptions, :size_t,
+             :guard_conditions, :pointer,
+             :size_of_guard_conditions, :size_t,
+             :timers, :pointer,
+             :size_of_timers, :size_t,
+             :clients, :pointer,
+             :size_of_clients, :size_t,
+             :services, :pointer,
+             :size_of_services, :size_t,
+             :events, :pointer,
+             :size_of_events, :size_t,
+             :impl, :pointer
+    end
+
+    # Misc
+
+    RclRetT = :int
+    class BoolPtr < FFI::Struct
+      layout  :value, :bool
+    end
+    class Int64Ptr < FFI::Struct
+      layout  :value, :int64
+    end
+    class SizeTPtr < FFI::Struct
+      layout  :value, :size_t
+    end
+
+    # init API
+    attach_function :rcl_get_zero_initialized_context, [], RclContextT.by_value
+    attach_function :rcl_context_fini, [:pointer], :int
+    attach_function :rcl_get_zero_initialized_init_options, [], RclInitOptionsT.by_value
+    attach_function :rcl_init_options_init, [RclInitOptionsT.by_ref, RclAllocatorT.by_value], RclRetT
+    attach_function :rcl_init, [:int, :pointer, RclInitOptionsT.by_ref, RclContextT.by_ref], RclRetT
+
+    # Node API
+    attach_function :rcl_node_init, [RclNodeT.by_ref, :string, :string, RclContextT.by_ref, RclNodeOptionsT.by_ref], RclRetT
+    attach_function :rcl_node_fini, [RclNodeT.by_ref], RclRetT
+    attach_function :rcl_node_get_default_options, [], RclNodeOptionsT.by_value
+    attach_function :rcl_node_get_namespace,  [RclNodeT.by_ref], :string
+
+    # Subscription API
+    attach_function :rcl_get_zero_initialized_subscription, [], RclSubscriptionT.by_value
+    attach_function :rcl_subscription_get_default_options, [], RclSubscriptionOptionsT.by_value
+    attach_function :rcl_subscription_init, [RclSubscriptionT.by_ref, RclNodeT.by_ref, RosidlMessageTypeSupportT.by_ref, :string, RclSubscriptionOptionsT.by_ref], RclRetT
+    attach_function :rcl_subscription_fini, [RclSubscriptionT.by_ref, RclNodeT.by_ref], RclRetT
+    attach_function :rcl_take, [RclSubscriptionT.by_ref, :pointer, RmwMessageInfoT.by_ref, RmwSubscriptionAllocationT.by_ref], RclRetT
+
+    # Publisher API
+    attach_function :rcl_get_zero_initialized_publisher, [], RclPublisherT.by_value
+    attach_function :rcl_publisher_get_default_options, [], RclPublisherOptionsT.by_value
+    attach_function :rcl_publisher_init, [RclPublisherT.by_ref, RclNodeT.by_ref, RosidlMessageTypeSupportT.by_ref, :string, RclPublisherOptionsT.by_ref], RclRetT
+    attach_function :rcl_publisher_fini, [RclPublisherT.by_ref, RclNodeT.by_ref], RclRetT
+    attach_function :rcl_publish, [RclPublisherT.by_ref, :pointer, RmwPublisherAllocationT.by_ref], RclRetT
+    attach_function :rcl_publisher_get_subscription_count, [RclPublisherT.by_ref, SizeTPtr.by_ref], RclRetT
+
+    # Service API
+    attach_function :rcl_get_zero_initialized_service, [], RclServiceT.by_value
+    attach_function :rcl_service_get_default_options, [], RclServiceOptionsT.by_value
+    attach_function :rcl_service_init, [RclServiceT.by_ref, RclNodeT.by_ref, RosidlServiceTypeSupportT.by_ref, :string, RclServiceOptionsT.by_ref], RclRetT
+    attach_function :rcl_service_fini, [RclServiceT.by_ref, RclNodeT.by_ref], RclRetT
+    attach_function :rcl_take_request, [RclServiceT.by_ref, RmwRequestIdT.by_ref, :pointer], RclRetT
+    attach_function :rcl_send_response, [RclServiceT.by_ref, RmwRequestIdT.by_ref, :pointer], RclRetT
+
+    # Client API
+    attach_function :rcl_get_zero_initialized_client, [], RclClientT.by_value
+    attach_function :rcl_client_get_default_options, [], RclClientOptionsT.by_value
+    attach_function :rcl_client_init, [RclClientT.by_ref, RclNodeT.by_ref, RosidlServiceTypeSupportT.by_ref, :string, RclClientOptionsT.by_ref], RclRetT
+    attach_function :rcl_client_fini, [RclClientT.by_ref, RclNodeT.by_ref], RclRetT
+    attach_function :rcl_send_request, [RclClientT.by_ref, :pointer, Int64Ptr.by_ref], RclRetT
+    attach_function :rcl_take_response_with_info, [RclClientT.by_ref, RmwServiceInfoT.by_ref, :pointer], RclRetT
+
+    # GuardCondition API
+    attach_function :rcl_get_zero_initialized_guard_condition, [], RclGuardConditionT.by_value
+    attach_function :rcl_guard_condition_init, [RclGuardConditionT.by_ref, RclContextT.by_ref, RclGuardConditionOptionsT.by_value], RclRetT
+    attach_function :rcl_guard_condition_get_default_options, [], RclGuardConditionOptionsT.by_value
+    attach_function :rcl_trigger_guard_condition, [RclGuardConditionT.by_ref], RclRetT
+
+    # Clock API
+    attach_function :rcl_clock_init, [:RclClockTypeT, RclClockT.by_ref, RclAllocatorT.by_ref], RclRetT
+
+    # Timer API
+    attach_function :rcl_get_zero_initialized_timer, [], RclTimerT.by_value
+    callback :timer_callback, [RclTimerT.by_ref, :int64], :void
+    attach_function :rcl_timer_init, [RclTimerT.by_ref, RclClockT.by_ref, RclContextT.by_ref, :int64, :timer_callback, RclAllocatorT.by_value], RclRetT
+    attach_function :rcl_timer_fini, [RclTimerT.by_ref, RclNodeT.by_ref], RclRetT
+    attach_function :rcl_timer_call, [RclTimerT.by_ref], RclRetT
+    attach_function :rcl_timer_is_ready, [RclTimerT.by_ref, BoolPtr.by_ref], RclRetT
+
+    # WaitSet API
+    attach_function :rcl_get_zero_initialized_wait_set, [], RclWaitSetT.by_value
+    attach_function :rcl_wait_set_init, [RclWaitSetT.by_ref, :size_t, :size_t, :size_t, :size_t, :size_t, :size_t, RclContextT.by_ref, RclAllocatorT.by_value], RclRetT
+    attach_function :rcl_wait_set_fini, [RclWaitSetT.by_ref], RclRetT
+    attach_function :rcl_wait, [RclWaitSetT.by_ref, :int64], RclRetT, blocking: true
+    attach_function :rcl_wait_set_add_subscription, [RclWaitSetT.by_ref, RclSubscriptionT.by_ref, SizeTPtr.by_ref], RclRetT
+    attach_function :rcl_wait_set_add_guard_condition, [RclWaitSetT.by_ref, RclGuardConditionT.by_ref, SizeTPtr.by_ref], RclRetT
+    attach_function :rcl_wait_set_add_timer, [RclWaitSetT.by_ref, RclTimerT.by_ref, SizeTPtr.by_ref], RclRetT
+    attach_function :rcl_wait_set_add_client, [RclWaitSetT.by_ref, RclClientT.by_ref, SizeTPtr.by_ref], RclRetT
+    attach_function :rcl_wait_set_add_service, [RclWaitSetT.by_ref, RclServiceT.by_ref, SizeTPtr.by_ref], RclRetT
+
+    # Graph API
+    attach_function :rcl_service_server_is_available, [RclNodeT.by_ref, RclClientT.by_ref, BoolPtr.by_ref], RclRetT
+
+    # rcutils API
+    attach_function :rcutils_get_default_allocator, [], RclAllocatorT.by_value
+    attach_function :rcutils_get_error_string, [], RcutilsErrorStringT.by_value
+    attach_function :rcutils_reset_error, [], :void
+
+    def CApi.create_type_support_library(module_name, package_name, longtype, shorttype, idltype, messages)
+      template = ERB.new <<-EOF
+module <%= module_name %>
+  module TypeSupportCApi
+    extend FFI::Library
+    ffi_lib '<%= package_name %>__rosidl_typesupport_c'
+    <% messages.each() do | m | %>
+    attach_function :rosidl_typesupport_c__get_#{longtype}_type_support_handle__<%= package_name %>__#{shorttype}__<%= m %>, [], CApi::#{idltype}.by_ref
+    TypeSupport_#{shorttype}_<%= m %> = rosidl_typesupport_c__get_#{longtype}_type_support_handle__<%= package_name %>__#{shorttype}__<%= m %>
+    <% end %>
+  end
+end
+EOF
+      Object.class_eval template.result(binding)
+    end
+
+    def CApi.create_node()
+      return CApi::RclNodeT.new()
+    end
+    def CApi.get_error_string()
+      msg = CApi.rcutils_get_error_string()[:str]
+      CApi.rcutils_reset_error();
+      return msg
+    end
+    def CApi.handle_result(ret, always_call = nil)
+      always_call.call if always_call
+      if ret != RCL_RET_OK
+        raise RclError.new "Error #{ret.to_i}: #{CApi.get_error_string()}"
+      end
+    end
+  end
+end

data/lib/rclrb/client.rb

@@ -0,0 +1,79 @@
+module Rclrb
+  ##
+  # Represent a ROS client
+  class Client
+    attr_reader :srv_type, :client_handle
+    # Construct a new publisher, this should not be called directly, instead use Node.create_publisher.
+    def initialize(client_handle, node_handle, srv_type, srv_name, qos_profile)
+      @client_handle = client_handle
+      @node_handle = node_handle
+      @srv_type = srv_type
+      @srv_name = srv_name
+      @qos_profile = qos_profile
+      @call_counter = 0
+
+      client_ops = CApi.rcl_client_get_default_options()
+      client_ops[:qos] = QoSProfile.get_profile(qos_profile).ros_profile
+      CApi.handle_result CApi.rcl_client_init(@client_handle, node_handle, srv_type.type_support(), @srv_name, client_ops)
+    end
+
+    rclrb_finalize_with :@client_handle, :@node_handle do |client_handle, node_handle|
+      CApi.handle_result CApi.rcl_client_fini client_handle, node_handle
+    end
+
+    ##
+    # Call a service asynchronously with the given +request+, it will return a Future object.
+    def call_async(request)
+      raise RclError.new unless request.kind_of? @srv_type::Request
+      
+      counter = CApi::Int64Ptr.new
+      counter[:value] = (@call_counter += 1)
+      future = Future.execute do
+        ros_req = @srv_type::Request.get_ros_message request
+        CApi.handle_result CApi.rcl_send_request(@client_handle, ros_req, counter), lambda { @srv_type::Request.destroy_ros_message(ros_req)}
+        ros_resp = @srv_type::Response::FFIType.new
+        info = CApi::RmwServiceInfoT.new
+        waiting_for_answer = true
+        wait_set = WaitSet.new
+        wait_set.add self
+        while waiting_for_answer
+          wait_set.wait
+          status = CApi.rcl_take_response_with_info @client_handle, info, ros_resp
+          if status == CApi::RCL_RET_OK
+            response = @srv_type::Response.parse_ros_message ros_resp
+            @srv_type::Response.destroy_ros_message ros_resp
+            waiting_for_answer = false
+          elsif status == CApi::RCL_RET_CLIENT_TAKE_FAILED
+            raise ServiceCallFailed.new
+          else
+            CApi.handle_result status
+          end
+          if Rclrb.rcl_shutdown_requested?
+            raise InterruptedClientCall.new "Call to #{@srv_name} was interrupted by shutdown"
+          end
+        end
+        response
+      end
+      return future
+    end
+    ##
+    # Wait for the service to be available. If +timeout_sec+ is nil, it will wait forever, otherwise it will timeout when 
+    # the amount of time specified by +timeout_sec+ has expired.
+    def wait_for_service(timeout_sec=nil)
+      sleep_time = 0.25
+      timeout_sec = Float::INFINITY if timeout_sec.nil?
+      until Rclrb.rcl_shutdown_requested? or self.service_is_ready() or timeout_sec < 0.0
+        sleep(sleep_time)
+        timeout_sec -= sleep_time
+      end
+      return self.service_is_ready
+    end
+    # Return true if the service is available.
+    def service_is_ready
+      b = CApi::BoolPtr.new
+      CApi.handle_result CApi.rcl_service_server_is_available @node_handle, @client_handle, b
+      return b[:value]
+    end
+  end
+end
+  

data/lib/rclrb/clock.rb

@@ -0,0 +1,17 @@
+module Rclrb
+  ##
+  # Represent a clock, instead of creating directly, use RosClock or SystemClock
+  class Clock
+    attr_reader :handle
+    def initialize(type)
+      @handle = CApi::RclClockT.new
+      CApi.handle_result(CApi.rcl_clock_init(type, @handle, CApi.rcutils_get_default_allocator()))
+    end
+  end
+  ##
+  # Represent a clock with Ros time (either system or simulated time)
+  RosClock = Clock.new(:RCL_ROS_TIME)
+  ##
+  # Represent a clock that is always set to system time
+  SystemClock = Clock.new(:RCL_SYSTEM_TIME)
+end

data/lib/rclrb/common.rb

@@ -0,0 +1,43 @@
+
+def Object.rclrb_finalize_with *ivars_names, &block
+  old_init = instance_method :initialize
+
+  define_method :initialize do |*args|
+    old_init.bind(self).call *args
+    ivars = ivars_names.map { |ivar| instance_variable_get(ivar) }
+    ObjectSpace.define_finalizer self, Object.rclrb_create_finalizer(block, ivars)
+  end
+  private_class_method def Object.rclrb_create_finalizer block, ivars
+    return proc { block.call(*ivars) }
+  end
+end
+
+module Rclrb
+  ##
+  # Base class for any exception triggered by Rclrb
+  class Error < StandardError
+  end
+  ##
+  # An unhandled error occured in the Rcl library. This is generally indicative of a bug in Rclrb. 
+  class RclError < Error
+  end
+  ##
+  # An error occured while parsing a message, service or action definiton.
+  class ParseError < Error
+  end
+  ##
+  # A call to service was interrupted
+  class InterruptedClientCall < Error
+  end
+  ##
+  # Failed to get answer to a service call
+  class ServiceCallFailed < Error
+  end
+  ##
+  # Publisher.call or Client.call was called with the wrong type of message.
+  class InvalidMessageTypeError < Error
+    def initialize got, expected
+      super "Invalid message type got '#{got}' expected '#{expected}"
+    end
+  end
+end

data/lib/rclrb/executor.rb

@@ -0,0 +1,34 @@
+module Rclrb
+  ##
+  # An executor controls the threading model used to process callbacks. Callbacks are units of work
+  # like subscription callbacks, timer callbacks, and service calls. An executor controls which threads
+  # callbacks get executed in.
+  class Executor
+    def initialize(*nodes)
+      @nodes = nodes
+      @groups = nodes.map { |x| x.default_callback_group }
+    end
+    ##
+    # Add a node to the executor
+    def add_node(node)
+      @nodes.append node
+      @groups.append node.default_callback_group
+    end
+    ##
+    # Spin continuously untill shutdown was requested
+    def spin()
+      until Rclrb.rcl_shutdown_requested?
+        wait_set = WaitSet.new
+        threads = @groups.map { |g| g.spin wait_set }
+        threads.each { |t| t.join }
+        wait_set.wait
+      end
+    end
+  end
+  ##
+  # Spin the given nodes using Executor
+  def Rclrb.spin(*nodes)
+    e = Executor.new *nodes
+    return e.spin
+  end
+end