ruby-player 0.0.1 → 0.1.0

data/lib/ruby-player/dev_addr.rb

@@ -0,0 +1,59 @@
+# Ruby Player - Ruby client library for Player (tools for robots) 
+#
+# Copyright (C) 2012  Timin Aleksey
+#
+# This program is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+# GNU General Public License for more details.
+
+module Player
+  class DevAddr
+    include Common
+
+    attr_reader :host, :robot, :interface, :index
+
+    def initialize(addr = {})
+      @host = addr[:host].to_i
+      @robot = addr[:robot].to_i
+      @interface = addr[:interface].to_i
+      @index = addr[:index].to_i
+    end
+
+    def DevAddr.decode(str)
+      ary = str.unpack("NNNN")
+      DevAddr.new(host: ary[0], robot: ary[1], interface: ary[2], index: ary[3])
+    end
+
+    def encode
+      [@host, @robot, @interface, @index].pack("NNNN")
+    end
+
+    def interface_name
+      @interface_name ||= search_interface_name(@interface)
+    end
+
+    def ==(other)
+      @host == other.host and @robot == other.robot and @interface == other.interface and @index == other.index
+    end
+
+    def to_s
+      "device addr [#@host:#@robot] #@interface_name:#@index"
+    end
+
+    def to_a
+      [@host, @robot, @interface, @index]
+    end
+
+    private
+    def search_interface_name(code)
+      name = search_const_name code, /PLAYER_[\w]*_CODE/ 
+      name.to_s.scan(/PLAYER_([\w]*)_CODE/).join.downcase
+    end
+  end
+end 

data/lib/ruby-player/device.rb

@@ -0,0 +1,60 @@
+# Ruby Player - Ruby client library for Player (tools for robots) 
+#
+# Copyright (C) 2012  Timin Aleksey
+#
+# This program is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+# GNU General Public License for more details.
+require "socket"
+
+module Player
+  class Device
+    include Common
+  
+    # Device address
+    attr_reader :addr
+
+    # Device geometry
+    # @return [Hash] geometry { :px, :py. :pz, :proll, :ppitch, :pyaw, :sw, :sl, :sh }
+    attr_reader :geom
+ 
+
+    def initialize(addr, client, log_level)
+      @addr, @client =  addr, client
+      @log_level = log_level
+
+      @geom = {px: 0.0, py: 0.0, pz: 0.0, proll: 0.0, ppitch: 0.0, pyaw: 0.0, sw: 0.0, sl: 0.0, sh: 0.0}
+    end
+
+    def fill(hdr,msg)
+      raise_error "Method `fill` isn't implemented for `#{self.class}`"
+    end
+
+    def handle_response(hdr, msg)
+      raise_error "Method `handle_response` isn't implemented for `#{self.class}`"
+    end
+   
+    private
+    def send_message(type, subtype, msg="")
+      @client.write( Header.new(
+          dev_addr: @addr,
+          type: type,
+          subtype: subtype,
+          size: msg.bytesize), msg)
+    end
+
+    def read_geom(msg)
+      data = msg.unpack("G*")
+      [:px,:py,:pz, :proll,:ppitch,:pyaw, :sw,:sl,:sh].each_with_index do |k,i|
+        @geom[k] = data[i]
+      end
+      debug("Get geom px=%.2f py=%.2f pz=%.2f; proll=%.2f, ppitch=%.2f, pyaw=%.2f, sw=%.2f, sl=%.2f, sh=%.2f" % @geom.values)
+    end
+  end
+end

data/lib/ruby-player/header.rb

