rosruby 0.0.1

data/lib/ros/ros.rb

@@ -0,0 +1,10 @@
+# ros/ros.rb
+#
+# License: BSD
+#
+# Copyright (C) 2012  Takashi Ogura <t.ogura@gmail.com>
+#
+# namespace ROS
+
+module ROS
+end

data/lib/ros/roscore.rb

@@ -0,0 +1,29 @@
+#!/usr/bin/env ruby
+
+require 'ros/master'
+require 'ros'
+
+module ROS
+
+  def self.start_roscore
+    # master
+    thread = Thread.new do 
+      ROS::Master.new.start
+    end
+    
+    sleep 1
+    
+    # rosout
+    rosout_node = ROS::Node.new('/rosout', :nologger=>true)
+    rosout_agg_publisher = rosout_node.advertise('/rosout_agg', Rosgraph_msgs::Log)
+    rosout_node.subscribe('/rosout', Rosgraph_msgs::Log) do |msg|
+      rosout_agg_publisher.publish(msg)
+    end
+    rosout_node.spin
+  end
+end
+
+if $0 == __FILE__
+  ROS::start_roscore
+end
+

data/lib/ros/service.rb

@@ -0,0 +1,37 @@
+# ros/service.rb
+#
+# License: BSD
+#
+# Copyright (C) 2012  Takashi Ogura <t.ogura@gmail.com>
+#
+# Super Class of ServiceServer and ServiceClient
+#
+
+module ROS
+
+  ##
+  # Super Class of ServiceServer and ServiceClient
+  #
+  class Service
+
+    #
+    # @param [String] caller_id caller_id of this node
+    # @param [String] service_name name of service (String)
+    # @param [class] service_type class of Service
+    def initialize(caller_id, service_name, service_type)
+      @caller_id = caller_id
+      @service_name = service_name
+      @service_type = service_type
+    end
+
+    # @return [String] caller id of this node
+    attr_reader :caller_id
+
+    # @return [String] service name (like '/add_two_ints')
+    attr_reader :service_name
+
+    # @return [Class] class instance of srv converted class (like Std_msgs/String)
+    attr_reader :service_type
+
+  end
+end

data/lib/ros/service_client.rb

@@ -0,0 +1,83 @@
+# ros/service_client.rb
+#
+# License: BSD
+#
+# Copyright (C) 2012  Takashi Ogura <t.ogura@gmail.com>
+#
+
+require 'ros/service'
+require 'ros/tcpros/service_client'
+require 'uri'
+
+module ROS
+
+
+  # This is an interface of ROS Service.
+  # {Node#service} returns {ServiceClient} instance.
+  # @example
+  #   node = ROS::Node.new('/rosruby/sample_service_client')
+  #   if node.wait_for_service('/add_two_ints', 1)
+  #     service = node.service('/add_two_ints', Roscpp_tutorials::TwoInts)
+  #     req = Roscpp_tutorials::TwoInts.request_class.new
+  #     res = Roscpp_tutorials::TwoInts.response_class.new
+  #     req.a = 1
+  #     req.b = 2
+  #     if service.call(req, res)
+  #        p res.sum
+  #      end
+  #    end
+  class ServiceClient < Service
+
+    # @param [String] master_uri URI of ROS Master
+    # @param [String] caller_id caller id of this node
+    # @param [String] service_name name of service
+    # @param [Class] service_type class of srv
+    # @param [Boolean] persistent use persistent connection with server or not.
+    def initialize(master_uri, caller_id, service_name, service_type, persistent=false)
+      super(caller_id, service_name, service_type)
+      @master_uri = master_uri
+      @persistent = persistent
+    end
+
+    ##
+    # get hostname and port from uri
+    # @param [String] uri decompose uri string to host and port
+    # @return [Array<String, Integer>] [host, port]
+    def get_host_port_from_uri(uri) #:nodoc:
+      uri_data = URI.split(uri)
+      [uri_data[2], uri_data[3]]
+    end
+
+    # call service
+    # @param [Message] srv_request srv Request instance
+    # @param [Message] srv_response srv Response instance
+    # @return [Boolean] result of call
+    def call(srv_request, srv_response)
+      if @persistent and @connection
+        # not connect
+      else
+        master = XMLRPC::Client.new2(@master_uri)
+        code, message, uri = master.call('lookupService',
+                                         @caller_id,
+                                         @service_name)
+        case code
+        when 1
+          host, port = get_host_port_from_uri(uri)
+          @connection = TCPROS::ServiceClient.new(host, port, @caller_id, @service_name, @service_type, @persistent)
+          return @connection.call(srv_request, srv_response)
+        when -1
+          raise "master error ${message}"
+        else
+          puts "fail to lookup"
+          nil
+        end
+      end
+    end
+
+    # shutdown this service client
+    def shutdown
+      @connection.close
+    end
+
+  end
+end

