rosruby 0.0.1

data/lib/ros/master_proxy.rb

@@ -0,0 +1,256 @@
+# ros/master_proxy.rb
+#
+# License: BSD
+#
+# Copyright (C) 2012 Takashi Ogura <t.ogura@gmail.com>
+#
+#
+# == ROS Master Proxy
+# access to ROS Master
+#
+#
+
+require 'xmlrpc/client'
+
+module ROS
+
+  # == ROS Master Proxy
+  # access to ROS Master.
+  # @see http://ros.org/wiki/ROS/Master_API
+  # But there are not documented API.
+  #
+  class MasterProxy
+
+    #
+    # @param [String] caller_id caller_id of this node
+    # @param [String] master_uri URI of ROS Master
+    # @param [String] slave_uri slave URI of this node
+    def initialize(caller_id, master_uri, slave_uri)
+      @caller_id = caller_id
+      @master_uri = master_uri
+      @slave_uri = slave_uri
+      @proxy = XMLRPC::Client.new2(@master_uri).proxy
+    end
+
+    # register a service
+    # @param [String] service name of service
+    # @param [String] service_api service api uri
+    # @return [Boolean] true success
+    # @raise RuntimeError
+    def register_service(service, service_api)
+      code, message, val = @proxy.registerService(@caller_id,
+                                                  service,
+                                                  service_api,
+                                                  @slave_uri)
+      if code == 1
+        return true
+      else
+        raise message
+      end
+    end
+
+    # unregister a service
+    # @param [String] service name of service
+    # @param [String] service_api service api uri
+    # @return [Boolean] true success
+    # @raise RuntimeError
+    def unregister_service(service, service_api)
+      code, message, val = @proxy.unregisterService(@caller_id,
+                                                    service,
+                                                    service_api)
+      if code == 1
+        return true
+      elsif code == 0
+        puts message
+        return true
+      else
+        raise message
+      end
+    end
+
+    # register a subscriber
+    # @param [String] topic topic name
+    # @param [String] topic_type topic type
+    # @return [Array] URI of current publishers
+    # @raise [RuntimeError] if error
+    def register_subscriber(topic, topic_type)
+      code, message,val = @proxy.registerSubscriber(@caller_id,
+                                                    topic,
+                                                    topic_type,
+                                                    @slave_uri)
+      if code == 1
+        val
+      elsif code == 0
+        puts message
+        val
+      else
+        raise message
+      end
+    end
+
+    # unregister a subscriber
+    # @param [String] topic name of topic to unregister
+    # @return [Boolean] true
+    # @raise RuntimeError
+    def unregister_subscriber(topic)
+      code, message,val = @proxy.unregisterSubscriber(@caller_id,
+                                                      topic,
+                                                      @slave_uri)
+      if code == 1
+        return true
+      elsif code == 0
+        puts message
+        return true
+      else
+        raise message
+      end
+    end
+
+    # register a publisher
+    # @param [String] topic topic name of topic
+    # @param [String] topic_type type of topic
+    # @return [Array] URI of current subscribers
+    # @raise RuntimeError
+    def register_publisher(topic, topic_type)
+      code, message, uris = @proxy.registerPublisher(@caller_id,
+                                                     topic,
+                                                     topic_type,
+                                                     @slave_uri)
+      if code == 1
+        uris
+      else
+        raise message
+      end
+    end
+
+    # unregister a publisher
+    # @param [String] topic name of topic
+    # @return [Boolean] true
+    # @raise RuntimeError
+    def unregister_publisher(topic)
+      code, message, val = @proxy.unregisterPublisher(@caller_id,
+                                                      topic,
+                                                      @slave_uri)
+      if code == 1
+        return val
+      elsif code == 0
+        puts message
+        return true
+      else
+        raise message
+      end
+      return false
+    end
+
+    ##
+    # this method is not described in the wiki.
+    # subscribe to the parameter key.
+    # @param [String] key name of parameter
+    # @return [Boolean] true
+    # @raise [RuntimeError] if fail
+    def subscribe_param(key)
+      code, message, uri = @proxy.subscribeParam(@caller_id, @slave_uri, key)
+      if code == 1
+        return true
+      else
+        raise message
+      end
+    end
+
+    ##
+    # unsubscribe to the parameter key.
+    # this method is not described in the wiki.
+    # @param [String] key name of parameter key
+    # @return [Boolean] true
+    # @raise [RuntimeError] if failt
+    def unsubscribe_param(key)
+      code, message, uri = @proxy.unsubscribeParam(@caller_id, @slave_uri, key)
+      if code == 1
+        return true
+      else
+        raise message
+      end
+    end
+
+    # lookup a node by name.
+    # @param [String] node_name
+    # @return [String, nil] URI of the node if it is found. nil not found.
+    def lookup_node(node_name)
+      code, message, uri = @proxy.lookupNode(@caller_id, node_name)
+      if code == 1
+        uri
+      else
+        nil
+      end
+    end
+
+    # get the all published topics
+    # @param [String] subgraph namespace for check
+    # @return [Array] topic names.
+    # @raise
+    def get_published_topics(subgraph='')
+      code, message, topics = @proxy.getPublishedTopics(@caller_id, subgraph)
+      if code == 1
+        return topics
+      elsif
+        raise message
+      end
+    end
+
+    # get system state
+    # @return [Array] state
+    def get_system_state
+      code, message, state = @proxy.getSystemState(@caller_id)
+      if code == 1
+        return state
+      else
+        raise message
+      end
+    end
+
+    # get the master URI
+    # @return [String] uri
+    # @raise
+    def get_uri
+      code, message, uri = @proxy.getUri(@caller_id)
+      if code == 1
+        return uri
+      else
+        raise message
+      end
+    end
+
+    # look up a service by name
+    # @param [String] service name of service
+    # @return [String, nil] URI of service if found, nil not found.
+    def lookup_service(service)
+      code, message, uri = @proxy.lookupService(@caller_id, service)
+      if code == 1
+        uri
+      else
+        false
+      end
+    end
+
+    # Master URI
+    # @return [String]
+    attr_reader :master_uri
+
+    # set the master uri
+    # @param [String] uri master uri
+    # @return [MasterProxy] self
+    def master_uri=(uri)
+      @master_uri = uri
+      @proxy = XMLRPC::Client.new2(@master_uri).proxy
+      self
+    end
+
+    # Slave URI
+    # @return [String]
+    attr_accessor :slave_uri
+
+    # caller id of this node
+    # @return [String]
+    attr_accessor :caller_id
+  end
+end