@@ -0,0 +1,81 @@
+# Ruby Player - Ruby client library for Player (tools for robots) 
+#
+# Copyright (C) 2012  Timin Aleksey
+#
+# This program is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+# GNU General Public License for more details.
+
+module Player
+  class Header
+    include Common
+
+    attr_reader :dev_addr, :type, :subtype, :time, :seq, :size
+
+    def initialize(param = {})
+      @dev_addr = param[:dev_addr]
+      @type = param[:type].to_i
+      @subtype = param[:subtype].to_i
+      @time = param[:time] || Time.now.to_f / 1000
+      @seq = param[:seq].to_i
+      @size = param[:size].to_i
+
+    end
+
+    def Header.decode(str)
+      dev_addr = DevAddr.decode(str[0,16])
+      type, subtype, time, seq, size = str[16,24].unpack("NNGNN")
+      Header.new(dev_addr: dev_addr, type: type, subtype: subtype, time: time, seq: seq, size: size)
+    end
+
+    def Header.from_a(ary)
+      Header.decode ary.pack("NNNNNNGNN")
+    end
+
+    def encode
+      @dev_addr.encode + [@type, @subtype, @time, @seq, @size].pack("NNGNN")
+    end
+
+    def ==(other)
+      @dev_addr == other.dev_addr and @type == other.type and @subtype == other.subtype and @time == other.time and @seq == other.seq and @size == other.size
+    end
+
+    def to_s
+      "header to #@dev_addr of message [type=#{type_name}, subtype=#{subtype_name}, size=#@size]"
+    end
+
+    def type_name
+      @type_name ||= search_msg_type_name(@type)
+    end
+
+    def subtype_name
+      @subtype_name ||= search_msg_subtype_name(dev_addr.interface_name, type_name, @subtype)
+    end
+
+    private
+    def search_msg_type_name(type)
+      name = search_const_name type, /PLAYER_MSGTYPE_[\w]*/
+      if name != ""
+        name[7..-1]
+      else
+        type.to_s
+      end
+    end
+
+    def search_msg_subtype_name(interface_name, type_name, subtype)
+      type_prefix = type_name == "MSGTYPE_RESP_ACK" ? "" : type_name.split("_")[-1] 
+      name = search_const_name subtype, "PLAYER_" + interface_name.upcase + "_" + type_prefix + ".*"
+      if name != ""
+        name[7..-1]
+      else
+        subtype.to_s
+      end
+    end
+  end
+end 

data/lib/ruby-player/position2d.rb

@@ -17,146 +17,192 @@ module Player
   #
   # @example
   #   # get proxy object
-  #   pos2d = client[:position2d, 0]
+  #   pos2d = client.subcribe("position2d", index: 0)
   #   # setup speed of robot
   #   pos2d.set_speed(vx: 1.2, vy: 0.1, va: 0.3)
   #
   #   #update data from server
-  #   client.read
+  #   client.read!
   #   #read velocityand position by X,Y and angle
-  #   pos2d.speed #=> { :vx => 1.2, :vy => 0.1, :va => 0.3 }
-  #   pos2d.odometry #=> { :px => 0.2321, :py => 0,01, :pa => 0.2 }
-  #   pos2d.stop 
-  class Position2d
-    include CType
-    include Common
+  #   pos2d.position #=> { :px => 0.2321, :py => 0,01, :pa => 0.2, :vx => 1.2, :vy => 0.1, :va => 0.3, :stall => 1  }
+  #   pos2d.stop!
+  class Position2d < Device
 
-    module C
-      extend FFI::Library
-      ffi_lib "playerc"
+    # Position of robot
+    # @return [Hash] hash position {:px, :py, :pa, :vx, :vy, :va, :stall }
+    attr_reader :position
 
-      attach_function :playerc_position2d_create, [:pointer, :int],  :pointer
-      attach_function :playerc_position2d_destroy, [:pointer],  :void
-      attach_function :playerc_position2d_subscribe, [:pointer, :int],  :int
-      attach_function :playerc_position2d_unsubscribe, [:pointer],  :int
+    def initialize(addr, client, log_level)
+      super
+      @position = {px: 0.0, py: 0.0, pa: 0.0, vx: 0.0, vy: 0.0, va: 0.0, stall: 0}
+    end
 
-      attach_function :playerc_position2d_get_geom, [:pointer], :int
-      attach_function :playerc_position2d_set_cmd_vel, [:pointer, :double, :double, :double, :int], :int
-      attach_function :playerc_position2d_set_cmd_car, [:pointer, :double, :double], :int
-      attach_function :playerc_position2d_enable, [:pointer, :int], :int
-      attach_function :playerc_position2d_set_odom, [:pointer, :double, :double, :double], :int
+    # Query robot geometry 
+    # @return self
+    def query_geom
+      send_message(PLAYER_MSGTYPE_REQ, PLAYER_POSITION2D_REQ_GET_GEOM)
+      self
+    end
+   
+    # Turn on motor
+    # @return self
+    def turn_on!
+      send_message(PLAYER_MSGTYPE_REQ, PLAYER_POSITION2D_REQ_MOTOR_POWER, [1].pack("N")) 
+      self
+    end
+    
+    # Turn off motor
+    def turn_off!
+      send_message(PLAYER_MSGTYPE_REQ, PLAYER_POSITION2D_REQ_MOTOR_POWER, [0].pack("N")) 
+      self
     end
 
