Class: CrazyflieZMQ

Inherits:
Object
  • Object
show all
Defined in:
lib/crazyflie-zmq.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

See Also:

Defined Under Namespace

Classes: Error, NotConnected, RequestError, ZMQError

Constant Summary collapse

VERSION =

CrazyflieZMQ version

"0.1.1"

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



44
45
46
47
48
49
50
51
52
53
54
55
56
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
 
# File 'lib/crazyflie-zmq.rb', line 44

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(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(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



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

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 


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

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) 


118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
 
# File 'lib/crazyflie-zmq.rb', line 118

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

    if @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

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

#disconnect ⇒ self

Disconnect from the crazyflie

Returns:

  • (self) 


144
145
146
147
148
149
 
# File 'lib/crazyflie-zmq.rb', line 144

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:



162
163
164
165
 
# File 'lib/crazyflie-zmq.rb', line 162

def is_connected!
    raise NotConnected unless is_connected?
    self
end

#is_connected? ⇒ Boolean

Are we connected to the crazyflie

Returns:

  • (Boolean) —

    connection status



154
155
156
 
# File 'lib/crazyflie-zmq.rb', line 154

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 variable 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) 


178
179
180
181
182
 
# File 'lib/crazyflie-zmq.rb', line 178

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) 


251
252
253
254
 
# File 'lib/crazyflie-zmq.rb', line 251

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

#log_start(name, file: nil, &block) ⇒ 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

  • block —

    block called on each new log

Returns:

  • (self) 


196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
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
 
# File 'lib/crazyflie-zmq.rb', line 196

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) 


241
242
243
244
245
246
 
# File 'lib/crazyflie-zmq.rb', line 241

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

#scan ⇒ Object

Returns the list of available crazyflie



108
109
110
 
# File 'lib/crazyflie-zmq.rb', line 108

def scan()
    _request(cmd: :scan)
end