ruby-player 0.0.1 → 0.1.0

data/lib/ruby-player/ranger.rb

@@ -14,61 +14,80 @@
 
 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
+  # TODO Implement PLAYER_RANGER_DATA_RANGESTAMPED and PLAYER_RANGER_DATA_INTNSTAMPED
+  #
+  # @example
+  #   ranger = robot.subscribe(:ranger, index: 0)
+  #   ranger.rangers #=> [0.2, 0.1, 0.2]
+  class Ranger < Device
 
-    def initialize(client, index)
-      @ranger =  RangerStruct.new(C.playerc_ranger_create(client, index))
-      try_with_error C.playerc_ranger_subscribe(@ranger, PLAYER_OPEN_MODE)
+    # Range data [m]
+    # @return [Array] fot each sensor
+    attr_reader :rangers
 
-      ObjectSpace.define_finalizer(self, Ranger.finilazer(@ranger))
+    # Intensity data [m]. 
+    # @return [Array] fot each sensor
+    attr_reader :intensities
+ 
+    # Configuration of ranger
+    # @see set_config
+    attr_reader :config
+
+    def initialize(addr, client, log_level)
+      super
+      @rangers = []
+      @intensities = []
+      @geom[:sensors] = []
+      @config = { min_angle: 0.0, max_angle: 0.0, angular_res: 0.0, min_range: 0.0, max_range: 0.0, range_res: 0.0, frequecy: 0.0 }
+    end
+    
+    # Query ranger geometry 
+    # @return self
+    def query_geom
+      send_message(PLAYER_MSGTYPE_REQ, PLAYER_RANGER_REQ_GET_GEOM)
+      self
+    end
+
+    # Turn on ranger
+    # @return self
+    def turn_on!
+      send_message(PLAYER_MSGTYPE_REQ, PLAYER_RANGER_REQ_POWER, [1].pack("N")) 
+      self
+    end
+    
+    # Turn off ranger
+    def turn_off!
+      send_message(PLAYER_MSGTYPE_REQ, PLAYER_RANGER_REQ_POWER, [0].pack("N")) 
+      self
     end
 
-    # Count of sensors 
-    # Return [Integer] count
-    def element_count
-      @ranger[:element_count]
+    def intensity_enable!
+      send_message(PLAYER_MSGTYPE_REQ, PLAYER_RANGER_REQ_INTNS, [1].pack("N"))
+      self
     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)
+    def intensity_disable!
+      send_message(PLAYER_MSGTYPE_REQ, PLAYER_RANGER_REQ_INTNS, [0].pack("N"))
+      self
     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)
+    # Query ranger configuration
+    def query_config
+      send_message(PLAYER_MSGTYPE_REQ, PLAYER_RANGER_REQ_GET_CONFIG)
+      self
     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]
+    # @option config :min_angle start angle of scans [rad] 
+    # @option config :max_angle end angle of scans
+    # @option config :angular_res scan resolution [rad]
+    # @option config :min_range maximum range [m]
+    # @option config :max_range minimum range [m]
+    # @option config :range_res range resolution [m]
+    # @option config :frequency scanning frequency [Hz]
     def set_config(config)
-      args = [
+      data = [
         config[:min_angle].to_f || @ranger[:min_angle],
         config[:max_angle].to_f || @ranger[:max_angle],            
         config[:angular_res].to_f || @ranger[:angular_res],            
@@ -78,59 +97,70 @@ module Player
         config[:frequecy].to_f || @ranger[:frequecy] 
       ]
 
-      try_with_error C.playerc_ranger_set_config(@ranger, *args)
+      send_message(PLAYER_MSGTYPE_REQ, PLAYER_RANGER_REQ_SET_CONFIG, data.pack("G*"))
     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]
-      )
+    def fill(hdr, msg)
+      case hdr.subtype
+      when PLAYER_RANGER_DATA_RANGE
+        data = msg.unpack("NNG*")
+        @rangers = data[2..-1]
+        debug "Get rangers #{@rangers.inspect}"
+      when PLAYER_RANGER_DATA_INTNS
+        data = msg.unpack("NNG*")
+        @intensities = data[2..-1]
+        debug "Get intensities #{@rangers.inspect}"
+      when PLAYER_RANGER_DATA_GEOM
+        read_geom(msg)
+      else
+        unexpected_message hdr
+      end
     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]