data/lib/ros/message.rb

@@ -0,0 +1,65 @@
+# ros/message.rb
+#
+# License: BSD
+#
+# Copyright (C) 2012  Takashi Ogura <t.ogura@gmail.com>
+#
+# == Message file for msg/srv (de)serialization
+#
+#
+
+module ROS
+
+  ##
+  # super class of all msg/srv converted class.
+  # Currently this is none.
+  class Message
+  end
+
+  ##
+  # used for serialization (for python like grammar)
+  # this is used by msg/srv converted *.rb files
+  # it can be removed, if there are more effective genmsg_ruby.
+  class Struct
+
+    # @param [String] format
+    def initialize(format)
+      @format = format
+    end
+
+    attr_reader :format
+
+    def self.calc_size(format)
+      array = []
+      start = 0
+      while start < format.length
+        re = /(\w)(\d*)/
+        re =~ format[start..(format.length-1)]
+        number = $2.to_i
+        if number == 0
+          array.push(0)
+        else
+          for i in 1..number
+            array.push(0)
+          end
+        end
+        start += $&.length
+      end
+      array.pack(format).length
+    end
+
+    # pack the data
+    # @param [Array] args
+    def pack(*args)
+      args.pack(@format)
+    end
+
+    # unpack from string
+    # @param [String] arg
+    # @return [Array] unpacked data
+    def unpack(arg)
+      arg.unpack(@format)
+    end
+
+  end
+end

data/lib/ros/name.rb

@@ -0,0 +1,88 @@
+# ros/name.rb
+#
+# License: BSD
+#
+# Copyright (C) 2012  Takashi Ogura <t.ogura@gmail.com>
+#
+# == Naming module
+#
+# ROS naming module.
+#
+
+require 'socket' # for gethostname
+
+module ROS
+
+  ##
+  # this module provides naming functions.
+  #
+  module Name
+
+    # name sparation char ('/')
+    SEP = '/'
+
+    ##
+    # start with '/' and use single '/' for names
+    #
+    # @param [String] name input name
+    # @return [String] canonicalized name
+    def canonicalize_name(name)
+      if name == nil or name == SEP
+        return name
+      elsif name[0] == SEP[0]
+        return name.split(/\/+/).join(SEP)
+      else
+        return SEP + name.split(/\/+/).join(SEP)
+      end
+    end
+
+    ##
+    # expand ~local_param like names.
+    #
+    # @param [String] caller_id caller id for replacing ~
+    # @param [String] name param name like '~param'
+    # @return [String] expanded name
+    def expand_local_name(caller_id, name)
+      if name[0] == '~'[0]
+        caller_id + SEP + name[1..-1]
+      else
+        name
+      end
+    end
+
+    ##
+    # generate anonymous name using input id.
+    # (arange from roslib)
+    # @param [String] id base id (String)
+    # @return [String] return generated id
+    def anonymous_name(id)
+      name = "#{id}_#{Socket.gethostname}_#{Process.pid}_#{rand(1000000)}"
+      name = name.gsub('.', '_')
+      name = name.gsub('-', '_')
+      name.gsub(':', '_')
+    end
+
+
+    ##
+    # expand local, canonicalize, remappings
+    # @param [String] caller_id caller_id
+    # @param [String] ns namespace
+    # @param [String] name target name
+    # @param [Array] remappings name remappings
+    # @return [String] resolved name
+    def resolve_name_with_call_id(caller_id, ns, name, remappings)
+      name = canonicalize_name(expand_local_name(caller_id, name))
+      if remappings
+        remappings.each_pair do |key, value|
+          if name == canonicalize_name(key)
+            name = value
+          end
+        end
+      end
+      if ns
+        name = ns + SEP + name
+      end
+      return canonicalize_name(name)
+    end
+  end
+end

