ruby-player 0.0.1

data/lib/ruby-player/position2d.rb

@@ -0,0 +1,175 @@
+# 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
+  # The position2d proxy provides an interface to a mobile robot base
+  #
+  # @example
+  #   # get proxy object
+  #   pos2d = client[:position2d, 0]
+  #   # setup speed of robot
+  #   pos2d.set_speed(vx: 1.2, vy: 0.1, va: 0.3)
+  #
+  #   #update data from server
+  #   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
+
+    module C
+      extend FFI::Library
+      ffi_lib "playerc"
+
+      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
+
+      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
+    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)
+
+      ObjectSpace.define_finalizer(self, Position2d.finilazer(@pos2d))
+    end
+
+    # Odometry of robot
+    # @return [Hash] hash odometry {:px, :py, :pa }
+    def odometry
+      { 
+        px: @pos2d[:px],
+        py: @pos2d[:py],
+        pa: @pos2d[:pa]
+      }
+    end
+    
+    # Set odometry of robot.
+    # @params [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]
+      ]
+
+      try_with_error C.playerc_position2d_set_odom(@pos2d, *args)
+      self
+    end
+
+    # Reset odometry to zero
+    # @return self
+    def reset_odometry
+      set_odometry(px: 0, py: 0, pa: 0)
+    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]
+      }
+    end
+
+
+    # Set speed of robot. All speeds are defined in the robot coordinate system.
+    # @params [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).     
+    # @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
+      ]
+      try_with_error C.playerc_position2d_set_cmd_vel(@pos2d, *args)
+      self
+    end
+
+    # Set speed of carlike robot
+    # @params [Hash] speeds 
+    # @option speeds :vx forward speed (m/s)
+    # @option speeds :a angle robot (rad).     
+    def set_car(speeds)
+      args = [ 
+        speeds[:vx].to_f || @pos2d[:vx],
+        speeds[:a].to_f || @pos2d[:pa]
+      ]
+      try_with_error C.playerc_position2d_set_cmd_car(@pos2d, *args)
+      self
+    end
+
+    # Velocity of robot
+    # @return [Hash] hash of speeds
+    def speed
+      {
+        vx: @pos2d[:vx],
+        vy: @pos2d[:vy],
+        va: @pos2d[:va]
+      }
+    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) 
+    end
+
+    # Stop robot set speed to 0
+    def stop
+      set_speed(vx: 0, vy: 0, va: 0)
+    end
+
+    # Check of robot state
+    def stoped?
+      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)
+      }
+    end
+  end
+end

data/lib/ruby-player/ranger.rb

