rosruby 0.0.1

data/lib/ros/tcpros/client.rb

@@ -0,0 +1,108 @@
+#  ros/tcpros/client.rb
+#
+# License: BSD
+#
+# Copyright (C) 2012  Takashi Ogura <t.ogura@gmail.com>
+#
+
+require 'socket'
+require 'ros/tcpros/message'
+require 'ros/tcpros/header'
+
+module ROS::TCPROS
+
+  ##
+  # rosrpc's client for subscriber
+  #
+  class Client
+
+    include ::ROS::TCPROS::Message
+
+    ##
+    # @param [String] host host name
+    # @param [Integer] port port number
+    # @param [String] caller_id caller id of this node
+    # @param [String] topic_name name of this topic
+    # @param [Class] topic_type type of topic
+    # @param [String] target_uri URI of connection target
+    # @param [Boolean] tcp_no_delay use tcp no delay option or not
+    def initialize(host, port,
+                   caller_id, topic_name, topic_type, target_uri,
+                   tcp_no_delay)
+      @caller_id = caller_id
+      @topic_name = topic_name
+      @topic_type = topic_type
+      @port = port
+      @host = host
+      @target_uri = target_uri
+      @msg_queue = Queue.new
+      @socket = TCPSocket.open(@host, @port)
+      @tcp_no_delay = tcp_no_delay
+      if tcp_no_delay
+        @socket.setsockopt(Socket::IPPROTO_TCP, Socket::TCP_NODELAY, 1)
+      end
+      @byte_received = 0
+      @is_running = true
+    end
+
+    ##
+    # build header data for subscription.
+    # @return [TCPROS::Header] built header
+    def build_header
+      header = Header.new
+      header.push_data("callerid", @caller_id)
+      header.push_data("topic", @topic_name)
+      header.push_data("md5sum", @topic_type.md5sum)
+      header.push_data("type", @topic_type.type)
+      if @tcp_no_delay
+        header.push_data("tcp_nodelay", '1')
+      else
+        header.push_data("tcp_nodelay", '0')
+      end
+      header
+    end
+
+    ##
+    # start recieving data.
+    # The received messages are pushed into a message queue.
+    def start
+      write_header(@socket, build_header)
+      read_header(@socket)
+      @thread = Thread.start do
+        while @is_running
+          data = read_all(@socket)
+          msg = @topic_type.new
+          msg.deserialize(data)
+          @byte_received += data.length
+          @msg_queue.push(msg)
+        end
+      end
+    end
+
+    ##
+    # close the connection.
+    # kill the thread if it is not response.
+    def shutdown
+      @is_running = false
+      if not @thread.join(0.1)
+        Thread::kill(@thread)
+      end
+      if not @socket.closed?
+        @socket.close
+      end
+    end
+
+    # @return [Integer] port number of this client
+    attr_reader :port
+    # @return [String] host name
+    attr_reader :host
+    # @return [Queue] message queue
+    attr_reader :msg_queue
+    # @return [Integer] received byte
+    attr_reader :byte_received
+    # @return [String] URI of connection target
+    attr_reader :target_uri
+    # @return [String] id for slave API
+    attr_accessor :id
+  end
+end

data/lib/ros/tcpros/header.rb

@@ -0,0 +1,89 @@
+# ros/tcpros/message.rb
+#
+# License: BSD
+#
+# Copyright (C) 2012  Takashi Ogura <t.ogura@gmail.com>
+#
+
+require 'ros/ros'
+
+module ROS::TCPROS
+
+  ##
+  # header of rorpc's protocol
+  #
+  class Header
+
+    # rosrpc uses this wild card for cancel md5sum check or any.
+    WILD_CARD = '*'
+
+    # initialize with hash
+    # @param [Hash] data
+    def initialize(data={})
+      @data = data
+    end
+
+    # add key-value data to this header.
+    # @param [String] key key for header
+    # @param [String] value value for key
+    # @return [Header] self
+    def push_data(key, value)
+      if (not key.kind_of?(String)) or (not value.kind_of?(String))
+        raise ArgumentError::new('header key and value must be string')
+      end
+      @data[key] = value
+      self
+    end
+
+    # @param [String] key
+    # @return [String] value of key
+    def get_data(key)
+      @data[key]
+    end
+
+    alias_method :[]=, :push_data
+    alias_method :[], :get_data
+
+    # deserialize the data to header.
+    # this does not contain total byte number.
+    # @param [String] data
+    # @return [Header] self
+    def deserialize(data)
+      while data.length > 0
+        len, data = data.unpack('Va*')
+        msg = data[0..(len-1)]
+        equal_position = msg.index('=')
+        key = msg[0..(equal_position-1)]
+        value = msg[(equal_position+1)..-1]
+        @data[key] = value
+        data = data[(len)..-1]
+      end
+      self
+    end
+
+    ##
+    # validate the key with the value.
+    # if it is WILD_CARD, it is ok!
+    # @param [String] key
+    # @param [String] value
+    # @return [Boolean] check result
+    def valid?(key, value)
+      (@data[key] == value) or value == WILD_CARD
+    end
+
+    # serialize the data into header.
+    # return the byte of the serialized data.
+    # @param [IO] buff where to write data
+    # @return [Integer] byte of serialized data
+    def serialize(buff)
+      serialized_data = ''
+      @data.each_pair do |key, value|
+        data_str = key + '=' + value
+        serialized_data = serialized_data + [data_str.length, data_str].pack('Va*')
+      end
+      total_byte = serialized_data.length
+      return buff.write([total_byte, serialized_data].pack('Va*'))
+    end
+
+  end
+end

