rosruby 0.0.1

data/lib/ros/tcpros/service_server.rb

@@ -0,0 +1,132 @@
+# ros/tcpros/service_server.rb
+#
+# License: BSD
+#
+# Copyright (C) 2012  Takashi Ogura <t.ogura@gmail.com>
+#
+require 'ros/tcpros/message'
+require 'gserver'
+
+module ROS::TCPROS
+
+  ##
+  # TCPROS protocol Service Server
+  #
+  class ServiceServer < ::GServer
+
+    ##
+    # max number of connection with clients
+    #
+    MAX_CONNECTION = 100
+
+    include ::ROS::TCPROS::Message
+
+    ##
+    # @param [String] caller_id caller_id of this node
+    # @param [String] service_name name of this service
+    # @param [Class] service_type class of this service message
+    # @param [Proc] callback of this service
+    # @param [Integer] port port number
+    # @param [host] host host name
+    def initialize(caller_id, service_name, service_type, callback,
+                   port=0, host=::GServer::DEFAULT_HOST)
+      super(port, host, MAX_CONNECTION)
+      @caller_id = caller_id
+      @service_name = service_name
+      @service_type = service_type
+      @callback = callback
+      @byte_received = 0
+      @byte_sent = 0
+    end
+
+    ##
+    # message must send 1 byte for service call result (success)
+    # @param [IO] socket
+    def send_ok_byte(socket)
+      socket.write([1].pack('c'))
+    end
+
+    ##
+    # message must send 1 byte for service call result (fail)
+    # @param [IO] socket
+    def send_ng_byte(socket)
+      socket.write([0].pack('c'))
+    end
+
+    ##
+    # main loop of this connection.
+    # read data and do callback.
+    # @param [IO] socket
+    # @return [Boolean] result of callback
+    def read_and_callback(socket)
+      request = @service_type.request_class.new
+      response = @service_type.response_class.new
+      data = read_all(socket)
+      @byte_received += data.length
+      request.deserialize(data)
+      result = @callback.call(request, response)
+      if result
+        send_ok_byte(socket)
+        data = write_msg(socket, response)
+        @byte_sent += data.length
+      else
+        send_ng_byte(socket)
+        write_header(socket, build_header)
+        # write some message
+      end
+      result
+    end
+
+    ##
+    # this is called by socket accept
+    # @param [IO] socket given socket
+    def serve(socket)
+      header = read_header(socket)
+      # not documented protocol?
+      if header['probe'] == '1'
+        write_header(socket, build_header)
+      elsif check_header(header)
+        write_header(socket, build_header)
+        read_and_callback(socket)
+        if header['persistent'] == '1'
+          loop do
+            read_and_callback(socket)
+          end
+        end
+      else
+        socket.close
+        raise 'header check error'
+      end
+    end
+
+    ##
+    # check header.
+    # check md5sum only.
+    # @param [Header] header header for checking
+    # @return [Boolean] check result (true means ok)
+    def check_header(header)
+      header.valid?('md5sum', @service_type.md5sum)
+    end
+
+    ##
+    # build header message for service server.
+    # It contains callerid, type, md5sum.
+    # @return [Header] built header
+    def build_header
+      header = Header.new
+      header["callerid"] = @caller_id
+      header['type'] = @service_type.type
+      header['md5sum'] = @service_type.md5sum
+      header
+    end
+
+    # received data amout for slave api
+    # @return [Integer] byte received
+    attr_reader :byte_received
+
+    # sent data amout for slave api
+    # @return [Integer] byte sent
+    attr_reader :byte_sent
+
+  end
+end

data/lib/ros/time.rb

