asterism 0.1.0 → 0.3.0

data/mrblib/asterism.rb

@@ -166,7 +166,7 @@ module Asterism
   # and Error when the same <node>/<app> is already on the network.
   def self.connect(locator, node:, app:, mode: nil, listen: nil)
     raise Error, "already connected (Asterism.close first)" if connected?
-    close
+    release
     node = check_name("node", node)
     app = check_name("app", app)
     begin
@@ -195,6 +195,7 @@ module Asterism
     @proxies = {}
     @depth = 0
     @lost = nil
+    @known = []    # the other nodes told to on_join so far
     self
   end
 
@@ -206,8 +207,9 @@ module Asterism
     found = false
     while true
       s.poll
+      ended = g.done? # before taking the replies (see Future#collect)
       found = true if g.each_reply.size > 0
-      break if found || g.done? || now_ms - t0 > 1500
+      break if found || ended || now_ms - t0 > 1500
       pause(WAIT_STEP_MS)
     end
     found
@@ -235,8 +237,17 @@ module Asterism
     @lost
   end
 
-  # Closes the connection (and with it every exposed object). Idempotent.
+  # Closes the connection (and with it every exposed object), and forgets
+  # the on_join / on_leave blocks. Idempotent.
   def self.close
+    release
+    @known = nil
+    @on_join = nil
+    @on_leave = nil
+    nil
+  end
+
+  def self.release
     s = @session
     @session = nil
     @objects = {}
@@ -247,7 +258,6 @@ module Asterism
     @node_token = nil
     @queryable = nil
     @watch = nil
-    nil
   end
 
   def self.session!
@@ -255,10 +265,11 @@ module Asterism
     @session
   end
 
+  # The connection went away: closed, but the next Asterism.poll still tells
+  # on_leave about every node known until then.
   def self.lost(reason)
     @lost = reason
-    close
-    @lost = reason
+    release
   end
 
   # ------------------------------------------------------------ exposing
@@ -300,10 +311,71 @@ module Asterism
   # ------------------------------------------------------------ polling
 
   # Call from the application's update loop. Answers the calls that came
-  # in and follows who is alive. false once the connection is closed.
+  # in, follows who is alive and calls the on_join / on_leave blocks. false
+  # once the connection is closed.
   def self.poll
-    return false unless @session
-    pump
+    ok = @session ? pump : false
+    tell_nodes
+    ok
+  end
+
+  # ------------------------------------------------------------ nodes coming and going
+
+  # on_join { |node| }: another node appeared (its node token or one of its
+  # objects). The nodes there already join on the first polls after
+  # connecting. on_leave { |node| }: a node is gone (no token and no object
+  # left); when the connection is lost, every node known until then leaves.
+  #
+  # The blocks run from Asterism.poll only, never from the polling inside a
+  # waiting call. What a block raises comes out of Asterism.poll (on CRuby
+  # with a receiving thread: to on_error). Asterism.close forgets them.
+  def self.on_join(&blk)
+    raise ArgumentError, "on_join needs a block" unless blk
+    (@on_join ||= []) << blk
+    self
+  end
+
+  def self.on_leave(&blk)
+    raise ArgumentError, "on_leave needs a block" unless blk
+    (@on_leave ||= []) << blk
+    self
+  end
+
+  # Compares the nodes alive now with those told before and calls the
+  # blocks for the difference.
+  def self.tell_nodes
+    known = @known
+    return if known.nil?
+    now = @session ? nodes : []
+    now.shift if @session # this node
+    joined = []
+    left = []
+    now.each { |n| joined << n unless known.include?(n) }
+    known.each { |n| left << n unless now.include?(n) }
+    # After a loss, everything has been told: nothing more to compare.
+    @known = @session ? now : nil
+    tell_each("on_join", @on_join, joined)
+    tell_each("on_leave", @on_leave, left)
+  end
+
+  # While loops, not each with a block: the blocks are the application's,
+  # and nothing else stands between them and the update loop on the stack.
+  def self.tell_each(where, blocks, names)
+    return if blocks.nil? || names.empty?
+    i = 0
+    while i < names.size
+      j = 0
+      while j < blocks.size
+        tell(where, blocks[j], names[i])
+        j += 1
+      end
+      i += 1
+    end
+  end
+
+  # One block for one node (CRuby routes what it raises to on_error).
+  def self.tell(_where, blk, node)
+    blk.call(node)
   end
 
   def self.pump

data/mrblib/future.rb

@@ -70,11 +70,16 @@ module Asterism
     def collect
       return if @finished
       g = @get