-    def initialize(client, index)
-      @pos2d =  Position2dStruct.new(C.playerc_position2d_create(client, index))
-      try_with_error C.playerc_position2d_subscribe(@pos2d, PLAYER_OPEN_MODE)
+    def direct_speed_control!
+      send_message(PLAYER_MSGTYPE_REQ, PLAYER_POSITION2D_REQ_VELOCITY_MODE, [0].pack("N"))
+      self
+    end
 
-      ObjectSpace.define_finalizer(self, Position2d.finilazer(@pos2d))
+    def separate_speed_control!
+      send_message(PLAYER_MSGTYPE_REQ, PLAYER_POSITION2D_REQ_VELOCITY_MODE, [1].pack("N"))
+      self
     end
 
-    # Odometry of robot
-    # @return [Hash] hash odometry {:px, :py, :pa }
-    def odometry
-      { 
-        px: @pos2d[:px],
-        py: @pos2d[:py],
-        pa: @pos2d[:pa]
-      }
+    def position_control!
+      send_message(PLAYER_MSGTYPE_REQ, PLAYER_POSITION2D_REQ_POSITION_MODE, [0].pack("N"))
+      self
     end
-    
+
+    def speed_control!
+      send_message(PLAYER_MSGTYPE_REQ, PLAYER_POSITION2D_REQ_POSITION_MODE, [1].pack("N"))
+      self
+    end
+
     # Set odometry of robot.
-    # @params [Hash] odometry
+    # @param [Hash] odometry 
     # @option odometry :px x position (m)
     # @option odometry :py y position (m)
     # @option odometry :pa angle (rad).     
     # @return self