data/lib/ros/service_server.rb

@@ -0,0 +1,92 @@
+# ros/service_server.rb
+#
+# License: BSD
+#
+# Copyright (C) 2012  Takashi Ogura <t.ogura@gmail.com>
+#
+# server of ROS Service.
+# This uses ROS::TCPROS::ServiceServer for data transfer.
+#
+require 'ros/service'
+require 'ros/tcpros/service_server'
+
+module ROS
+
+  # server of ROS Service.
+  # {Node#advertise_service} return a instance of this class.
+  # Service can be shutdown by {ServiceServer#shutdown}.
+  # This class uses {TCPROS::ServiceServer} for data transfer.
+  # @example
+  #   node = ROS::Node.new('/rosruby/sample_service_server')
+  #   server = node.advertise_service('/add_two_ints', Roscpp_tutorials::TwoInts) do |req, res|
+  #     res.sum = req.a + req.b
+  #     node.loginfo("a=#{req.a}, b=#{req.b}")
+  #     node.loginfo("  sum = #{res.sum}")
+  #     true
+  #   end
+  #   while node.ok?
+  #     sleep (1.0)
+  #   end
+  class ServiceServer < Service
+
+    # @param [String] caller_id caller id of this node
+    # @param [String] service_name name of this service (String)
+    # @param [Class] service_type class of srv
+    # @param [Proc] callback callback object of this service.
+    def initialize(caller_id, service_name, service_type, callback)
+      super(caller_id, service_name, service_type)
+      @callback = callback
+      @server = TCPROS::ServiceServer.new(@caller_id,
+                                          @service_name,
+                                          @service_type,
+                                          self)
+      @server.start
+      @num_request = 0
+    end
+
+    ##
+    # execute the service callback.
+    # User should not call this directly.
+    # @param [Message] request srv Request instance
+    # @param [Message] response srv Response instance
+    # @return [Boolean] callback result
+    def call(request, response)
+      @num_request += 1
+      @callback.call(request, response)
+    end
+
+    # URI of this service (rosrpc://**)
+    # @return [String] rosrpc service uri
+    def service_uri
+      'rosrpc://' + @server.host + ':' + @server.port.to_s
+    end
+
+    ##
+    # user should not call this method.
+    # Please use shutdown method.
+    def close #:nodoc:
+      @server.shutdown
+    end
+
+    ##
+    # shutdown the service connection
+    #
+    def shutdown
+      @manager.shutdow_service_server(self)
+    end
+
+    # set GraphManager for shutdown
+    # @param [GraphManager] manager set as manager
+    def set_manager(manager) #:nodoc:
+      @manager = manager
+    end
+
+    # @return [Array] connection data
+    def get_connection_data
+      [@num_request, @server.byte_received, @server.byte_sent]
+    end
+
+    # @return [Integer] how many times this service called
+    attr_reader :num_request
+  end
+end

data/lib/ros/slave_proxy.rb

