rosruby 0.0.1

data/lib/ros/package.rb

@@ -0,0 +1,144 @@
+# ros/package.rb
+#
+# License: BSD
+#
+# Copyright (C) 2012  Takashi Ogura <t.ogura@gmail.com>
+#
+# == ROS Package manager
+#
+# This is used for adding RUBYLIB path.
+# This file will provide rospack functions, in the future
+#
+
+require 'rexml/document'
+
+module ROS
+
+  # This is used for adding RUBYLIB path.
+  #
+  class Package
+
+    ##
+    # at first check the rospack's cache, if found use it.
+    # if not found, check all package path.
+    # @param [String] cache_file cache file of rospack
+    # @return [Array] fullpath list of all packages
+    def self.read_cache_or_find_all(cache_file="#{ENV['HOME']}/.ros/rospack_cache")
+      if File.exists?(cache_file)
+        f = File.open(cache_file)
+        root_line = f.gets.chop
+        package_path_line = f.gets.chop
+        if root_line == "#ROS_ROOT=#{ENV['ROS_ROOT']}" and
+            package_path_line == "#ROS_PACKAGE_PATH=#{ENV['ROS_PACKAGE_PATH']}"
+          packages = {}
+          while line = f.gets
+            packages[File.basename(line.chop)] = line.chop
+          end
+          packages
+        else
+          self.find_all_packages
+        end
+      else
+        self.find_all_packages
+      end
+    end
+
+    ##
+    # search all packages that has manifest.xml
+    # @param [Hash] packages current found packages
+    # @param [Array] roots root directories for searching
+    # @return [Array] fullpath list of all packages
+    def self.find_all_packages(packages={}, roots=ENV['ROS_PACKAGE_PATH'].split(':').push(ENV['ROS_ROOT']))
+      roots.each do |root|
+        if File.exists?("#{root}/manifest.xml")
+          packages[File.basename(root)] = root
+        else
+          if File.exists?(root)
+            Dir.foreach(root) do |path|
+              if path != "." and path != ".."
+                full_path = "#{root}/#{path}"
+                if File.directory?(full_path)
+                  self.find_all_packages(packages, [full_path])
+                end
+              end
+            end
+          end
+        end
+      end
+      packages
+    end
+
+    ##
+    # all package path hash
+    #
+    @@all_packages = self.read_cache_or_find_all
+
+
+    ##
+    # get the depend packages of the arg
+    # @param [String] package find depends packages of this package
+    # @param [Array] packages current found depends
+    # @return [Array] packages
+    def self.depends(package, packages=[])
+      file = File.open("#{@@all_packages[package]}/manifest.xml")
+      doc = REXML::Document.new(file)
+      doc.elements.each('/package/depend') do |element|
+        depend_package = element.attributes['package']
+        if not packages.include?(depend_package)
+          packages.push(depend_package)
+          self.depends(depend_package, packages)
+        end
+      end
+      packages
+    end
+
+
+    ##
+    # get the current program's package
+    # @return [String] name of running programs's package
+    def self.find_this_package
+      path = File::dirname(File.expand_path($PROGRAM_NAME))
+      while path != '/'
+        if File.exists?("#{path}/manifest.xml")
+          return File::basename(path)
+        end
+        path = File::dirname(path)
+      end
+      nil
+    end
+
+    ##
+    # add package's [lib/, msg_gen/ruby, srv_gen/ruby] to '$:'.
+    # this enables load ruby files easily
+    # @param [String] package name of package
+    def self.add_path_of_package(package)
+      path = @@all_packages[package]
+      ["#{path}/msg_gen/ruby", "#{path}/srv_gen/ruby", "#{path}/lib"].each do |path|
+        if File.exists?(path)
+          if not $:.include?(path)
+            $:.push(path)
+          end
+        end
+      end
+    end
+
+    ##
+    # add [lib/, msg_gen/ruby, srv_gen/ruby] dirs of all depend packages
+    # to RUBYLIB, if the directory exists
+    # @param [String] package name of package
+    def self.add_path_with_depend_packages(package)
+      add_path_of_package(package)
+      Package::depends(package).each do |pack|
+        add_path_of_package(pack)
+      end
+    end
+
+  end
+
+  ##
+  # load manifest and add all dependencies
+  # @param [String] package name of package
+  def self.load_manifest(package)
+    ROS::Package.add_path_with_depend_packages(package)
+  end
+end