-    def set_odometry(odometry)
-      args = [
-        odometry[:px].to_f || @pos2d[:px],
-        odometry[:py].to_f || @pos2d[:py],
-        odometry[:pa].to_f || @pos2d[:pa]
+    def set_odometry(odom)
+      data = [
+        odom[:px].to_f,
+        odom[:py].to_f,
+        odom[:pa].to_f
       ]
+      
+      send_message(PLAYER_MSGTYPE_REQ, PLAYER_POSITION2D_REQ_SET_ODOM, data.pack("GGG"))
+      self
+    end
 
-      try_with_error C.playerc_position2d_set_odom(@pos2d, *args)
+    # Set speed PID paramters
+    # @param [Hash] params PID params
+    # @option params :kp P
+    # @option params :ki I
+    # @option params :kd D
+    # @return self
+    def set_speed_pid(params={})
+      data = [
+        params[:kp].to_f,
+        params[:ki].to_f,
+        params[:kd].to_f
+      ]
+      send_message(PLAYER_MSGTYPE_REQ, PLAYER_POSITION2D_REQ_SPEED_PID, data.pack("GGG"))
       self
     end
 
-    # Reset odometry to zero
+    # Set position PID paramters
+    # @param [Hash] params PID params
+    # @option params :kp P
+    # @option params :ki I
+    # @option params :kd D
     # @return self
-    def reset_odometry
-      set_odometry(px: 0, py: 0, pa: 0)
+    def set_position_pid(params={})
+      data = [
+        params[:kp].to_f,
+        params[:ki].to_f,
+        params[:kd].to_f
+      ]
+      send_message(PLAYER_MSGTYPE_REQ, PLAYER_POSITION2D_REQ_POSITION_PID, data.pack("GGG"))
+      self
     end
 
-    # Robot geometry
-    # @return [Hash] position :px, :py, :pa, size :sx, :sy
-    def geom
-      try_with_error C.playerc_position2d_get_geom(@pos2d)
-      p = @pos2d[:pose].to_a
-      s = @pos2d[:size].to_a
-      {
-        px: p[0],
-        py: p[1],
-        pa: p[2],
-        sx: s[0],
-        sy: s[1]
-      }
+    # Set speed profile
+    # @param [Hash] params profile prarams
+    # @option params :spped max speed (m/s)
+    # @option params :acc max acceleration (m/s^2)
+    # @return self
+    def set_speed_profile(params={})
+      data = [
+        params[:speed].to_f,
+        params[:acc].to_f
+      ]
+
+      send_message(PLAYER_MSGTYPE_REQ, PLAYER_POSITION2D_REQ_SPEED_PROF, data.pack("GG"))
+      self
     end
 
+    # Reset odometry to zero
+    # @return self
+    def reset_odometry
+      send_message(PLAYER_MSGTYPE_REQ, PLAYER_POSITION2D_REQ_RESET_ODOM)
+      self
+    end
 
     # Set speed of robot. All speeds are defined in the robot coordinate system.
-    # @params [Hash] speeds 
+    # @param [Hash] speeds 
     # @option speeds :vx forward speed (m/s)
     # @option speeds :vy sideways speed (m/s); this field is used by omni-drive robots only. 
     # @option speeds :va rotational speed (rad/s).     
+    # @option speeds :stall state of motor
     # @return self
     def set_speed(speeds)
-      args = [
-        speeds[:vx].to_f || @pos2d[:vx],
-        vy = speeds[:vy].to_f || @pos2d[:vy],
-        va = speeds[:va].to_f || @pos2d[:va],
-        1
+      data = [
+        speeds[:vx] || @position[:vx],
+        speeds[:vy] || @position[:vy],
+        speeds[:va] || @position[:va],
+        speeds[:stall] || @position[:stall]
       ]
-      try_with_error C.playerc_position2d_set_cmd_vel(@pos2d, *args)
+      send_message(PLAYER_MSGTYPE_CMD, PLAYER_POSITION2D_CMD_VEL, data.pack("GGGN"))
       self
     end
 
     # Set speed of carlike robot
-    # @params [Hash] speeds 
+    # @param [Hash] speeds 
     # @option speeds :vx forward speed (m/s)
-    # @option speeds :a angle robot (rad).     
+    # @option speeds :a turning angle (rad).     
     def set_car(speeds)
-      args = [ 
-        speeds[:vx].to_f || @pos2d[:vx],
-        speeds[:a].to_f || @pos2d[:pa]
+      data = [ 
+        speeds[:vx] || @position[:vx],
+        speeds[:a] || @position[:pa]
       ]
-      try_with_error C.playerc_position2d_set_cmd_car(@pos2d, *args)
+      send_message(PLAYER_MSGTYPE_CMD, PLAYER_POSITION2D_CMD_CAR, data.pack("GG"))
       self
     end
 
-    # Velocity of robot
-    # @return [Hash] hash of speeds
-    def speed
-      {
-        vx: @pos2d[:vx],
-        vy: @pos2d[:vy],
-        va: @pos2d[:va]
-      }
+    # The position interface accepts translational velocity + absolute heading commands
+    # (speed and angular position) for the robot's motors (only supported by some drivers)
+    # @param [Hash] speeds 
+    # @option speeds :vx forward speed (m/s)
+    # @option speeds :a absolutle angle (rad).     
+    def set_speed_head(speeds)
+      data = [ 
+        speeds[:vx] || @position[:vx],
+        speeds[:a] || @position[:pa]
+      ]
+      send_message(PLAYER_MSGTYPE_CMD, PLAYER_POSITION2D_CMD_VEL_HEAD, data.pack("GG"))
+      self
     end
 
+
     # State of motor
     # @return [Boolean] true - on
-    def enable
-      @pos2d[:stall] == 1 
-    end
-
-    # Turn on\off motor
-    # @param [Boolean] true - turn on 
-    def enable=(val)
-      try_with_error C.playerc_position2d_enable(@pos2d, val ? 1 : 0) 
+    def power
+      @position[:stall] == 1 
     end
 
     # Stop robot set speed to 0
-    def stop
+    # @return self
+    def stop!
       set_speed(vx: 0, vy: 0, va: 0)
     end
 
@@ -165,11 +211,35 @@ module Player
       speed[:vx] == 0 && speed[:vy] == 0 && speed[:va] == 0
     end
 
-    def Position2d.finilazer(pos)
-      lambda{
-        try_with_error C.playerc_position2d_unsubscribe(pos)
-        C.playerc_position2d_destroy(pos)
-      }
+    def fill(hdr, msg)
+      case hdr.subtype
+      when PLAYER_POSITION2D_DATA_STATE
+        read_position(msg)
+      when PLAYER_POSITION2D_DATA_GEOM
+        read_geom(msg)
+      else
+        unexpected_message hdr
+      end
+    end
+
+    def handle_response(hdr, msg)
+      case hdr.subtype
+      when PLAYER_POSITION2D_REQ_GET_GEOM
+        read_geom(msg)
+      when 2..9
+        nil #null response
+      else
+        unexpected_message hdr
+      end
+    end
+
+    private
+    def read_position(msg)
+      data = msg.unpack("GGGGGGN")
+      @position.keys.each_with_index do |k,i|
+        @position[k] = data[i]
+      end
+      debug("Get position px=%.2f py=%.2f pa=%.2f; vx=%.2f, vy=%.2f, va=%.2f, stall=%d" % position.values)
     end
   end
 end