+      # done? before taking the replies: with a binding whose replies come
+      # in on another thread (zenoh-c), a reply and the end of the get may
+      # both arrive between the two; read the other way round, the get
+      # would look finished with its reply still queued.
+      ended = g.done?
       # The Array form, not a block: a block called from C costs another
       # interpreter entry on the C stack, and this runs inside nested waits.
       replies = g.each_reply
       @reply = replies[0][1] if @reply.nil? && replies.size > 0
-      if !@reply.nil? || g.done? || ::Asterism.now_ms >= @deadline || !::Asterism.connected?
+      if !@reply.nil? || ended || ::Asterism.now_ms >= @deadline || !::Asterism.connected?
         @finished = true
         @took = ::Asterism.now_ms - @started
       end

data/mrblib/ros.rb

@@ -11,8 +11,11 @@
 #   add = Asterism::ROS.require_type("example_interfaces/srv/AddTwoInts")
 #   node.service("/add_two_ints_fmrb", add) { |req| { sum: req.a + req.b } }
 #   cli = node.client("/add_two_ints", add)
+#   node.subscribe("/cmd", "std_msgs/msg/String") { |msg, info| puts msg.data }
+#   node.every(1.0) { pub << { data: "tick" } }
 #   loop do
-#     node.poll                                   # session.poll + answer services
+#     node.poll                                   # session.poll, services, then the
+#                                                 # subscribe and every blocks
 #     pub << { data: "hello" }                    # or pub.publish(str.new(data: "hello"))
 #     sub.each_pending { |msg, info| puts msg.data }   # info: Attachment or nil
 #   end
@@ -55,7 +58,7 @@ module Asterism
     def self.mangle(name)
       s = name.to_s
       return "%" if s.empty? || s == "/"
-      out = ""
+      out = "".dup
       i = 0
       n = s.bytesize
       while i < n
@@ -101,7 +104,7 @@ module Asterism
     # rmw_zenoh hashes the entity's liveliness key the same way (with XXH3);
     # receivers only use the GID to tell publishers apart.
     def self.gid_for(key)
-      out = ""
+      out = "".dup
       lane = 0
       while lane < 4
         h = 0x811c9dc5 ^ (lane * 0x9e3779b9 & 0xffffffff)
@@ -140,6 +143,11 @@ module Asterism
         @gid = gid
       end
 
+      # For pattern matching: case info in {sequence:, gid:}.
+      def deconstruct_keys(_keys)
+        { sequence: @sequence, stamp_ns: @stamp_ns, gid: @gid }
+      end
+
       def encode
         g = @gid.to_s
         ::Asterism::CDR.le_bytes(@sequence, 8) + ::Asterism::CDR.le_bytes(@stamp_ns, 8) +
@@ -217,7 +225,7 @@ module Asterism
 
     # Asterism::ROS::<Pkg>::<Name>, or nil.
     def self.type_constant(pkg, name)
-      mod_name = ""
+      mod_name = "".dup  # a fresh String: literals may be frozen (Ruby 4)
       pkg.split("_").each do |w|
         mod_name << w.byteslice(0, 1).upcase << w.byteslice(1, w.bytesize - 1) if w.bytesize > 0
       end
@@ -290,6 +298,20 @@ module Asterism
         end
       end
 
+      # For pattern matching: case msg in {linear: {x:}, angular: {z:}}.
+      # Nested messages stay messages (they match the same way).
+      def deconstruct_keys(keys)
+        h = {}
+        fs = self.class::FIELDS
+        i = 0
+        while i < fs.size
+          f = fs[i]
+          h[f] = __send__(f) if keys.nil? || keys.include?(f)
+          i += 1
+        end
+        h
+      end
+
       def ==(other)
         other.class == self.class && to_h == other.to_h
       end
@@ -318,6 +340,8 @@ module Asterism
         @next_id = 1
         @entities = []
         @services = []
+        @handled = []  # subscriptions with a block
+        @timers = []
         @key = "#{LIVELINESS_ROOT}/#{@domain}/#{@zid}/#{@nid}/#{@nid}/NN/" \
                "#{::Asterism::ROS.mangle(@enclave)}/#{::Asterism::ROS.mangle(@namespace)}/#{@name}"
         @token = session.liveliness(@key)
@@ -353,6 +377,35 @@ module Asterism
         e
       end
 