@@ -0,0 +1,136 @@
+# 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
+  # The ranger proxy provides an interface to the ranger sensors built into robots
+  class Ranger
+    include CType
+    include Common
+
+    module C
+      extend FFI::Library
+      ffi_lib "playerc"
+
+      attach_function :playerc_ranger_create, [:pointer, :int],  :pointer
+      attach_function :playerc_ranger_destroy, [:pointer],  :void
+      attach_function :playerc_ranger_subscribe, [:pointer, :int],  :int
+      attach_function :playerc_ranger_unsubscribe, [:pointer],  :int
+
+      attach_function :playerc_ranger_power_config, [:pointer, :uint8],  :int
+      attach_function :playerc_ranger_intns_config, [:pointer, :uint8],  :int
+      attach_function :playerc_ranger_set_config, [:pointer] + [:double] * 7,  :int
+      attach_function :playerc_ranger_get_config, [:pointer] + [:pointer] * 7,  :int
+    end
+
+    def initialize(client, index)
+      @ranger =  RangerStruct.new(C.playerc_ranger_create(client, index))
+      try_with_error C.playerc_ranger_subscribe(@ranger, PLAYER_OPEN_MODE)
+
+      ObjectSpace.define_finalizer(self, Ranger.finilazer(@ranger))
+    end
+
+    # Count of sensors 
+    # Return [Integer] count
+    def element_count
+      @ranger[:element_count]
+    end
+
+    # Power control
+    # @param enable nil or false power off
+    def power_enable=(enable)
+      try_with_error C.playerc_ranger_power_config(@ranger, enable ? 1 : 0)
+    end
+
+    # Enable intensity
+    # @param enable nil or false disable 
+    def intensity_enable=(enable)
+      try_with_error C.playerc_ranger_intns_config(@ranger, enable ? 1 : 0)
+    end
+
+    # Set config of ranger
+    # @param [Hash] config params for setup
+    # @option :min_angle start angle of scans [rad] 
+    # @option :max_angle end angle of scans
+    # @option :angular_res scan resolution [rad]
+    # @option :min_range maximum range [m]
+    # @option :max_range minimum range [m]
+    # @option :range_res range resolution [m]
+    # @option :frequency scanning frequency [Hz]
+    def set_config(config)
+      args = [
+        config[:min_angle].to_f || @ranger[:min_angle],
+        config[:max_angle].to_f || @ranger[:max_angle],            
+        config[:angular_res].to_f || @ranger[:angular_res],            
+        config[:min_range].to_f || @ranger[:min_range],
+        config[:max_range].to_f || @ranger[:max_range],
+        config[:range_res].to_f || @ranger[:range_res],
+        config[:frequecy].to_f || @ranger[:frequecy] 
+      ]
+
+      try_with_error C.playerc_ranger_set_config(@ranger, *args)
+    end
+
+    # Configuration of ranger
+    # @see set_config
+    def config
+      try_with_error C.playerc_ranger_get_config(@ranger, *([nil] * 7))
+      {  
+        min_range:    @ranger[:min_angle],
+        max_range:    @ranger[:max_angle],            
+        angular_res:  @ranger[:angular_res],            
+        min_range:    @ranger[:min_range],
+        max_range:    @ranger[:max_range],
+        range_res:    @ranger[:range_res],
+        frequecy:     @ranger[:frequecy] 
+      }
+    end
+
+    # Range data [m]
+    # @return [Array] fot each sensor
+    def ranges
+      @ranger[:ranges].read_array_of_type(
+        FFI::Type::DOUBLE,
+        :read_double,
+        @ranger[:ranges_count]
+      )
+    end
+
+    # Intensity data [m]. 
+    # @return [Array] fot each sensor
+    def intensities
+      @ranger[:intensities].read_array_of_type(
+        FFI::Type::DOUBLE, 
+        :read_double, 
+        @ranger[:intensities_count]
+      )
+    end
+
+    # Scan bearings in the XY plane [radians]. 
+    # @return [Array] fot each sensor
+    def bearings
+      @ranger[:bearings].read_array_of_type(
+        FFI::Type::DOUBLE, 
+        :read_double, 
+        @ranger[:bearings_count]
+      )
+    end
+
+    def Ranger.finilazer(ranger)
+      lambda{
+        try_with_error C.playerc_ranger_unsubscribe(pos)
+        C.playerc_ranger_destroy(pos)
+      }
+    end
+  end
+end 

data/lib/ruby-player/version.rb

@@ -0,0 +1,17 @@
+# 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
+  VERSION = "0.0.1"
+end

data/lib/ruby-player.rb

@@ -0,0 +1,23 @@
+# 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 "ffi"
+
+require "ruby-player/c_type"
+
+require "ruby-player/version"
+require "ruby-player/common"
+require "ruby-player/position2d"
+require "ruby-player/ranger"
+require "ruby-player/client"

data/ruby-player.gemspec

@@ -0,0 +1,27 @@
+# -*- encoding: utf-8 -*-
+$:.push File.expand_path("../lib", __FILE__)
+require "ruby-player/version"
+
+Gem::Specification.new do |s|
+  s.name        = "ruby-player"
+  s.version     = Player::VERSION
+  s.authors     = ["Aleksey Timin"]
+  s.email       = ["atimin@gmail.com"]
+  s.homepage    = "http://www.github.com/flipback/ruby-player"
+  s.summary     = %q{Ruby Player - Ruby client library for Player (tools for robots)}
+  s.description = %q{Ruby Player - Ruby client library for Player (tools for robots)}
+
+  s.files         = `git ls-files`.split("\n")
+  s.test_files    = `git ls-files -- {test,spec,features}/*`.split("\n")
+  s.executables   = `git ls-files -- bin/*`.split("\n").map{ |f| File.basename(f) }
+  s.require_paths = ["lib"]
+
+  s.required_ruby_version = '>= 1.9.2'
+
+  s.add_runtime_dependency "ffi", '~>1.0.0'
+  s.add_development_dependency "rspec", '~> 2.7'
+  s.add_development_dependency "rake", '~> 0.9'
+  s.add_development_dependency "pry"
+  s.add_development_dependency "yard"
+  s.add_development_dependency "redcarpet"
+end

