Class: Player::Position2d

Inherits:
Device
  • Object
show all
Defined in:
lib/ruby-player/position2d.rb

Overview

The position2d proxy provides an interface to a mobile robot base

Examples:

# get proxy object
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!
#read velocityand position by X,Y and angle
pos2d.state #=> { :px => 0.2321, :py => 0,01, :pa => 0.2, :vx => 1.2, :vy => 0.1, :va => 0.3, :stall => 1  }
pos2d.stop!

Instance Attribute Summary collapse

Attributes inherited from Device

#addr

Instance Method Summary collapse

Methods inherited from Device

#send_message

Constructor Details

#initialize(addr, client) ⇒ Position2d

Returns a new instance of Position2d.



40
41
42
43
44
 
# File 'lib/ruby-player/position2d.rb', line 40

def initialize(addr, client)
  super
  @state = {px: 0.0, py: 0.0, pa: 0.0, vx: 0.0, vy: 0.0, va: 0.0, stall: 0}
  @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

Instance Attribute Details

#geom ⇒ Hash (readonly)

Device geometry

Returns:

  • (Hash) —

    geometry { :px, :py. :pz, :proll, :ppitch, :pyaw, :sw, :sl, :sh }



37
38
39
 
# File 'lib/ruby-player/position2d.rb', line 37

def geom
  @geom
end

#state ⇒ Hash (readonly)

Position of robot

Returns:

  • (Hash) —

    hash position {:px, :py, :pa, :vx, :vy, :va, :stall }



33
34
35
 
# File 'lib/ruby-player/position2d.rb', line 33

def state
  @state
end

Instance Method Details

#direct_speed_control! ⇒ Position2d

Returns self.

Returns:



110
111
112
113
 
# File 'lib/ruby-player/position2d.rb', line 110

def direct_speed_control!
  send_message(PLAYER_MSGTYPE_REQ, PLAYER_POSITION2D_REQ_VELOCITY_MODE, [0].pack("N"))
  self
end

#fill(hdr, msg) ⇒ Object 



288
289
290
291
292
293
294
295
296
297
 
# File 'lib/ruby-player/position2d.rb', line 288

def fill(hdr, msg)
  case hdr.subtype
  when PLAYER_POSITION2D_DATA_STATE
    read_state(msg)
  when PLAYER_POSITION2D_DATA_GEOM
    read_geom(msg)
  else
    unexpected_message hdr
  end
end

#handle_response(hdr, msg) ⇒ Object 



299
300
301
302
303
304
305
306
307
308
 
# File 'lib/ruby-player/position2d.rb', line 299

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

#pa ⇒ Float

Yaw [rad]

Returns:

  • (Float) 


60
61
62
 
# File 'lib/ruby-player/position2d.rb', line 60

def pa
  state[:pa]
end

#position_control! ⇒ Position2d

Returns self.

Returns:



122
123
124
125
 
# File 'lib/ruby-player/position2d.rb', line 122

def position_control!
  send_message(PLAYER_MSGTYPE_REQ, PLAYER_POSITION2D_REQ_POSITION_MODE, [0].pack("N"))
  self
end

#power? ⇒ Boolean

State of motor

Returns:

  • (Boolean) —

    true - on



84
85
86
 
# File 'lib/ruby-player/position2d.rb', line 84

def power?
  state[:stall] != 0 
end

#power_off! ⇒ Position2d

Turn off motor

Returns:



104
105
106
107
 
# File 'lib/ruby-player/position2d.rb', line 104

def power_off!
  send_message(PLAYER_MSGTYPE_REQ, PLAYER_POSITION2D_REQ_MOTOR_POWER, [0].pack("N")) 
  self
end

#power_on! ⇒ Position2d

Turn on motor

Returns:



97
98
99
100
 
# File 'lib/ruby-player/position2d.rb', line 97

def power_on!
  send_message(PLAYER_MSGTYPE_REQ, PLAYER_POSITION2D_REQ_MOTOR_POWER, [1].pack("N")) 
  self
end

#px ⇒ Float

X position [m]

Returns:

  • (Float) 