+      # Subscribes and gives each message (and its Attachment, or nil) to
+      # the block, from node.poll. Returns the Subscription (close ends it).
+      # The block runs on the update loop's stack: do not wait in it (no
+      # service call, no proxy call); start a call_async and look at it
+      # later instead.
+      def subscribe(topic, type, qos: DEFAULT_QOS, depth: 16, &handler)
+        raise ArgumentError, "subscribe needs a block (or use subscription)" unless handler
+        e = subscription(topic, type, qos: qos, depth: depth)
+        e.handler = handler
+        @handled << e
+        e
+      end
+
+      # Calls the block every `seconds` (the first time one period from now),
+      # from node.poll. Returns a Timer (cancel). It keeps the period; after
+      # a long stall it starts again from now instead of firing the missed
+      # times at once. As for subscribe: do not wait in the block.
+      def every(seconds, &blk)
+        raise ArgumentError, "every needs a block" unless blk
+        t = ::Asterism::ROS::Timer.new(self, seconds, blk)
+        @timers << t
+        t
+      end
+
+      def cancel_timer(timer)
+        @timers.delete(timer)
+        nil
+      end
+
       # node.call("/add_two_ints", AddTwoInts, a: 1, b: 2) -> response.
       # A client per service name is made on first use and kept.
       def call(service, type, request = nil, timeout_ms: ::Asterism::ROS::Client::DEFAULT_TIMEOUT_MS, **fields)
@@ -365,21 +418,85 @@ module Asterism
         c.call(request, timeout_ms: timeout_ms, **fields)
       end
 
-      # Polls the session (Asterism::Zenoh::Session#poll) and answers the
-      # requests that came in for this node's services. Returns what
-      # session.poll returns (false once the session is closed).
+      # Polls the session (Asterism::Zenoh::Session#poll), answers the
+      # requests that came in for this node's services, then gives the
+      # messages that came in to the subscribe blocks and fires the timers
+      # that are due. Returns what session.poll returns (false once the
+      # session is closed).
       def poll(steps = 8)
+        ok = pump(steps)
+        deliver
+        fire_timers
+        ok
+      end
+
+      # What a waiting service call polls: the session and the services, not
+      # the subscribe / every blocks (those run from node.poll only).
+      def pump(steps = 8)
         ok = @session.poll(steps)
-        @services.each { |sv| sv.handle_pending }
+        i = 0
+        while i < @services.size
+          @services[i].handle_pending
+          i += 1
+        end
         ok
       end
 
+      # While loops, not each with a block: the application's blocks are
+      # called from here, and nothing else should stand between them and
+      # the update loop on the stack.
+      def deliver
+        i = 0
+        while i < @handled.size
+          sub = @handled[i]
+          if sub.closed?
+            @handled.delete_at(i)
+            next
+          end
+          got = sub.each_pending
+          j = 0
+          while j < got.size
+            handle(sub, got[j][0], got[j][1])
+            j += 1
+          end
+          i += 1
+        end
+      end
+
+      def fire_timers
+        return if @timers.empty?
+        now = ::Asterism::ROS.now_ms
+        due = []
+        i = 0
+        while i < @timers.size
+          due << @timers[i] if @timers[i].due?(now)
+          i += 1
+        end
+        i = 0
+        while i < due.size
+          fire(due[i], now)
+          i += 1
+        end
+      end
+
+      # One message for one subscribe block, one timer firing (CRuby routes
+      # what they raise to on_error).
+      def handle(sub, msg, info)
+        sub.handler.call(msg, info)
+      end
+
+      def fire(timer, now)
+        timer.fire(now)
+      end
+
       # Withdraws the node and its publishers, subscriptions, services and
       # clients. Idempotent.
       def close
         @entities.each { |e| e.close }
         @entities = []
         @services = []
+        @handled = []
+        @timers = []
         @clients = {}
         @token.close if @token
         @token = nil
@@ -419,8 +536,10 @@ module Asterism
       # take their defaults).
       def publish(msg)
         raise ::Asterism::Zenoh::Error, "publisher closed" if @token.nil?
-        @sequence += 1
-        att = ::Asterism::ROS::Attachment.new(@sequence, ::Asterism::ROS.now_ns, @gid)
+        # Read once (a second thread may publish meanwhile on CRuby).
+        seq = @sequence + 1
+        @sequence = seq
+        att = ::Asterism::ROS::Attachment.new(seq, ::Asterism::ROS.now_ns, @gid)
         @session.put(@topic_key, @type.encode(msg), attachment: att.encode)
         nil
       end
@@ -440,6 +559,8 @@ module Asterism
 
     class Subscription
       attr_reader :topic_key, :token_key, :errors
+      # The block of node.subscribe (nil for node.subscription).
+      attr_accessor :handler
 
       def initialize(node, session, topic, type, qos, depth)
         @type = type