data/spec/client_spec.rb

@@ -0,0 +1,32 @@
+require "ruby-player"
+
+describe Player::Client do
+  before do 
+    @cl = Player::Client.new("localhost")
+  end
+
+  it "should raise error if connection doesn't success" do
+    lambda{ Player::Client.new("localhost", 6666) }.should raise_error(StandardError, 
+      "connect call on [localhost:6666] failed with error [111:Connection refused]")
+  end
+
+  it "should have close method" do
+    @cl.closed?.should be_false
+    @cl.close
+    @cl.closed?.should be_true
+  end
+
+  it "should have block for connection" do
+    @cl.close
+    Player::Client.connect("localhost") do |cl|
+      @cl = cl
+      @cl.closed?.should be_false
+    end
+
+    @cl.closed?.should be_true
+  end
+
+  after do
+    @cl.close unless @cl.closed?
+  end
+end

data/spec/position2d_spec.rb

@@ -0,0 +1,77 @@
+require "ruby-player"
+
+describe Player::Position2d do
+  before do
+    @cl = Player::Client.connect("localhost") 
+    @pos2d = @cl[:position2d, 0]
+    @pos2d.stop
+    @cl.read
+  end
+
+  it 'should set odometry' do
+    pending "Don't work with Player/Stage"
+    new_od = { px: 1.0, py: 2.0, pa: 3 }
+    @pos2d.set_odometry(new_od)
+    @pos2d.reset_odometry
+    
+    sleep(1.0)
+    @cl.read
+
+    @pos2d.odometry.should eql(new_od)
+  end
+
+  it 'should move' do
+    pos = @pos2d.odometry
+    speed = {vx: -0.4, vy: 0.2, va: -0.1 }
+    @pos2d.set_speed(speed)
+
+    sleep(1.1)
+    @cl.read
+    @pos2d.speed.should eq(speed)
+
+    #change position
+    @pos2d.odometry[:px].should be_within(0.1).of(pos[:px] + speed[:vx]*Math.cos(pos[:pa]) - speed[:vy]*Math.sin(pos[:pa]))
+    @pos2d.odometry[:py].should be_within(0.1).of(pos[:py] + speed[:vx]*Math.sin(pos[:pa]) + speed[:vy]*Math.cos(pos[:pa]))
+    Math.sin(@pos2d.odometry[:pa]).should be_within(0.1).of(Math.sin(pos[:pa] + speed[:va]))
+  end
+
+
+  it 'should move like car' do
+    pos = @pos2d.odometry
+    speed = { vx: 0.4, a: 0.3 }
+    @pos2d.set_car(speed)
+
+    sleep(1.1)
+    @cl.read
+    @pos2d.speed.should eq(vx: 0.4, vy: 0.0, va: 0.3)
+
+    #change position
+    @pos2d.odometry[:px].should be_within(0.1).of(pos[:px] + speed[:vx]*Math.cos(pos[:pa]))
+    @pos2d.odometry[:py].should be_within(0.1).of(pos[:py] + speed[:vx]*Math.sin(pos[:pa]))
+    Math.sin(@pos2d.odometry[:pa]).should be_within(0.1).of(Math.sin(pos[:pa] + speed[:a]))
+  end
+
+  it 'should have stop' do
+    # Move robot
+    @pos2d.set_speed(vx: 1.0, vy: 0.5, va: -0.2)
+    @cl.read
+    @pos2d.speed.should eql(vx: 1.0, vy: 0.5, va: -0.2)
+    @pos2d.stoped?.should be_false
+
+    # Stop robot
+    @pos2d.stop
+    @cl.read
+    @pos2d.speed.should eql(vx: 0.0, vy: 0.0, va: 0.0)
+    @pos2d.stoped?.should be_true
+  end
+
+  it 'should turn on motor' do
+    pending "Don't work with Player/Stage"
+    @pos2d.enable.should be_false
+    @pos2d.enable = true
+  end
+
+  after do 
+    @cl.close unless @cl.closed?
+  end
+end