-      )
+    def handle_response(hdr, msg)
+      case hdr.subtype
+      when PLAYER_RANGER_REQ_GET_GEOM
+        read_geom(msg)
+      when 2..4
+        nil
+      when PLAYER_RANGER_REQ_GET_CONFIG 
+        data = msg.unpack("G*")
+        [:min_angle, :max_angle, :angular_res, :min_range, :max_range, :range_res, :frequecy].each_with_index do |k,i|
+          @config[k] = data[i]
+        end
+      else
+        unexpected_message hdr
+      end
     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
+    private
+    def read_geom(msg)
+      super(msg[0,72])
+
+      p_count =  msg[72,8].unpack("NN")
+      p_count = p_count[0] + p_count[1] * 256
+      poses = msg[80, 48*p_count].unpack("G" + (6*p_count).to_s)
+
+      s_count = msg[80 + 48*p_count, 8].unpack("NN")
+      s_count = s_count[0] + s_count[1] * 256
+      sizes = msg[88 + 48*p_count, 24*s_count].unpack("G" +(3* s_count).to_s)
+
+      p_count.times do |i|
+        @geom[:sensors][i] ||= {}
+        [:px, :py, :pz, :proll, :ppitch, :pyaw].each_with_index do |k,j|
+          @geom[:sensors][i][k] = poses[6*i + j]
+        end
+        debug("Get poses for ##{i} sensor: px=%.2f, py=%.2f, pz=%.2f, proll=%.2f, ppitch=%.2f, pyaw=%.2f" % @geom[:sensors][i].values[0,6])
+      end
+      
+      s_count.times do |i|
+        @geom[:sensors][i] ||= {}
+        [:sw, :sl, :sh].each_with_index do |k,j|
+          @geom[:sensors][i][k] = sizes[3*i + j]
+        end
+        debug("Get sizes for ##{i} sensor: sw=%.2f, sl=%.2f, sh=%.2f" % @geom[:sensors][i].values[6,3])
+      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

@@ -13,5 +13,5 @@
 # GNU General Public License for more details.
 
 module Player
-  VERSION = "0.0.1"
+  VERSION = "0.1.0"
 end

data/ruby-player.gemspec

@@ -18,10 +18,12 @@ Gem::Specification.new do |s|
 
   s.required_ruby_version = '>= 1.9.2'
 
-  s.add_runtime_dependency "ffi", '~>1.0.0'
+  s.add_runtime_dependency "isna", '~>0.0.4'
+
   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"
+  s.add_development_dependency 'guard-rspec'
 end

data/spec/client_spec.rb

@@ -1,32 +1,166 @@
 require "ruby-player"
 
+include Player
 describe Player::Client do
   before do 
-    @cl = Player::Client.new("localhost")
-  end
+    @socket = mock("Socket")
+
+    TCPSocket.stub!(:new).with("localhost", 6665).and_return(@socket) 
+    @socket.stub! :flush
+    Time.stub!(:now).and_return(0)
+    
+    @socket.should_receive(:read).with(PLAYER_IDENT_STRLEN).and_return("Mock player for 3.1-svn")
+
+    should_send_message(
+      hdr(0,0,1,0, PLAYER_MSGTYPE_REQ,PLAYER_PLAYER_REQ_DATAMODE, 0.0, 0, 4),
+      [PLAYER_DATAMODE_PULL].pack("N")
+    )
 
-  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]")
+    @cl = Player::Client.new("localhost", log_level: "debug")
   end
 
+#  it "should raise error if connection doesn't success" do
+#    lambda{ Player::Client.new(host: "localhost", post: 6666) }.should raise_error(StandardError, 
+#      "connect call on [localhost:6666] failed with error [111:Connection refused]")
+#  end
+
   it "should have close method" do
+    @socket.should_receive(:closed?).and_return(false)
     @cl.closed?.should be_false
+
+    @socket.should_receive(:closed?).and_return(true)
+    @socket.should_receive(:close)
     @cl.close
     @cl.closed?.should be_true
   end
 
   it "should have block for connection" do
-    @cl.close
+    @socket.stub!(:read)
+    @socket.stub!(:write)
+    @socket.should_receive(:closed?).and_return(false)
+    @socket.should_receive(:close)
+
     Player::Client.connect("localhost") do |cl|
-      @cl = cl
-      @cl.closed?.should be_false
+      cl.closed?.should be_false
     end
+  end
 