data/lib/ros/tcpros/message.rb

@@ -0,0 +1,74 @@
+# ros/tcpros/message.rb
+#
+# License: BSD
+#
+# Copyright (C) 2012  Takashi Ogura <t.ogura@gmail.com>
+#
+# super class of TCPROS connections
+# document is http://ros.org/wiki/ROS/TCPROS
+#
+require 'stringio'
+require 'ros/tcpros/header'
+
+
+# TCP connection between nodes.
+# protocol document is http://ros.org/wiki/ROS/TCPROS
+module ROS::TCPROS
+
+  # functions for TCPROS
+  module Message
+
+    ##
+    # write message to socket.
+    # @param [IO] socket socket for writing
+    # @param [Message] msg message for writing
+    # @return [String] wrote data
+    def write_msg(socket, msg)
+      sio = StringIO.new('', 'r+')
+      len = msg.serialize(sio)
+      sio.rewind
+      data = sio.read
+      len = data.length
+      data = [len, data].pack("La#{len}")
+      socket.write(data)
+      data
+    end
+
+    ##
+    # read the size of data and read it from socket.
+    # @param [IO] socket socket for reading
+    # @return [String] received data
+    def read_all(socket)
+      total_bytes = socket.recv(4).unpack("V")[0]
+      if total_bytes and total_bytes > 0
+        socket.recv(total_bytes)
+      else
+        ''
+      end
+    end
+
+    ##
+    # read a connection header from socket
+    # @param [String] socket socket for reading
+    # @return [Header] header
+    def read_header(socket)
+      header = ::ROS::TCPROS::Header.new
+      header.deserialize(read_all(socket))
+      header
+    end
+
+    ##
+    # write a connection header to socket.
+    # @param [IO] socket socket for writing.
+    # @param [Header] header header data
+    def write_header(socket, header)
+      header.serialize(socket)
+      socket.flush
+    end
+
+    # @return [String] prototol string 'TCPROS'
+    def protocol
+      'TCPROS'
+    end
+  end
+end

data/lib/ros/tcpros/server.rb

@@ -0,0 +1,137 @@
+# ros/subscriber.rb
+#
+# License: BSD
+#
+# Copyright (C) 2012  Takashi Ogura <t.ogura@gmail.com>
+#
+require 'ros/tcpros/message'
+require 'gserver'
+
+module ROS::TCPROS
+
+  ##
+  # This is TCPROS connection for {ROS::Publihser}
+  #
+  class Server < ::GServer
+
+    include ::ROS::TCPROS::Message
+
+    # max number of connections with {TCPROS::Client}
+    MAX_CONNECTION = 100
+
+    ##
+    # @param [String] caller_id caller id of this node
+    # @param [String] topic_name name of this topic
+    # @param [Class] topic_type type of topic
+    # @param [Hash] options :latched, :port, host, :last_published_msg
+    # @option [Boolean] options :latched (false)
+    # @option [Integer] options :port (0) tcp port number.
+    # @option [String] options :host (GServer::DEFAULT_HOST) host name
+    # @option [Message] options :last_published_msg
+    def initialize(caller_id, topic_name, topic_type, options={})
+      if options[:port]
+        port = options[:port]
+      else
+        port = 0
+      end
+      if options[:host]
+        host = options[:host]
+      else
+        host = GServer::DEFAULT_HOST
+      end
+
+      super(port, host, MAX_CONNECTION)
+      @caller_id = caller_id
+      @topic_name = topic_name
+      @topic_type = topic_type
+      @msg_queue = Queue.new
+      @is_latched = options[:latched]
+      @last_published_msg = options[:last_published_msg]
+      @byte_sent = 0
+      @num_sent = 0
+    end
+
+    ##
+    # Is this latching publisher?
+    # @return [Boolean] is latched or not
+    def latching?
+      @is_latched
+    end
+
+    ##
+    # send a message to reciever
+    # @param [IO] socket socket for writing
+    # @param [Class] msg msg class instance
+    def publish_msg(socket, msg)
+      data = write_msg(socket, msg)
+      @last_published_msg = msg
+      # for getBusStats
+      @byte_sent += data.length
+      @num_sent += 1
+    end
+
+    ##
+    # this is called if a socket accept a connection.
+    # This is GServer's function
+    # @param [IO] socket given socket
+    def serve(socket) #:nodoc:
+      header = read_header(socket)
+      if check_header(header)
+        if header['tcp_nodelay'] == '1'
+          socket.setsockopt(Socket::IPPROTO_TCP, Socket::TCP_NODELAY, 1)
+        end
+        begin
+          write_header(socket, build_header)
+          if latching?
+            if @last_published_msg
+              publish_msg(socket, @last_published_msg)
+            end
+          end
+          loop do
+            @last_published_msg = @msg_queue.pop
+            publish_msg(socket, @last_published_msg)
+          end
+        rescue
+          socket.shutdown
+        end
+      else
+        socket.shutdown
+        p "header check error: #{header}"
+        raise 'header check error'
+      end
+    end
+
+    attr_reader :caller_id, :msg_queue, :byte_sent, :num_sent
+
+    # id for slave API
+    # @return [String]
+    attr_accessor :id
+    attr_accessor :last_published_msg
+
+    ##
+    # validate header for this publisher
+    # @param [Header] header for checking
+    # @return [Boolean] ok(true) or not
+    def check_header(header)
+      header.valid?('type', @topic_type.type) and
+        header.valid?('md5sum', @topic_type.md5sum)
+    end
+
+    ##
+    # build {TCPROS::Header} message for this publisher.
+    # It contains callerid, topic, md5sum, type, latching.
+    # @return [Header] built header
+    def build_header
+      header = Header.new
+      header["callerid"] = @caller_id
+      header["topic"] = @topic_name
+      header["md5sum"] = @topic_type.md5sum
+      header["type"] = @topic_type.type
+      if latching?
+        header["latching"]  = '1'
+      end
+      header
+    end
+
+  end
+end