@@ -0,0 +1,153 @@
+# ros/slave_proxy.rb
+#
+# License: BSD
+#
+# Copyright (C) 2012 Takashi Ogura <t.ogura@gmail.com>
+#
+#
+# == ROS Slave Proxy
+# you can access to ROS Slave nodes.
+#
+#
+
+require 'xmlrpc/client'
+
+module ROS
+
+  # you can access to ROS Slave nodes.
+  # @see http://ros.org/wiki/ROS/Slave_API
+  class SlaveProxy
+
+    # @param [String] caller_id caller id of this node
+    # @param [String] slave_uri URI to connect
+    def initialize(caller_id, slave_uri)
+      @caller_id = caller_id
+      @slave_uri = slave_uri
+      @proxy = XMLRPC::Client.new2(@slave_uri).proxy
+    end
+
+    # @return [Array] stats
+    # @raise [RuntimeError] if fail
+    def get_bus_stats
+      code, message, stats = @proxy.getBusStats(@caller_id)
+      if code == 1
+        return stats
+      else
+        raise message
+      end
+    end
+
+    # @return [Array] bus information
+    # @raise [RuntimeError] if fail
+    def get_bus_info
+      code, message, info = @proxy.getBusInfo(@caller_id)
+      if code == 1
+        return info
+      else
+        raise message
+      end
+    end
+
+    # @return [String] URI of master
+    # @raise [RuntimeError] if fail
+    def get_master_uri
+      code, message, uri = @proxy.getMasterUri(@caller_id)
+      if code == 1
+        return uri
+      else
+        raise message
+      end
+    end
+
+    # @param [String] msg message to slave (reason of shutdown request)
+    # @raise [RuntimeError] if fail
+    def shutdown(msg='')
+      code, message, val = @proxy.shutdown(@caller_id, msg)
+      if code == 1
+        return true
+      elsif code == 0
+        return false
+      else
+        raise message
+      end
+    end
+
+    # @return [Integer] pid of the slave process
+    # @raise [RuntimeError] if fail
+    def get_pid
+      code, message, pid = @proxy.getPid(@caller_id)
+      if code == 1
+        return pid
+      else
+        raise message
+      end
+    end
+
+    # @return [Array] topiccs
+    # @raise [RuntimeError] if fail
+    def get_subscriptions
+      code, message, topic = @proxy.getSubscriptions(@caller_id)
+      if code == 1
+        return topic
+      else
+        raise message
+      end
+    end
+
+    # @return [Array] publications
+    # @raise [RuntimeError] if fail
+    def get_publications
+      code, message, topic = @proxy.getPublications(@caller_id)
+      if code == 1
+        return topic
+      else
+        raise message
+      end
+    end
+
+    # @param [String] param_key  key for param
+    # @param [String, Integer, Float, Boolean, Array] param_value  new value for key
+    # @return [Boolean] true
+    # @raise [RuntimeError] if fail
+    def param_update(param_key, param_value)
+      code, message, val = @proxy.paramUpdate(@caller_id, param_key, param_value)
+      if code == 1
+        return true
+      else
+        raise message
+      end
+    end
+
+    # @param [String] topic name of topic
+    # @param [Array] publishers array of publisher uri
+    # @return [Boolean] true
+    # @raise [RuntimeError] if fail
+    def publisher_update(topic, publishers)
+      code, message, val = @proxy.publisherUpdate(@caller_id, topic, publishers)
+      if code == 1
+        return true
+      else
+        raise message
+      end
+    end
+
+    # @param [String] topic name of topic
+    #
+    def request_topic(topic, protocols)
+      code, message, protocol = @proxy.requestTopic(@caller_id,
+                                                    topic,
+                                                    protocols)
+      if code == 1
+        return protocol
+      else
+        raise message
+      end
+    end
+
+    # @return [String] URI of target
+    attr_accessor :slave_uri
+
+    # @return [String] caller_id of this caller
+    attr_accessor :caller_id
+  end
+end