-    @cl.closed?.should be_true
+  describe "Device manage" do
+    before do
+      #subscribe two position2d
+      should_request_data
+      should_read_message(
+        hdr(0, 0, PLAYER_PLAYER_CODE, 0, PLAYER_MSGTYPE_RESP_ACK, PLAYER_PLAYER_REQ_DEV, 0.0, 0, 35),
+        [0, 0, PLAYER_POSITION2D_CODE, 0, PLAYER_OPEN_MODE, 5, 5].pack("N*") + "mock"
+      )
+
+      @dev_1 = mock("dev_1")
+      @cl.should_receive(:make_device).and_return(@dev_1)
+      @dev_1.stub!(:addr).and_return(Player::DevAddr.decode([0, 6665, PLAYER_POSITION2D_CODE, 0].pack("N*")))
+
+      should_read_message(
+        hdr(0, 0, PLAYER_PLAYER_CODE, 0, PLAYER_MSGTYPE_RESP_ACK, PLAYER_PLAYER_REQ_DEV, 0.0, 0, 35),
+        [0, 0, PLAYER_POSITION2D_CODE, 1, PLAYER_OPEN_MODE, 5, 5].pack("N*") + "mock"
+      )
+
+      @dev_2 = mock("dev_2")
+      @dev_2.stub!(:addr).and_return(Player::DevAddr.decode([0, 6665, PLAYER_POSITION2D_CODE, 1].pack("N*")))
+      @cl.should_receive(:make_device).and_return(@dev_2)
+
+      should_recive_sync
+      @cl.read!
+    end
+
+    it "should read data and fill deivice" do
+      hdr, msg = 
+        hdr(0, 6665, 4, 0, PLAYER_MSGTYPE_DATA, PLAYER_POSITION2D_DATA_STATE, 0.0, 0, 52),
+        [0, 0, 0, 0, 0, 0, 0].pack("GGGGGGN")
+      should_request_data
+
+      should_read_message(hdr, msg)
+
+      should_recive_sync
+      @dev_1.should_receive(:fill).with(hdr, msg)
+
+      @cl.read!
+    end
+
+    it "should recive response and handle deivice" do
+      hdr, msg = 
+        hdr(0, 6665, 4, 1, PLAYER_MSGTYPE_RESP_ACK, PLAYER_POSITION2D_REQ_GET_GEOM, 0.0, 0, 72),
+        ([0.0]*9).pack("G*")
+
+      should_request_data
+
+      should_read_message(hdr, msg)
+
+      should_recive_sync
+      @dev_2.should_receive(:handle_response).with(hdr, msg)
+
+      @cl.read!
+    end
+  end
+
+  describe "Subscribe" do
+    def mock_subscribe(code, index=0)
+      should_send_message(
+        hdr(0, 0, 1, 0, PLAYER_MSGTYPE_REQ, PLAYER_PLAYER_REQ_DEV, 0.0, 0, 28),
+        [0, 0, code, index, PLAYER_OPEN_MODE, 0, 0].pack("N*")
+      )
+
+      should_request_data
+
+      should_read_message(
+        hdr(16777343, 6665, PLAYER_PLAYER_CODE, 0, PLAYER_MSGTYPE_RESP_ACK, PLAYER_PLAYER_REQ_DEV, 0.0, 0, 35),
+        [0, 6665, code, index, PLAYER_OPEN_MODE, 5, 5].pack("N*") + "mock"
+      )
+
+      should_recive_sync
+    end
+
+    it "should describe to position2d:0" do
+      mock_subscribe(PLAYER_POSITION2D_CODE)
+   
+      pos2d = @cl.subscribe("position2d")
+      pos2d.addr.interface_name.should eql("position2d")
+      pos2d.addr.index.should eql(0)
+    end
+
+    it "should describe to ranger:1" do
+      mock_subscribe(PLAYER_RANGER_CODE, 1)
+
+      ranger = @cl.subscribe(:ranger, index: 1)
+      ranger.addr.interface_name.should eql("ranger")
+      ranger.addr.index.should eql(1)
+    end
+  end
+
+  private
+  def hdr(*ary)
+    Player::Header.from_a(ary)
+  end
+
+  def should_send_message(hdr, msg)
+    @socket.should_receive(:write).with(hdr.encode)
+    @socket.should_receive(:write).with(msg)
+  end
+
+  def should_read_message(hdr, msg)
+    @socket.should_receive(:read).with(PLAYERXDR_MSGHDR_SIZE).and_return(hdr.encode)
+    @socket.should_receive(:read).with(hdr.size).and_return(msg)
+  end
+
+  def should_request_data
+    should_send_message(
+      hdr(0, 0, 1, 0, PLAYER_MSGTYPE_REQ, PLAYER_PLAYER_REQ_DATA, 0.0, 0, 0),
+      ""
+    )
   end
 