data/spec/ranger_spec.rb

@@ -0,0 +1,67 @@
+require "ruby-player"
+
+describe Player::Ranger do
+  before do 
+    @cl = Player::Client.connect("localhost") 
+    @ranger = @cl[:ranger,0]
+    @cl.read
+  end
+
+  it 'should have ranges' do
+    @ranger.ranges.should eq([3.0, 3.0]) 
+  end
+
+  it 'should have intensities' do
+    @ranger.intensities.should eq([0.0, 0.0]) 
+  end
+
+  it 'should have bearings' do
+    @ranger.bearings.size.should eql(2)
+  end
+
+  it 'should have element count' do
+    @ranger.element_count.should eq(0)
+  end
+
+  it 'should have power control' do
+    pending "Don't work with Player/Stage"
+    #@ranger.power_enable = false
+    #@ranger.power_enable = true
+  end
+
+  it 'should have intensity control' do
+    pending "Don't work with Player/Stage"
+    #@ranger.intensity_enable = false
+    #@ranger.intensity_enable = true
+  end
+
+  it 'should porovide method to configurate' do
+    pending "Don't work with Player/Stage"
+    config = {
+      min_angle: 0.3,
+      max_angle: 1.0,
+      angular_res: 0.1,
+      min_range: 0.3,
+      max_range: 1,
+      range_res: 0.1,
+      frequecy: 2
+    }
+
+
+    #@ranger.set_config(config)
+  end
+
+  it 'should have configuration' do
+    @ranger.config.should eq(
+      :min_range=>0.1, 
+      :max_range=>3.0, 
+      :angular_res=>0.5235987755982988, 
+      :range_res=>0.02, 
+      :frequecy=>10.0
+    )
+  end
+
+  after do 
+    @cl.close unless @cl.closed?
+  end
+end

data/spec/world/test.cfg

@@ -0,0 +1,18 @@
+# Desc: Player test configuration file
+
+driver
+(
+  name "stage"
+  provides [ "simulation:0" ]
+  plugin "stageplugin"
+
+  worldfile "test.world"
+)
+
+driver
+(
+  name "stage"
+  provides [ "position2d:0" "ranger:0" ]
+
+  model "bender"
+)

data/spec/world/test.world

@@ -0,0 +1,89 @@
+quit_time 3600
+
+paused 0
+
+resolution 0.02
+
+window
+(
+  size [ 635.000 666.000 ] # in pixels
+  scale 36.995   # pixels per meter
+  center [ -0.040  -0.274 ]
+  rotate [ 0  0 ]
+
+  show_data 1              # 1=on 0=off
+)
+
+define carton model
+(
+  color "yelow"
+  #carton is retangular
+  # so make a square shape and use size[]
+  block
+  (
+   points 4
+   point[0] [1 0]
+   point[1] [1 4]
+   point[2] [0 4]
+   point[3] [0 0]
+   z [0 1]
+  )
+  # average litre carton size is ~ 20cm x 10cm x 5cm ish
+  size [0.4 4 2]
+)
+
+
+define bender_ranger ranger
+(
+  color "red"
+
+  sensor
+  (
+    pose [0 0.1 0.3 15]
+    size [0.4 0.4 0.4]
+    fov 30
+    range [0.1 3]
+  )
+
+  sensor
+  (
+    pose [0 -0.1 0.3 -15]
+    size [0.4 0.4 0.4]
+    fov 30
+    range [0.1 3]
+  )
+)
+
+define bender2dx position
+(
+  color "grey"
+
+  block
+  (
+    points 5
+    point[0] [-3 -3]
+    point[1] [-3 3]
+    point[2] [3 3]
+    point[3] [9 0]
+    point[4] [3 -3]
+    z [0 1]
+  )
+
+  drive "omni"
+
+  bender_ranger()
+  size [1 1 1]
+)
+
+#carton
+#(
+#  name "c0"
+#  pose [ 1 0 0 0.0]
+#)
+
+bender2dx
+(
+  name "bender"
+  pose [ 0 0 0 0.000 ]
+
+)