data/lib/ros/tcpros/service_client.rb

@@ -0,0 +1,104 @@
+# ros/tcpros/service_client.rb
+#
+# License: BSD
+#
+# Copyright (C) 2012  Takashi Ogura <t.ogura@gmail.com>
+#
+require 'socket'
+require 'ros/tcpros/header'
+require 'ros/tcpros/message'
+
+module ROS::TCPROS
+
+  ##
+  # TCPROS protocol service client connection
+  #
+  class ServiceClient
+
+    include ::ROS::TCPROS::Message
+
+    # @param [String] host host name
+    # @param [Integer] port port number
+    # @param [String] caller_id caller_id of this node
+    # @param [String] service_name name of service
+    # @param [Class] service_type class of this service
+    # @param [Boolean] persistent use persistent connection or not
+    def initialize(host, port, caller_id, service_name, service_type, persistent)
+      @caller_id = caller_id
+      @service_name = service_name
+      @service_type = service_type
+      @port = port
+      @host = host
+      @socket = TCPSocket.open(@host, @port)
+      @persistent = persistent
+      @socket.setsockopt(Socket::IPPROTO_TCP, Socket::TCP_NODELAY, 1)
+    end
+
+    ##
+    # build henader message for service client.
+    # It contains callerid, service, md5sum, type, persistent.
+    # @return [Header] header
+    def build_header
+      header = Header.new
+      header.push_data("callerid", @caller_id)
+      header.push_data("service", @service_name)
+      header.push_data("md5sum", @service_type.md5sum)
+      header.push_data("type", @service_type.type)
+      if @persistent
+	header.push_data("persistent", '1')
+      end
+      header
+    end
+
+    ##
+    # call the service by sending srv request message,
+    # and receive response message.
+    # @param [Message] srv_request call with this request
+    # @param [Message] srv_response response is stored in this message
+    # @return [Boolean] result of call
+    def call(srv_request, srv_response)
+      write_header(@socket, build_header)
+      if check_header(read_header(@socket))
+	write_msg(@socket, srv_request)
+	@socket.flush
+	ok_byte = read_ok_byte
+	if ok_byte == 1
+	  srv_response.deserialize(read_all(@socket))
+	  return true
+	end
+	false
+      end
+      false
+    end
+
+    ##
+    # read ok byte for boolean service result.
+    # @return [Integer] 1 for OK, 0 for NG
+    def read_ok_byte
+      @socket.recv(1).unpack('c')[0]
+    end
+
+    ##
+    # check md5sum only.
+    # @param [Header] header received header
+    # @return [Boolean] true if it is ok.
+    def check_header(header)
+      header.valid?('md5sum', @service_type.md5sum)
+    end
+
+    ##
+    # close the socket
+    def shutdown
+      @socket.close
+    end
+
+    # port number of this socket
+    # @return [Integer] port number
+    attr_reader :port
+
+    # host of this connection
+    # @return [String] host name
+    attr_reader :host
+
+  end
+end