data/lib/ros/parameter_manager.rb

@@ -0,0 +1,127 @@
+# ros/parameter_manager.rb
+#
+# License: BSD
+#
+# Copyright (C) 2012  Takashi Ogura <t.ogura@gmail.com>
+#
+# == Control Parameter Server Interface
+#
+# ROS parameter sever inteface.
+# API document is here http://ros.org/wiki/ROS/Parameter%20Server%20API
+#
+require "xmlrpc/client"
+
+module ROS
+
+  # ROS parameter sever inteface.
+  # API document is here http://ros.org/wiki/ROS/Parameter%20Server%20API
+  class ParameterManager
+
+    # @param [String] master_uri URI of ROS Master (parameter server)
+    # @param [String] caller_id caller_id of this node
+    # @param [Array] remappings remapps to use for local remappings
+    def initialize(master_uri, caller_id, remappings)
+      @caller_id = caller_id
+      @master_uri = master_uri
+      @remappings = remappings
+      @server = XMLRPC::Client.new2(@master_uri)
+    end
+
+    ##
+    # get parameter named 'key'
+    # @param [String] key name of parameter
+    # @return [String] parameter value
+    def get_param(key)
+      if @remappings[key]
+        return @remappings[key]
+      end
+      code, message, value = @server.call("getParam", @caller_id, key)
+      case code
+      when 1
+        return value
+      else
+        return nil
+      end
+    end
+
+    ##
+    # set parameter for 'key'
+    # @param [String] key key of parameter
+    # @param [String, Integer, Float, Boolean] value value of parameter
+    # @return [Boolean] true if succeed
+    # @raise
+    def set_param(key, value)
+      code, message, value = @server.call("setParam", @caller_id, key, value)
+      case code
+      when 1
+        return true
+      when -1
+        raise message
+      else
+        return false
+      end
+    end
+
+    ##
+    # delete parameter 'key'
+    # @param [String] key key for remove
+    # @return [Boolean] return true if success, false if it is not exist
+    def delete_param(key)
+      code, message, value = @server.call("deleteParam", @caller_id, key)
+      case code
+      when 1
+        return true
+      else
+        return false
+      end
+    end
+
+    ##
+    # search the all namespace for key
+    # @param [String] key key for search
+    # @return [Array] values
+    def search_param(key)
+      code, message, value = @server.call("searchParam", @caller_id, key)
+      case code
+      when 1
+        return value
+      when -1
+        raise message
+      else
+        return false
+      end
+    end
+
+    ##
+    # check if the master has the key
+    # @param [String] key key for check
+    # @return [String, Integer, Float, Boolean] value of key
+    def has_param(key)
+      code, message, value = @server.call("hasParam", @caller_id, key)
+      case code
+      when 1
+        return value
+      when -1
+        raise message
+      else
+        return false
+      end
+    end
+
+    ##
+    # get the all keys of parameters
+    # @return [Array] all keys
+    #
+    def get_param_names
+      code, message, value = @server.call("getParamNames", @caller_id)
+      case code
+      when 1
+        return value
+      when -1
+        raise message
+      else
+        return false
+      end
+    end
+  end
+end

data/lib/ros/parameter_subscriber.rb

@@ -0,0 +1,47 @@
+# ros/parameter_subscriber.rb
+#
+# License: BSD
+#
+# Copyright (C) 2012 Takashi Ogura <t.ogura@gmail.com>
+#
+#
+# == Parameter Subscriber
+# callback object for paramUpdate
+#
+#
+
+module ROS
+
+  # callback object for paramUpdate
+  #
+  class ParameterSubscriber
+    # do not make instance directory. Plese use Node#subscribe_parameter.
+    # @param [String] key param key to subscribe
+    # @param [Proc] callback callback when parameter updated
+    def initialize(key, callback)
+      @key = key
+      @callback = callback
+    end
+
+    # callback with param value
+    # @param [String] value value of updated parameter
+    def call(value)
+      @callback.call(value)
+    end
+
+    # set GraphManager for management
+    # @param [GraphManager] manager GraphManager
+    def set_manager(manager) #:nodoc
+      @manager = manager
+    end
+
+    # shutdown this subscription
+    def shutdown
+      @manager.shutdown_parameter_subscriber(self)
+    end
+
+    # key of parameter for subscription
+    attr_accessor :key
+
+  end
+end