-  after do
-    @cl.close unless @cl.closed?
+  def should_recive_sync
+    should_read_message(
+      hdr(0, 0, 1, 0, PLAYER_MSGTYPE_SYNCH, 0, 0.0, 0, 0),
+      ""
+    )
   end
 end

data/spec/position2d_spec.rb

@@ -1,77 +1,146 @@
 require "ruby-player"
 
+include Player
 describe Player::Position2d do
   before do
-    @cl = Player::Client.connect("localhost") 
-    @pos2d = @cl[:position2d, 0]
-    @pos2d.stop
-    @cl.read
+    @client = mock("Client")
+    @client.stub!(:write)
+
+    @pos2d = Player::Position2d.new(
+      Player::DevAddr.new(host: 0, robot:0, interface: 4, index: 0),
+      @client,
+      :debug
+    )
+  end
+
+  it 'should have default values' do
+    @pos2d.position.should eql(px:0.0, py:0.0, pa:0.0, vx:0.0, vy:0.0, va:0.0, stall: 0)
+    @pos2d.geom.should eql(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
+
+  it 'should query geometry' do
+    should_send_message(PLAYER_MSGTYPE_REQ, PLAYER_POSITION2D_REQ_GET_GEOM)
+    @pos2d.query_geom
+  end
+
+  it 'should set motor power state' do
+    should_send_message(PLAYER_MSGTYPE_REQ, PLAYER_POSITION2D_REQ_MOTOR_POWER, [0].pack("N"))
+    @pos2d.turn_off!
+
+    should_send_message(PLAYER_MSGTYPE_REQ, PLAYER_POSITION2D_REQ_MOTOR_POWER, [1].pack("N"))
+    @pos2d.turn_on!
+  end
+
+  it 'should set velocity mode' do
+    should_send_message(PLAYER_MSGTYPE_REQ, PLAYER_POSITION2D_REQ_VELOCITY_MODE, [1].pack("N"))
+    @pos2d.separate_speed_control!
+
+    should_send_message(PLAYER_MSGTYPE_REQ, PLAYER_POSITION2D_REQ_VELOCITY_MODE, [0].pack("N"))
+    @pos2d.direct_speed_control!
+  end
+
+  it 'should set control mode' do
+    should_send_message(PLAYER_MSGTYPE_REQ, PLAYER_POSITION2D_REQ_POSITION_MODE, [1].pack("N"))
+    @pos2d.speed_control!
+
+    should_send_message(PLAYER_MSGTYPE_REQ, PLAYER_POSITION2D_REQ_POSITION_MODE, [0].pack("N"))
+    @pos2d.position_control!
   end
 
   it 'should set odometry' do
-    pending "Don't work with Player/Stage"
     new_od = { px: 1.0, py: 2.0, pa: 3 }
+    should_send_message(PLAYER_MSGTYPE_REQ, PLAYER_POSITION2D_REQ_SET_ODOM, new_od.values.pack("GGG"))
     @pos2d.set_odometry(new_od)
+  end
+
+  it 'should reset odometry' do
+    should_send_message(PLAYER_MSGTYPE_REQ, PLAYER_POSITION2D_REQ_RESET_ODOM)
     @pos2d.reset_odometry
-    
-    sleep(1.0)
-    @cl.read
+  end
 
-    @pos2d.odometry.should eql(new_od)
+  it 'should set PID params for speed' do
+    should_send_message(PLAYER_MSGTYPE_REQ, PLAYER_POSITION2D_REQ_SPEED_PID, [1, 2, 3].pack("GGG"))
+    @pos2d.set_speed_pid(kp: 1, ki: 2, kd: 3)
   end
 
-  it 'should move' do
-    pos = @pos2d.odometry
-    speed = {vx: -0.4, vy: 0.2, va: -0.1 }
-    @pos2d.set_speed(speed)
+  it 'should set PID params for position' do
+    should_send_message(PLAYER_MSGTYPE_REQ, PLAYER_POSITION2D_REQ_POSITION_PID, [1, 2, 3].pack("GGG"))
+    @pos2d.set_position_pid(kp: 1, ki: 2, kd: 3)
+  end
+
+  it 'should set speed profile parameters' do
+    should_send_message(PLAYER_MSGTYPE_REQ, PLAYER_POSITION2D_REQ_SPEED_PROF, [1, 2].pack("GG"))
+    @pos2d.set_speed_profile(speed: 1, acc: 2)
+  end
+
+  it 'should fill position data' do
+    pos = {
+      px: 0.0, py: 1.0, pa: 2.0,
+      vx: 3.0, vy: 4.0, va: 5.0,
+      stall: 1
+    }
+    @pos2d.fill(
+      Player::Header.from_a([0,0,4,0, PLAYER_MSGTYPE_DATA, PLAYER_POSITION2D_DATA_STATE, 0.0, 0, 52]),
+      pos.values.pack("GGGGGGN")
+    )
+    @pos2d.position.should eql(pos)
+  end
 
-    sleep(1.1)
-    @cl.read
-    @pos2d.speed.should eq(speed)
+  it 'should fill geom data' do
+    geom = {px: 1.0, py: 2.0, pz: 3.0, proll: 4.0, ppitch: 5.0, pyaw: 6.0, sw: 7.0, sl: 8.0, sh: 9.0}
+    @pos2d.fill(
+      Player::Header.from_a([0,0,4,0, PLAYER_MSGTYPE_DATA, PLAYER_POSITION2D_DATA_GEOM, 0.0, 0, 72]),
+      geom.values.pack("G*")
+    )
+    @pos2d.geom.should eql(geom)
+  end
 
-    #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]))
+  it 'should get geom by request' do
+    geom = {px: 1.0, py: 2.0, pz: 3.0, proll: 4.0, ppitch: 5.0, pyaw: 6.0, sw: 7.0, sl: 8.0, sh: 9.0}
+    @pos2d.handle_response(
+      Player::Header.from_a([0,0,4,0, PLAYER_MSGTYPE_RESP_ACK, PLAYER_POSITION2D_REQ_GET_GEOM, 0.0, 0, 72]),
+      geom.values.pack("G*")
+    )
+    @pos2d.geom.should eql(geom)
   end
 
+  it 'should set speed' do
+    speed = {vx: -0.4, vy: 0.2, va: -0.1, stall: 1 }
+    should_send_message(PLAYER_MSGTYPE_CMD, PLAYER_POSITION2D_CMD_VEL, speed.values.pack("GGGN"))
 
-  it 'should move like car' do
-    pos = @pos2d.odometry
+    @pos2d.set_speed(speed)
+  end
+
+  it 'should set speed like car' do
     speed = { vx: 0.4, a: 0.3 }
+    should_send_message(PLAYER_MSGTYPE_CMD, PLAYER_POSITION2D_CMD_CAR, speed.values.pack("GG"))
+
     @pos2d.set_car(speed)
+  end
 
-    sleep(1.1)
-    @cl.read
-    @pos2d.speed.should eq(vx: 0.4, vy: 0.0, va: 0.3)
+  it 'should set speed head' do
+    speed = { vx: 0.4, a: 0.3 }
+    should_send_message(PLAYER_MSGTYPE_CMD, PLAYER_POSITION2D_CMD_VEL_HEAD, speed.values.pack("GG"))
 
-    #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]))
+    @pos2d.set_speed_head(speed)
   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
+    @pos2d.should_receive(:set_speed).with(vx: 0, vy: 0, va: 0)
+    @pos2d.stop!
   end
 
-  it 'should turn on motor' do
-    pending "Don't work with Player/Stage"
-    @pos2d.enable.should be_false
-    @pos2d.enable = true
+  it 'should not puts warn message for ACK subtypes 2..9' do  
+    @pos2d.should_not_receive(:unexpected_message)
+    (2..9).each do |i|
+      @pos2d.handle_response(
+        Player::Header.from_a([0,0,4,0, PLAYER_MSGTYPE_RESP_ACK, i, 0.0, 0, 0]),
+        "")
+    end
   end
 
-  after do 
-    @cl.close unless @cl.closed?
+  def should_send_message(*args)
+    @pos2d.should_receive(:send_message)
+      .with(*args)
   end
 end