Class: CrazyflieZMQ

Inherits:
Object
  • Object
show all
Defined in:
lib/crazyflie-zmq.rb,
lib/crazyflie-zmq/version.rb

Overview

Allow control of a / Crazyflie drone using the ZMQ protocol.

To use this you need to have a ZMQ server running, it is started using the crazyflie python clients API (/ crazyflie-clients-python):

crazyflie-clients-python/bin/cfzmq --url tcp://* -d

Small demonstration:

$cf = CrazyflieZMQ.new('tcp://_ip_address_') # Connect to ZMQ sockets
$cf.connect('radio://0/80/1M/E7E7E7E7E7')    # Connect to drone
$cf.log_create(:range,                       # Create log block
   'ranging.distance0', 'ranging.distance1', 'ranging.distance2')
$cf['posCtlPid','xKp']= 1.0                  # Parameter (group, name notation)
$cf['posCtlPid.yKp'  ]= 1.0                  # Paramerer (name dotted notation)
$cf.log_start(:range, file: :csv)            # Log to csv generated file
sleep(120)                                   # Wait 2 minutes
$cf.log_stop(:range)                         # Stop logging of :range
$cf.disconnect                               # Disconnect from drone

See Also:

Defined Under Namespace

Classes: Error, NotConnected, RequestError, ZMQError

Constant Summary collapse

VERSION =

CrazyflieZMQ version

"0.1.2"

Instance Method Summary collapse

Constructor Details

#initialize(url, log: nil) ⇒ CrazyflieZMQ

Create a CrazyflieZMQ instance

Parameters:

  • url (String) —

    the url to connect to the ZMQ socket



57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
 
# File 'lib/crazyflie-zmq.rb', line 57

def initialize(url, log: nil)
    @url          = url
    @log_cb       = log
    @log_data_cb  = {}
    @log_file     = {}
    @log_count    = {}
    @log_blocks   = nil
    
    @param        = {}
    @log          = {}
    @connected    = nil
    
    @ctx    = ZMQ::Context.create(1)

    @client_sock = @ctx.socket(ZMQ::REQ)
    _zmq_ok!(@client_sock.setsockopt(ZMQ::LINGER, 0),  "client setsockopt")
    _zmq_ok!(@client_sock.connect("#{@url}:2000"),     "client connect"   )

    
    @param_sock = @ctx.socket(ZMQ::SUB)
    _zmq_ok!(@param_sock.setsockopt(ZMQ::LINGER, 0),   "param setsockopt" )
    _zmq_ok!(@param_sock.setsockopt(ZMQ::SUBSCRIBE,''),"param setsockopt" )
    _zmq_ok!(@param_sock.connect("#{@url}:2002"),      "param connect"    )

    @param_thr = Thread.new {
        loop {
            data = ''
            @param_sock.recv_string(data)
            resp = JSON.parse(_json_fix(data))
            version     = resp.delete('version'  )
            name        = resp.delete('name'     )
            value       = resp.delete('value'    )
            group, name = name.split('.', 2)
            @param.dig(group, name)&.merge('value' => value.to_s)
        }
    }
    @param_thr.abort_on_exception = true

    
    @log_sock = @ctx.socket(ZMQ::SUB)
    _zmq_ok!(@log_sock.setsockopt(ZMQ::LINGER, 0),     "log setsockopt"   )
    _zmq_ok!(@log_sock.setsockopt(ZMQ::SUBSCRIBE, ''), "log setsockopt"   )
    _zmq_ok!(@log_sock.connect("#{@url}:2001"),        "log connect"      )

    @log_thr = Thread.new {
        loop {
            data = ''
            @log_sock.recv_string(data)
            resp = JSON.parse(_json_fix(data))
            version   = resp.delete('version'  )
            event     = resp.delete('event'    ).to_sym
            name      = resp.delete('name'     ).to_sym
            timestamp = resp.delete('timestamp')
            resp = Hash[resp.map {|key, value| [ key.to_sym, value ] }]
            @log_cb&.(event, name, timestamp, resp)
            @log_data_cb[name]&.each {|cb|
                cb.(timestamp, resp) } if event == :data
        }
    }
    @log_thr.abort_on_exception = true
end

Instance Method Details

#[](group = nil, name) ⇒ Object

Get a parameter value from the crazyflie

If a parameter group is not specified it is possible to use a . in the parameter name to indicate it: “group.name”

Parameters:

  • group (String, nil) (defaults to: nil) —

    group on which the parameter belongs

  • name (String) —

    parameter name



295
296
297
298
 
# File 'lib/crazyflie-zmq.rb', line 295

def [](group=nil, name)
    group, name = name.split('.', 2) if group.nil?
    @param.dig(group, name, 'value')
end

#[]=(group = nil, name, value) ⇒ Object

Assign a parameter value to the crazyflie

If a parameter group is not specified it is possible to use a . in the parameter name to indicate it: “group.name”

Parameters:

  • group (String, nil) (defaults to: nil) —

    group on which the parameter belongs

  • name (String) —

    parameter name

  • value 


281
282
283
284
285
 
# File 'lib/crazyflie-zmq.rb', line 281

def []=(group=nil, name, value)
    name = [ group, name ].join('.') if group
    _request(cmd: :param, name: name, value: value)
    value