@@ -0,0 +1,109 @@
+# ros/time.rb
+#
+# License: BSD
+#
+# Copyright (C) 2012  Takashi Ogura <t.ogura@gmail.com>
+#
+# Time object for ROS.
+#
+module ROS
+
+  ##
+  # super class of times
+  class TimeValue
+
+    include Comparable
+
+    # @return [Integer] seconds
+    attr_accessor :secs
+
+    # @return [Integer] nano seconds
+    attr_accessor :nsecs
+
+    # canonicalize secs and nsecs
+    # @return [TimeValue] self
+    def canonicalize
+      while @nsecs >= 1e9.to_i
+        @secs += 1
+        @nsecs -= 1e9.to_i
+      end
+      while @nsecs < 0
+        @secs -= 1
+        @nsecs += 1e9.to_i
+      end
+      self
+    end
+
+    # convert to seconds
+    # @return [Float] seconds
+    def to_sec
+      @secs + (@nsecs / 1e9)
+    end
+
+    # convert to nano seconds
+    # @return [Float] nano seconds
+    def to_nsec
+      @nsecs + (@secs * 1e9)
+    end
+
+    # compare time value
+    # @param [TimeValue] other compare target
+    # @return [Integer] result
+    def <=>(other)
+      diff = self.to_nsec - other.to_nsec
+      if diff > 0
+        1
+      elsif diff < 0
+        -1
+      else
+        0
+      end
+    end
+
+  end
+
+  ##
+  # ROS Time object. This is used as msg object for time
+  #
+  class Time < TimeValue
+
+    # initialize with current time
+    def self.now
+      self.new(::Time::now)
+    end
+
+    # @overload initialize(time)
+    #   @param [::Time] initialize with this time
+    # @overload initialize
+    def initialize(time=nil)
+      if time
+        @secs = time.to_i
+        @nsecs = ((time.to_f - @secs) * 1e9.to_i).to_i
+      else
+        @secs = 0
+        @nsecs = 0
+      end
+      canonicalize
+    end
+
+    # add time value
+    # @param [Duration] duration duration for adding
+    # @return [Time] new time
+    def +(duration)
+      tm = ::ROS::Time.new
+      tm.secs = @secs + duration.secs
+      tm.nsecs = @nsecs + duration.nsecs
+      tm.canonicalize
+    end
+
+    # subtract time value
+    # @param [Time] other
+    # @return [Duration] duration
+    def -(other)
+      d = ::ROS::Duration.new
+      d.secs = @secs - other.secs
+      d.nsecs = @nsecs - other.nsecs
+      d.canonicalize
+    end
+  end
+end

data/lib/ros/topic.rb

@@ -0,0 +1,47 @@
+# ros/topic.rb
+#
+# License: BSD
+#
+# Copyright (C) 2012  Takashi Ogura <t.ogura@gmail.com>
+#
+# Super class of Publisher/Subscriber
+#
+module ROS
+
+  ##
+  # Base class of {Publisher} and {Subscriber}
+  class Topic
+
+    # initialize member variables
+    # @param [String] caller_id caller id of this node
+    # @param [String] topic_name name of this topic
+    # @param [Class] topic_type class of msg
+    def initialize(caller_id, topic_name, topic_type)
+      @caller_id = caller_id
+      @topic_name = topic_name
+      @topic_type = topic_type
+      @connections = []
+      @connection_id_number = 0
+    end
+
+    # @return [String] caller id
+    attr_reader :caller_id
+
+    # @return [String] name of this topic
+    attr_reader :topic_name
+
+    # @return [Class] class of msg
+    attr_reader :topic_type
+
+    # shutdown all connections
+    def close #:nodoc:
+      @connections.each {|connection| connection.shutdown}
+    end
+
+    # set manager for shutdown
+    # @param [GraphManager] manager
+    def set_manager(manager) #:nodoc:
+      @manager = manager
+    end
+  end
+end

data/lib/ros/xmlrpcserver.rb

@@ -0,0 +1,40 @@
+# ros/xmlrpcserver.rb
+#
+# License: BSD
+#
+# Copyright (C) 2012 Takashi Ogura <t.ogura@gmail.com>
+#
+#
+
+#
+
+require 'xmlrpc/server'
+
+module ROS
+
+  # original XMLRPC Server (remove access log)
+  class XMLRPCServer < XMLRPC::WEBrickServlet
+    def initialize(port=8080,
+                   host="127.0.0.1",
+                   maxConnections=100,
+                   stdlog="#{ENV['HOME']}/.ros/log/rosruby.log")
+      super({})
+      require 'webrick'
+      @server = WEBrick::HTTPServer.new(:Port => port, 
+                                        :BindAddress => host,
+                                        :MaxClients => maxConnections,
+                                        :Logger => WEBrick::Log.new(stdlog),
+                                        :AccessLog => [])
+      @server.mount("/", self)
+    end
+    
+    def serve
+      @server.start
+    end
+  
+    def shutdown
+      @server.shutdown
+    end
+  end
+end
+