48
49
50
 
# File 'lib/ruby-player/position2d.rb', line 48

def px
  state[:px]
end

#py ⇒ Float

Y position [m]

Returns:

  • (Float) 


54
55
56
 
# File 'lib/ruby-player/position2d.rb', line 54

def py
  state[:py]
end

#query_geom ⇒ Position2d

Query robot geometry

Returns:



90
91
92
93
 
# File 'lib/ruby-player/position2d.rb', line 90

def query_geom
  send_message(PLAYER_MSGTYPE_REQ, PLAYER_POSITION2D_REQ_GET_GEOM)
  self
end

#reset_odometry ⇒ Position2d

Reset odometry to zero

Returns:



199
200
201
202
 
# File 'lib/ruby-player/position2d.rb', line 199

def reset_odometry
  send_message(PLAYER_MSGTYPE_REQ, PLAYER_POSITION2D_REQ_RESET_ODOM)
  self
end

#separate_speed_control! ⇒ Position2d

Returns self.

Returns:



116
117
118
119
 
# File 'lib/ruby-player/position2d.rb', line 116

def separate_speed_control!
  send_message(PLAYER_MSGTYPE_REQ, PLAYER_POSITION2D_REQ_VELOCITY_MODE, [1].pack("N"))
  self
end

#set_car(speeds = {}) ⇒ Position2d

Set speed of carlike robot

Parameters:

  • speeds (Hash) (defaults to: {})

Options Hash (speeds):

  • :vx (Object) —

    forward speed (m/s)

  • :a (Object) —

    turning angle (rad).

Returns:



227
228
229
230
231
232
233
234
 
# File 'lib/ruby-player/position2d.rb', line 227

def set_car(speeds={})
  data = [ 
    speeds[:vx] || @state[:vx],
    speeds[:a] || 0
  ]
  send_message(PLAYER_MSGTYPE_CMD, PLAYER_POSITION2D_CMD_CAR, data.pack("GG"))
  self
end

#set_odometry(odom) ⇒ Position2d

Set odometry of robot.

Parameters:

  • odom (Hash) —

    odometry

Options Hash (odom):

  • :px (Object) —

    x position (m)

  • :py (Object) —

    y position (m)

  • :pa (Object) —

    angle (rad).

Returns:



139
140
141
142
143
144
145
146
147
148
 
# File 'lib/ruby-player/position2d.rb', line 139

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

#set_pose(pose = {}) ⇒ Position2d

Set the target pose with motion vel. (gx, gy, ga) is the target pose in the odometric coordinate system.

Parameters:

  • pose (Hash) (defaults to: {})

Options Hash (pose):

  • :gx (Object) —

    x coordinate [m]

  • :gy (Object) —

    y coordinate [m]

  • :ga (Object) —

    angle [rad]

  • :vx (Object) —

    forward vel [m/s] (default current)

  • :vy (Object) —

    sideways vel [m/s] (default current)

  • :va (Object) —

    rotational speed [rad/s] (default current)

  • :stall (Object) —

    state of motor (default current)

Returns:



262
263
264
265
266
267
268
269
270
271
272
273
274
275
 
# File 'lib/ruby-player/position2d.rb', line 262

def set_pose(pose={})
  data = [
    pose[:gx].to_f,
    pose[:gy].to_f,
    pose[:ga].to_f,
    pose[:vx] || @state[:vx],
    pose[:vy] || @state[:vy],
    pose[:va] || @state[:va],
    pose[:stall] || @state[:stall]
  ]

  send_message(PLAYER_MSGTYPE_CMD, PLAYER_POSITION2D_CMD_POS, data.pack("GGGGGGN"))
  self
end

#set_position_pid(params = {}) ⇒ Position2d

Set position PID paramters

Parameters:

  • params (Hash) (defaults to: {}) —

    PID params

Options Hash (params):

  • :kp (Object) —

    P

  • :ki (Object) —

    I

  • :kd (Object) —

    D

Returns:



172
173
174
175
176
177
178
179
180
 
# File 'lib/ruby-player/position2d.rb', line 172

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