end

#connect(uri, log_blocks: nil) ⇒ self

Establish a connection with a crazyflie

Parameters:

  • uri (String) —

    crazyflie URI

  • log_blocks (Hash{Symbol=>Hash}) (defaults to: nil) —

    predefined log blocks

Returns:

  • (self) 


132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
 
# File 'lib/crazyflie-zmq.rb', line 132

def connect(uri, log_blocks: nil)
    toc = _request(cmd: :connect, uri: uri)
    @param      = toc['param'] || {}
    @log        = toc['log'  ] || {}
    @connected  = Time.now.freeze
    @log_blocks = log_blocks

    @log_blocks&.each {|key, data|
        variables, period =
            case data
            when Hash   then [ data[:variables], data[:period] ]
            when Array  then [ data ]
            when String then [ [ data ] ]
            end

        if !variables.nil? && !variables.empty?
            self.log_create(key, *variables, period: period || 100)
        end
    }
    
    self
end

#disconnect ⇒ self

Disconnect from the crazyflie

Returns:

  • (self) 


158
159
160
161
162
163
 
# File 'lib/crazyflie-zmq.rb', line 158

def disconnect()
    @log_blocks&.each_key {|key| self.log_delete(key) }
    _request(cmd: :disconnect)
    @connected = @param = @log = @log_blocks = nil
    self
end

#is_connected! ⇒ self

Ensure we are in a connect state

Returns:

  • (self)

Raises:



176
177
178
179
 
# File 'lib/crazyflie-zmq.rb', line 176

def is_connected!
    raise NotConnected unless is_connected?
    self
end

#is_connected? ⇒ Boolean

Are we connected to the crazyflie

Returns:

  • (Boolean) —

    connection status



168
169
170
 
# File 'lib/crazyflie-zmq.rb', line 168

def is_connected?
    !@connected.nil?
end

#log_create(name, *variables, period: 1000) ⇒ self

Note:

logging is usually done through the crazyflie radio link so you are limited in the number of variables that you can log at the same time as well as the minimal logging period that you can use.

Create a log block

Parameters:

  • name (Symbole, String) —

    log block name

  • variables (Array<String>) —

    name of the variable to logs

  • period (Integer) (defaults to: 1000) —

    milliseconds between consecutive logs

Returns:

  • (self) 


193
194
195
196
197
 
# File 'lib/crazyflie-zmq.rb', line 193

def log_create(name, *variables, period: 1000)
    _request(cmd: :log, action: :create, name: name,
             variables: variables, period: period)
    self
end

#log_delete(name) ⇒ self

Delete a registerd log block

Parameters:

  • name (Symbol, String) —

    name of the log block

Returns:

  • (self) 


266
267
268
269
 
# File 'lib/crazyflie-zmq.rb', line 266

def log_delete(name)
    _request(cmd: :log, action: :delete, name: name)
    self
end

#log_start(name, file: nil) { ... } ⇒ self

Start logging information

It is possible to automatically create a log file using the ‘file` parameter, in this case you can specify the file name to use for logging (must end in .csv as for now only CSV format is supported), or you can use :csv and a filename will be generated using timestamp and counter

Parameters:

  • name (Symbol, String) —

    name of the log block to start

  • file (String, :csv, nil) (defaults to: nil) —

    name or type of file where to automatically log data

Yields:

  • log data

Returns:

  • (self) 


211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
 
# File 'lib/crazyflie-zmq.rb', line 211

def log_start(name, file: nil, &block)
    count = (@log_count[name] || 0) + 1

    if block
        (@log_data_cb[name] ||= []) << block
    end

    if file
        case file
        when String
            if ! file.end_with?('.csv')
                raise ArgumentError,
                      "only file with csv extension/format is supported"
            end
        when :csv
            prefix = [ @connected.strftime("%Y%m%dT%H%M"),
                       count
                     ].join('-')
            file   = "#{prefix}-#{name}.csv"
        else
            raise ArgumentError, "unsupported file specification"
        end
        
        variables = case data = @log_blocks[name]
                    when Array then data
                    when Hash  then data[:variables]
                    end
        io = @log_file[name] =
            CSV.open(file, 'wb',
                     :write_headers => true,
                     :headers       => [ 'timestamp' ] + variables)
        (@log_data_cb[name] ||= []) << ->(timestamp, variables:) {
            io << variables.merge('timestamp' => timestamp)
        }
    end

    _request(cmd: :log, action: :start,  name: name)
    @log_count[name] = count
    self
end

#log_stop(name) ⇒ self

Stop logging of the specified log block

Parameters:

  • name (Symbol, String) —

    name of the log block

Returns:

  • (self) 


256
257
258
259
260
261
 
# File 'lib/crazyflie-zmq.rb', line 256

def log_stop(name)
    _request(cmd: :log, action: :stop,   name: name)
    @log_data_cb.delete(name)
    @log_file.delete(name)&.close
    self
end

#scan ⇒ Array<Hash{String=>String}>

Returns the list of available crazyflie

Returns:

  • (Array<Hash{String=>String}>) —

    list of available interfaces



122
123
124
 
# File 'lib/crazyflie-zmq.rb', line 122

def scan()
    _request(cmd: :scan).dig('interfaces')
end