data/samples/add_two_ints_client.rb

@@ -0,0 +1,25 @@
+#!/usr/bin/env ruby
+
+require 'ros'
+
+require 'roscpp_tutorials/TwoInts'
+
+def main
+  if ARGV.length < 2
+    puts "usage: #{$0} X Y"
+    return
+  end
+  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 = ARGV[0].to_i
+    req.b = ARGV[1].to_i
+    if service.call(req, res)
+      p res.sum
+    end
+  end
+end
+
+main

data/samples/add_two_ints_server.rb

@@ -0,0 +1,20 @@
+#!/usr/bin/env ruby
+
+require 'ros'
+require 'roscpp_tutorials/TwoInts'
+
+def main
+  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
+end
+
+main

data/samples/gui.rb

@@ -0,0 +1,126 @@
+#! /usr/bin/env ruby
+
+#
+# = sample of gui (Ruby/Tk)
+# this sample needs geometry_msgs/Twist.
+# The GUI publishes velocity as Twist.
+#
+# if you don't have the message files, try
+#
+# $ rosrun rosruby rosruby_genmsg.py geometry_msgs
+#
+
+require 'tk'
+require 'ros'
+require 'geometry_msgs/Twist'
+
+topic_name = '/cmd_vel'
+node = ROS::Node.new('/gui_test')
+pub = node.advertise(topic_name, Geometry_msgs::Twist)
+
+linear_vel = 0.5
+angular_vel = 1.0
+
+TkButton.new {
+  text "FORWARD"
+  command do
+    msg = Geometry_msgs::Twist.new
+    msg.linear.x = linear_vel
+    pub.publish(msg)
+  end
+  width 10
+  grid("row"=>0, "column"=>1)
+}
+
+TkButton.new {
+  text "LEFT"
+  command do
+    msg = Geometry_msgs::Twist.new
+    msg.angular.z = angular_vel
+    pub.publish(msg)
+  end
+  width 10
+  grid("row"=>1, "column"=>0)
+}
+
+TkButton.new {
+  text "STOP"
+  command do
+    msg = Geometry_msgs::Twist.new
+    pub.publish(msg)
+  end
+  width 10
+  grid("row"=>1, "column"=>1)
+}
+
+TkButton.new {
+  text "RIGHT"
+  command do
+    msg = Geometry_msgs::Twist.new
+    msg.angular.z = -angular_vel
+    pub.publish(msg)
+  end
+  width 10
+  grid("row"=>1, "column"=>2)
+}
+
+
+TkButton.new {
+  text "BACKWARD"
+  command do
+    msg = Geometry_msgs::Twist.new
+    msg.linear.x = -linear_vel
+    pub.publish(msg)
+  end
+  width 10
+  grid("row"=>2, "column"=>1)
+}
+
+TkScale.new {
+  label 'linear vel(%)'
+  from 0
+  to 100
+  orient 'horizontal'
+  command do |val|
+    linear_vel = val.to_f * 0.005
+  end
+  grid("row"=>3, "column"=>0)
+}.set(50)
+
+TkScale.new {
+  label 'angular vel(%)'
+  from 0
+  to 100
+  orient 'horizontal'
+  command do |val|
+    angular_vel = val.to_f * 0.01
+  end
+  grid("row"=>3, "column"=>1)
+}.set(50)
+
+TkButton.new {
+  text "exit"
+  command do
+    node.shutdown
+    exit
+  end
+  width 5
+  grid("row"=>3, "column"=>2)
+}
+
+TkLabel.new {
+  text 'topic'
+  grid("row"=>4, "column"=>0)
+}
+
+TkEntry.new {
+  self.value = topic_name
+  bind 'Return', proc {
+    topic_name = self.value
+    pub.shutdown
+    pub = node.advertise(topic_name, Geometry_msgs::Twist)
+  }
+  grid("row"=>4, "column"=>1, "columnspan"=>2, "sticky"=>"news")
+}
+
+Tk.mainloop