data/lib/ros/node.rb

@@ -0,0 +1,442 @@
+# ros/node.rb
+#
+# License: BSD
+#
+# Copyright (C) 2012  Takashi Ogura <t.ogura@gmail.com>
+#
+# == ROS Node
+#
+# user interface of ROS.
+#
+require 'ros/parameter_manager'
+require 'ros/name'
+require 'ros/graph_manager'
+require 'ros/publisher'
+require 'ros/subscriber'
+require 'ros/parameter_subscriber'
+require 'ros/service_server'
+require 'ros/service_client'
+require 'ros/log'
+require 'ros/time'
+require 'ros/rate'
+require 'ros/duration'
+
+module ROS
+
+# main interface of rosruby.
+#
+# @example Sample for Publisher
+#   node = ROS::Node.new('/rosruby/sample_publisher')
+#   publisher = node.advertise('/chatter', Std_msgs::String)
+#   sleep(1)
+#   msg = Std_msgs::String.new
+#   i = 0
+#   while node.ok?
+#     msg.data = "Hello, rosruby!: #{i}"
+#     publisher.publish(msg)
+#
+# @example Sample for Subscriber
+#   node = ROS::Node.new('/rosruby/sample_subscriber')
+#   node.subscribe('/chatter', Std_msgs::String) do |msg|
+#     puts "message come! = \'#{msg.data}\'"
+#   end
+#
+#   while node.ok?
+#     node.spin_once
+#     sleep(1)
+#   end
+#
+  class Node
+
+    # for node shutdown hook
+    @@shutdown_hook = []
+
+    # Naming functions of ROS
+    include Name
+
+    ##
+    # initialization of ROS node
+    # get env, parse args, and start slave xmlrpc servers.
+    #
+    # @param [String] node_name name of this node
+    # @param [Hash] options options
+    # @option options [Boolean] :anonymous (false) use anonymous name if true. anonymous node generates a unique name
+    def initialize(node_name, options={})
+      @remappings = {}
+      # @host is rewrited by ARGS[ROS_IP] or ARGS[ROS_HOSTNAME]
+      @host = Socket.gethostname
+      get_env
+      if options[:anonymous]
+        node_name = anonymous_name(node_name)
+      end
+      @node_name = resolve_name(node_name)
+      @remappings = parse_args(ARGV)
+      if not @master_uri
+        raise 'ROS_MASTER_URI is nos set. please check environment variables'
+      end
+
+      @manager = GraphManager.new(@node_name, @master_uri, @host)
+      @parameter = ParameterManager.new(@master_uri, @node_name, @remappings)
+      if not options[:nologger]
+        @logger = ::ROS::Log.new(self)
+      end
+      # because xmlrpc server use signal trap, after serve, it have to trap sig      trap_signals
+      ObjectSpace.define_finalizer(self, proc {|id| self.shutdown})
+    end
+
+    ##
+    #  Is this node running? Please use for 'while loop' and so on..
+    #
+    # @return [Boolean] true if node is running.
+    #
+    def ok?
+      @manager.is_ok?
+    end
+
+    # URI of master
+    # @return [String] uri string of master
+    attr_reader :master_uri
+
+    # hostname of this node.
+    # @return [String] host name
+    attr_reader :host
+
+    # name of this node (caller_id).
+    # @return [String] name of this node (=caller_id)
+    attr_reader :node_name
+
+    ##
+    # resolve the name by this node's remapping rule.
+    # @param [String] name name for resolved
+    # @return [String] resolved name
+    #
+    def resolve_name(name)
+      resolve_name_with_call_id(@node_name, @ns, name, @remappings)
+    end
+
+    ##
+    # get the param for key.
+    # You can set default value. That is uesed when the key is not set yet.
+    # @param [String] key key for search the parameters
+    # @param [String, Integer, Float, Boolean] default default value
+    # @return [String, Integer, Float, Boolean] parameter value for key
+    #
+    def get_param(key, default=nil)
+      key = expand_local_name(@node_name, key)
+      param = @parameter.get_param(key)
+      if param
+        param
+      else
+        default
+      end
+    end
+
+    ##
+    # get all parameters.
+    #
+    # @return [Array] all parameter list
+    #
+    def get_param_names
+      @parameter.get_param_names
+    end
+
+    ##
+    # check if the parameter server has the param for 'key'.
+    # @param [String] key key for check
+    # @return [Boolean] true if exits
+    def has_param(key)
+      @parameter.has_param(expand_local_name(@node_name, key))
+    end
+
+    ##
+    # delete the parameter for 'key'
+    #
+    # @param [String] key key for delete
+    # @return [Boolean]  true if success, false if it is not exist
+    def delete_param(key)
+      @parameter.delete_param(expand_local_name(@node_name, key))
+    end
+
+    ##
+    # set parameter for 'key'.
+    # @param [String] key key of parameter
+    # @param [String, Integer, Float, Boolean] value value of parameter
+    # @return [Boolean] return true if succeed
+    def set_param(key, value)
+      @parameter.set_param(expand_local_name(@node_name, key), value)
+    end
+
+    ##
+    # start publishing the topic.
+    #
+    # @param [String] topic_name name of topic (string)
+    # @param [Class] topic_type topic class
+    # @param [Hash] options :latched, :resolve
+    # @option options [Boolean] :latched (false) latched topic
+    # @option options [Boolean] :resolve (true) resolve topic_name or not. This is for publish /rosout with namespaced node.
+    # @return [Publisher] Publisher instance
+    def advertise(topic_name, topic_type, options={})
+      if options[:no_resolve]
+        name = topic_name
+      else
+        name = resolve_name(topic_name)
+      end
+      publisher = Publisher.new(@node_name,
+                                name,
+                                topic_type,
+                                options[:latched],
+                                @manager.host)
+      @manager.add_publisher(publisher)
+      trap_signals
+      publisher
+    end
+
+    ##
+    # start service server.
+    #
+    # @param [String] service_name name of this service (string)
+    # @param [Service] service_type service class
+    # @param [Proc] callback service definition
+    # @return [ServiceServer] ServiceServer instance
+    def advertise_service(service_name, service_type, &callback)
+      server = ::ROS::ServiceServer.new(@node_name,
+                                        resolve_name(service_name),
+                                        service_type,
+                                        callback)
+      @manager.add_service_server(server)
+      trap_signals
+      server
+    end
+
+    ##
+    # wait until start the service.
+    # @param [String] service_name name of service for waiting
+    # @param [Float] timeout_sec time out seconds. default infinity.
+    # @return [Boolean] true if success, false if timeouted
+    def wait_for_service(service_name, timeout_sec=nil)
+      @manager.wait_for_service(service_name, timeout_sec)
+    end
+
+    ##
+    # create service client.
+    # @param [String] service_name name of this service (string)
+    # @param [Class] service_type service class
+    # @return [ServiceClient] created ServiceClient instance
+    def service(service_name, service_type)
+      ROS::ServiceClient.new(@master_uri,
+                             @node_name,
+                             resolve_name(service_name),
+                             service_type)
+    end
+
+    ##
+    # start to subscribe a topic.
+    #
+    # @param [String] topic_name name of topic (string)
+    # @param [Class] topic_type Topic instance
+    # @return [Subscriber] created Subscriber instance
+    def subscribe(topic_name, topic_type, &callback)
+      sub = Subscriber.new(@node_name,
+                           resolve_name(topic_name),
+                           topic_type,
+                           callback)
+      @manager.add_subscriber(sub)
+      trap_signals
+      sub
+    end
+
+    ##
+    # subscribe to the parameter.
+    #
+    # @param [String] param name of parameter to subscribe
+    # @param [Proc] callback callback when parameter updated
+    # @return [ParameterSubscriber] created ParameterSubscriber instance
+    def subscribe_parameter(param, &callback)
+      sub = ParameterSubscriber.new(param, callback)
+      @manager.add_parameter_subscriber(sub)
+      sub
+    end
+
+    ##
+    # spin once. This invoke subscription/service_server callbacks
+    #
+    def spin_once
+      @manager.spin_once
+    end
+
+    ##
+    # spin forever.
+    #
+    def spin
+      while ok?
+        spin_once
+        sleep(0.01)
+      end
+    end
+
+    ##
+    # unregister to master and shutdown all connections.
+    # @return [Node] self
+    def shutdown
+      if ok?
+        begin
+          @manager.shutdown
+        rescue => message
+          p message
+          puts 'ignoring errors while shutdown'
+        end
+      end
+      self
+    end
+
+    ##
+    # outputs log message for INFO (INFORMATION).
+    # @param [String] message message for output
+    # @return [Node] self
+    def loginfo(message)
+      file, line, function = caller[0].split(':')
+      @logger.log('INFO', message, file, function, line.to_i)
+      self
+    end
+
+    ##
+    # outputs log message for DEBUG
+    # @param [String] message message for output
+    # @return [Node] self
+    def logdebug(message)
+      file, line, function = caller[0].split(':')
+      @logger.log('DEBUG', message, file, function, line.to_i)
+      self
+    end
+
+    ##
+    # outputs log message for WARN (WARING).
+    #
+    # @param [String] message message for output
+    # @return [Node] self
+    def logwarn(message)
+      file, line, function = caller[0].split(':')
+      @logger.log('WARN', message, file, function, line.to_i)
+      self
+    end
+
+    ##
+    # outputs log message for ERROR.
+    #
+    # @param [String] message message for output
+    # @return [Node] self
+    def logerror(message)
+      file, line, function = caller[0].split(':')
+      @logger.log('ERROR', message, file, function, line.to_i)
+      self
+    end
+
+    alias_method :logerr, :logerror
+
+    ##
+    # outputs log message for FATAL.
+    #
+    # @param [String] message message for output
+    # @return [Node] self
+    def logfatal(message)
+      file, line, function = caller[0].split(':')
+      @logger.log('FATAL', message, file, function, line.to_i)
+      self
+    end
+
+    ##
+    # get all topics by this node.
+    #
+    # @return [Array] topic names
+    def get_published_topics
+      @manager.publishers.map do |pub|
+        pub.topic_name
+      end
+    end
+
+    private
+
+    ##
+    # parse all environment variables.
+    #
+    def get_env  #:nodoc:
+      @master_uri = ENV['ROS_MASTER_URI']
+      @ns = ENV['ROS_NAMESPACE']
+      if ENV['ROS_IP']
+        @host = ENV['ROS_IP']
+      elsif ENV['ROS_HOSTNAME']
+        @host = ENV['ROS_HOSTNAME']
+      end
+    end
+
+    ##
+    # converts strings if it is float and int numbers.
+    # @example
+    #   convert_if_needed('10') # => 10
+    #   convert_if_needed('0.1') # => 0.1
+    #   convert_if_needed('string') # => 'string'
+    # @param [String] value string
+    # @return [Float, Integer, String] return converted value.
+    def convert_if_needed(value)  #:nodoc:
+      if value =~ /^[+-]?\d+\.?\d*$/ # float
+        value = value.to_f
+      elsif value =~ /^[+-]?\d+$/ # int
+        value = value.to_i
+      else
+        value
+      end
+    end
+
+    ##
+    # parse all args.
+    # @param [Array] args arguments for parse
+    def parse_args(args) #:nodoc:
+      remapping = {}
+      for arg in args
+        splited = arg.split(':=')
+        if splited.length == 2
+          key, value = splited
+          if key == '__name'
+            @node_name = resolve_name(value)
+          elsif key == '__ip'
+            @host = value
+          elsif key == '__hostname'
+            @host = value
+          elsif key == '__master'
+            @master_uri = value
+          elsif key == '__ns'
+            @ns = value
+          elsif key[0] == '_'[0]
+            # local name remaps
+            key[0] = '~'
+            remapping[resolve_name(key)] = convert_if_needed(value)
+          else
+            # remaps
+            remapping[key] = convert_if_needed(value)
+          end
+        end
+      end
+      remapping
+    end
+
+    # trap signals for safe shutdown.
+    def trap_signals  #:nodoc:
+      ["INT", "TERM", "HUP"].each do |signal|
+        Signal.trap(signal) do
+          ROS::Node.shutdown_all_nodes
+        end
+      end
+    end
+
+    def self.add_shutdown_hook(proc)
+      @@shutdown_hook.push(proc)
+    end
+
+    # shutdown all nodes.
+    def self.shutdown_all_nodes
+      GraphManager.shutdown_all
+      @@shutdown_hook.each {|obj| obj.call}
+    end
+
+  end
+end