#set_speed(speeds = {}) ⇒ Position2d

Set speed of robot. All speeds are defined in the robot coordinate system.

Parameters:

  • speeds (Hash) (defaults to: {})

Options Hash (speeds):

  • :vx (Object) —

    forward speed (m/s)

  • :vy (Object) —

    sideways speed (m/s); this field is used by omni-drive robots only.

  • :va (Object) —

    rotational speed (rad/s).

  • :stall (Object) —

    state of motor

Returns:



211
212
213
214
215
216
217
218
219
220
 
# File 'lib/ruby-player/position2d.rb', line 211

def set_speed(speeds={})
  data = [
    speeds[:vx] || @state[:vx],
    speeds[:vy] || @state[:vy],
    speeds[:va] || @state[:va],
    speeds[:stall] || @state[:stall]
  ]
  send_message(PLAYER_MSGTYPE_CMD, PLAYER_POSITION2D_CMD_VEL, data.pack("GGGN"))
  self
end

#set_speed_head(speeds = {}) ⇒ Position2d

The position interface accepts translational velocity + absolute heading commands (speed and angular position) for the robot’s motors (only supported by some drivers)

Parameters:

  • speeds (Hash) (defaults to: {})

Options Hash (speeds):

  • :vx (Object) —

    forward speed (m/s)

  • :a (Object) —

    absolutle angle (rad).

Returns:



242
243
244
245
246
247
248
249
 
# File 'lib/ruby-player/position2d.rb', line 242

def set_speed_head(speeds={})
  data = [ 
    speeds[:vx] || @state[:vx],
    speeds[:a] || @state[:pa],
  ]
  send_message(PLAYER_MSGTYPE_CMD, PLAYER_POSITION2D_CMD_VEL_HEAD, data.pack("GG"))
  self
end

#set_speed_pid(params = {}) ⇒ Position2d

Set speed PID paramters

Parameters:

  • params (Hash) (defaults to: {}) —

    PID params

Options Hash (params):

  • :kp (Object) —

    P

  • :ki (Object) —

    I

  • :kd (Object) —

    D

Returns:



156
157
158
159
160
161
162
163
164
 
# File 'lib/ruby-player/position2d.rb', line 156

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

#set_speed_profile(params = {}) ⇒ Position2d

Set speed profile

Parameters:

  • params (Hash) (defaults to: {}) —

    profile prarams

Options Hash (params):

  • :spped (Object) —

    max speed (m/s)

  • :acc (Object) —

    max acceleration (m/s^2)

Returns:



187
188
189
190
191
192
193
194
195
 
# File 'lib/ruby-player/position2d.rb', line 187

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

#speed_control! ⇒ Position2d

Returns self.

Returns:



128
129
130
131
 
# File 'lib/ruby-player/position2d.rb', line 128

def speed_control!
  send_message(PLAYER_MSGTYPE_REQ, PLAYER_POSITION2D_REQ_POSITION_MODE, [1].pack("N"))
  self
end

#stop! ⇒ Position2d

Stop robot set speed to 0

Returns:



279
280
281
 
# File 'lib/ruby-player/position2d.rb', line 279

def stop!
  set_speed(vx: 0, vy: 0, va: 0)
end

#stoped? ⇒ Boolean

Check of robot state

Returns:

  • (Boolean) 


284
285
286
 
# File 'lib/ruby-player/position2d.rb', line 284

def stoped?
  @state[:vx] == 0 && @state[:vy] == 0 && @state[:va] == 0
end

#va ⇒ Float

Yaw speed [rad/s]

Returns:

  • (Float) 


78
79
80
 
# File 'lib/ruby-player/position2d.rb', line 78

def va
  state[:va]
end

#vx ⇒ Float

X speed [m/s]

Returns:

  • (Float) 


66
67
68
 
# File 'lib/ruby-player/position2d.rb', line 66

def vx
  state[:vx]
end

#vy ⇒ Float

Y speed [m/s]

Returns:

  • (Float) 


72
73
74
 
# File 'lib/ruby-player/position2d.rb', line 72

def vy
  state[:vy]
end