@@ -480,6 +601,47 @@ module Asterism
         @token = nil
         nil
       end
+
+      def closed?
+        @token.nil?
+      end
+    end
+
+    # node.every: a block called every period, from node.poll.
+    class Timer
+      # period: seconds as given; fired: how many times it ran.
+      attr_reader :period, :fired
+
+      def initialize(node, seconds, blk)
+        @period_ms = (seconds * 1000).to_i
+        raise ArgumentError, "the period must be positive" unless @period_ms > 0
+        @node = node
+        @period = seconds
+        @blk = blk
+        @next = ::Asterism::ROS.now_ms + @period_ms
+        @fired = 0
+      end
+
+      def due?(now_ms)
+        now_ms >= @next
+      end
+
+      # Keeps the period without drifting; after a long stall it starts
+      # again from now instead of firing the missed times at once.
+      def fire(now_ms)
+        @next += @period_ms
+        @next = now_ms + @period_ms if @next <= now_ms
+        @fired += 1
+        @blk.call
+      end
+
+      def cancel
+        @node.cancel_timer(self)
+      end
+
+      def inspect
+        "#<Asterism::ROS::Timer every #{@period} s>"
+      end
     end
   end
 end
@@ -600,6 +762,8 @@ module Asterism
       def collect
         return if @finished
         g = @get
+        # done? before taking the replies, as in Asterism::Future#collect.
+        ended = g.done?
         # The Array form, as in Asterism::Future (no block called from C).
         g.each_reply.each do |r|
           next unless @response.nil?
@@ -613,7 +777,7 @@ module Asterism
           end
         end
         now = ::Asterism::ROS.now_ms
-        if !@response.nil? || g.done? || now >= @deadline
+        if !@response.nil? || ended || now >= @deadline
           @nobody = @response.nil? && now < @deadline
           @finished = true
           @took_ms = now - @started
@@ -647,11 +811,15 @@ module Asterism
         raise ::Asterism::Zenoh::Error, "client closed" if @token.nil?
         req = request.nil? ? fields : request
         payload = @type::Request.encode(req)
-        @sequence += 1
-        att = ::Asterism::ROS::Attachment.new(@sequence, ::Asterism::ROS.now_ns, @gid)
+        # Read once into a local: the request and its Call carry the same
+        # number even when another thread (CRuby) calls through this client
+        # meanwhile.
+        seq = @sequence + 1
+        @sequence = seq
+        att = ::Asterism::ROS::Attachment.new(seq, ::Asterism::ROS.now_ns, @gid)
         g = @session.get(@service_key, timeout_ms, nil, payload, attachment: att.encode,
                          target: :all_complete, consolidation: :none)
-        ::Asterism::ROS::Call.new(self, g, @sequence, timeout_ms)
+        ::Asterism::ROS::Call.new(self, g, seq, timeout_ms)
       end
 
       # Sends the request and waits for the response (polling the node, so
@@ -661,9 +829,11 @@ module Asterism
         call_async(request, timeout_ms: timeout_ms, **fields).value
       end
 
+      # Polls the node's session and services (not its subscribe / every
+      # blocks) until the call is done.
       def wait_for(c)
         until c.done?
-          break unless @node.poll
+          break unless @node.pump
           break if c.done?
           ::Asterism::ROS.pause(WAIT_STEP_MS)
         end

metadata

@@ -1,7 +1,7 @@
 --- !ruby/object:Gem::Specification
 name: asterism
 version: !ruby/object:Gem::Version
-  version: 0.1.0
+  version: 0.3.0
 platform: ruby
 authors:
 - Katsuhiko Kageyama
@@ -16,14 +16,14 @@ dependencies:
     requirements:
     - - "~>"
       - !ruby/object:Gem::Version
-        version: 0.1.0
+        version: 0.3.0
   type: :runtime
   prerelease: false
   version_requirements: !ruby/object:Gem::Requirement
     requirements:
     - - "~>"
       - !ruby/object:Gem::Version
-        version: 0.1.0
+        version: 0.3.0
 - !ruby/object:Gem::Dependency
   name: msgpack
   requirement: !ruby/object:Gem::Requirement
@@ -115,6 +115,10 @@ files:
 - data/msgs/std_msgs/msg/UInt8MultiArray.rb
 - lib/asterism.rb
 - lib/asterism/cruby.rb
+- lib/asterism/cruby/objects.rb
+- lib/asterism/cruby/ros.rb
+- lib/asterism/cruby/runner.rb
+- lib/asterism/cruby/zenoh.rb
 - lib/asterism/version.rb
 - mrblib/asterism.rb
 - mrblib/cdr.rb