data/lib/ros/publisher.rb

@@ -0,0 +1,96 @@
+# ros/publisher.rb
+#
+# License: BSD
+#
+# Copyright (C) 2012  Takashi Ogura <t.ogura@gmail.com>
+#
+
+require 'ros/topic'
+require 'ros/tcpros/server'
+
+module ROS
+# Publisher interface of ROS.
+# A publisher contains multi connection with subscribers.
+# TCPROS protocol implementation is in ROS::TCPROS::Server.
+# Here is sample code. You can shutdown publisher by using this
+# instance (ROS::Publisher#shutdown)
+#   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)
+#     i += 1
+#   end
+  class Publisher < Topic
+
+    # @param [String] caller_id caller_id of this node
+    # @param [String] topic_name name of topic to publish (String)
+    # @param [Class] topic_type class of topic
+    # @param [Boolean] is_latched latched topic?
+    # @param [String] host host name of this node
+    def initialize(caller_id, topic_name, topic_type, is_latched, host)
+      super(caller_id, topic_name, topic_type)
+      @host = host
+      @is_latched = is_latched
+      @seq = 0
+    end
+
+    ##
+    # publish msg object
+    # @param [Class] message instance of topic_type class
+    # @return [Publisher] self
+    def publish(message)
+      @seq += 1
+      @last_published_msg = message
+      if message.has_header?
+        message.header.seq = @seq
+      end
+      @connections.each do |connection|
+        connection.msg_queue.push(message)
+      end
+      self
+    end
+
+    ##
+    # add tcpros connection as server
+    # @param [String] caller_id caller_id of subscriber
+    # @return [TCPROS::Server] connection object
+    def add_connection(caller_id) #:nodoc:
+      connection = TCPROS::Server.new(@caller_id, @topic_name, @topic_type,
+                                      :host=>@host,
+                                      :latched=>@is_latched,
+                                      :last_published_msg=>@last_published_msg)
+      connection.start
+      connection.id = "#{@topic_name}_out_#{@connection_id_number}"
+      @connection_id_number += 1
+      @connections.push(connection)
+      return connection
+    end
+
+    # @return [Array] connection data for slave api
+    def get_connection_data
+      @connections.map do |connection|
+        [connection.id, connection.byte_sent, connection.num_sent, 1]
+      end
+    end
+
+    # @return [array] connection info for slave api
+    def get_connection_info
+      info = []
+      @connections.each do |connection|
+        info.push([connection.id, connection.caller_id, 'o', connection.protocol, @topic_name])
+      end
+      info
+    end
+
+    ##
+    # user interface of shutdown this publisher
+    # @return [nil] nil
+    def shutdown
+      @manager.shutdown_publisher(self)
+    end
+  end
+end

data/lib/ros/rate.rb

@@ -0,0 +1,41 @@
+# ros/rate.rb
+#
+# License: BSD
+#
+# Copyright (C) 2012  Takashi Ogura <t.ogura@gmail.com>
+#
+#
+
+module ROS
+
+  ##
+  # sleep in a Hz timing.
+  # @example
+  #   r = ROS::Rate.new(10)
+  #   r.sleep
+  #
+  class Rate
+
+    # @param [Float] hz Hz
+    def initialize(hz)
+      @sleep_duration = 1.0 / hz
+      @last_time = ::Time.now
+    end
+
+    ##
+    # sleep for preset rate [Hz]
+    #
+    def sleep
+      current_time = ::Time.now
+      elapsed = current_time - @last_time
+      Kernel.sleep(@sleep_duration - elapsed)
+      @last_time = @last_time + @sleep_duration
+
+      # detect time jumping forwards, as well as loops that are
+      # inherently too slow
+      if current_time - @last_time > @sleep_duration * 2
+          @last_time = current_time
+      end
+    end
+  end
+end