data/lib/ros/subscriber.rb

@@ -0,0 +1,119 @@
+# ros/subscriber.rb
+#
+# License: BSD
+#
+# Copyright (C) 2012  Takashi Ogura <t.ogura@gmail.com>
+#
+require 'ros/topic'
+require 'ros/tcpros/client'
+require 'ros/slave_proxy'
+
+module ROS
+
+  # subscriber of ROS topic. This is created by ROS::Node#subscribe.
+  # Please use proc block for callback method.
+  # It uses ROS::TCPROS::Client for message transfer.
+  # Subscription can be shutdown by this ROS::Subscriber#shutdown
+  #   node = ROS::Node.new('/rosruby/sample_subscriber')
+  #   sub = node.subscribe('/chatter', Std_msgs::String) do |msg|
+  #     puts "message come! = \'#{msg.data}\'"
+  #   end
+  #   while node.ok?
+  #     node.spin_once
+  #     sleep(1)
+  #   end
+  class Subscriber < Topic
+
+    # @param [String] caller_id caller id of this node
+    # @param [String] topic_name name of this topic (String)
+    # @param [Class] topic_type class of msg
+    # @param [Proc] callback callback for this topic
+    # @param [Boolean] tcp_no_delay use tcp no delay option or not
+    def initialize(caller_id, topic_name, topic_type, callback=nil, tcp_no_delay=nil)
+      super(caller_id, topic_name, topic_type)
+      @callback = callback
+      @tcp_no_delay = tcp_no_delay
+    end
+
+    # @return [Boolean] use tcp no delay option or not
+    attr_reader :tcp_no_delay
+
+    # @return [Proc] callback of this subscription
+    attr_reader :callback
+
+    ##
+    # execute callback for all queued messages.
+    # This is called by {Node#spin_once}.
+    # It checks all queues of connections and callback for all messages.
+    def process_queue #:nodoc:
+      @connections.each do |connection|
+        while not connection.msg_queue.empty?
+          msg = connection.msg_queue.pop
+          if @callback
+            @callback.call(msg)
+          end
+        end
+      end
+    end
+
+    ##
+    # request topic to master and start connection with publisher.
+    # @param [String] uri uri to connect
+    # @return [TCPROS::Client] new connection
+    def add_connection(uri) #:nodoc:
+      publisher = SlaveProxy.new(@caller_id, uri)
+      begin
+        protocol, host, port = publisher.request_topic(@topic_name, [["TCPROS"]])
+        if protocol == "TCPROS"
+          connection = TCPROS::Client.new(host, port, @caller_id, @topic_name, @topic_type, uri, @tcp_no_delay)
+          connection.start
+        else
+          puts "not support protocol: #{protocol}"
+          raise "not support protocol: #{protocol}"
+        end
+        connection.id = "#{@topic_name}_in_#{@connection_id_number}"
+        @connection_id_number += 1
+        @connections.push(connection)
+        return connection
+      rescue
+        puts "request fail"
+        return false
+      end
+    end
+
+    ##
+    # data of connection. for slave API
+    # @return [Array] connection data
+    def get_connection_data
+      @connections.map do |connection|
+        [connection.id, connection.byte_received, 1]
+      end
+    end
+
+    ##
+    # connection information fro slave API
+    # @return [Array] connection info
+    def get_connection_info
+      info = []
+      @connections.each do |connection|
+        info.push([connection.id, connection.target_uri, 'i', connection.protocol, @topic_name])
+      end
+      info
+    end
+
+    def has_connection_with?(uri)
+      get_connected_uri.include?(uri)
+    end
+
+    def get_connected_uri
+      @connections.map {|x| x.target_uri}
+    end
+
+    ##
+    # user interface of shutdown this subscriber
+    #
+    def shutdown
+      @manager.shutdown_subscriber(self)
+    end
+  end
+end