class CrazyflieZMQ

Allow control of a {www.bitcraze.io/crazyflie-2/ 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 ({github.com/bitcraze/crazyflie-clients-python/ 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 wiki.bitcraze.io/doc:crazyflie:client:cfzmq:index @see wiki.bitcraze.io/doc:crazyflie:dev:arch:logparam @see wiki.bitcraze.io/projects:crazyflie:firmware:comm_protocol @see wiki.bitcraze.io/doc:crazyflie:crtp:log

Constants

VERSION

Public Class Methods

new(url, log: nil) click to toggle source

Create a {CrazyflieZMQ} instance

@param url [String] the url to connect to the ZMQ socket

# 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

Public Instance Methods

[](group=nil, name) click to toggle source

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”+

@param group [String,nil] group on which the parameter belongs @param name [String] parameter name

# 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) click to toggle source

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”+

@param group [String,nil] group on which the parameter belongs @param name [String] parameter name @param value

# 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) click to toggle source

Establish a connection with a crazyflie

@param uri [String] crazyflie URI @param log_blocks [Hash{Symbol=>Hash}] predefined log blocks @return [self]

# 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() click to toggle source

Disconnect from the crazyflie

@return [self]

# 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!() click to toggle source

Ensure we are in a connect state

@raise [NotConnected] if not connected to a crazyflie @return [self]

# File lib/crazyflie-zmq.rb, line 176
def is_connected!
    raise NotConnected unless is_connected?
    self
end

is_connected?() click to toggle source

Are we connected to the crazyflie

@return [Boolean] connection status

# File lib/crazyflie-zmq.rb, line 168
def is_connected?
    !@connected.nil?
end

log_create(name, *variables, period: 1000) click to toggle source

Create a log block

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

@param name [Symbole,String] log block name @param variables [Array<String>] name of the variable to logs @param period [Integer] milliseconds between consecutive logs @return [self]

# 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) click to toggle source

Delete a registerd log block @param name [Symbol,String] name of the log block @return [self]

# 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, &block) click to toggle source

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

@param name [Symbol, String] name of the log block to start @param file [String, :csv, nil] name or type of file where to automatically log data @yield log data @return [self]

# 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) click to toggle source

Stop logging of the specified log block @param name [Symbol,String] name of the log block @return [self]

# 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() click to toggle source

Returns the list of available crazyflie @return [Array<Hash{String=>String}>] list of available interfaces

# File lib/crazyflie-zmq.rb, line 122
def scan()
    _request(cmd: :scan).dig('interfaces')
end

Private Instance Methods

_json_fix(str) click to toggle source

# File lib/crazyflie-zmq.rb, line 303
def _json_fix(str)
    str.gsub(/(?<=:)
               (?:[\+\-]?Infinity|NaN)
               (?=,|\})/x, 'null')
end

_request(data) click to toggle source

# File lib/crazyflie-zmq.rb, line 315
def _request(data)
    data = data.merge(version: 1)
    resp = ''
    @client_sock.send_string(data.to_json)
    @client_sock.recv_string(resp)
    resp = JSON.parse(_json_fix(resp))
    version = resp.delete('version')
    status  = resp.delete('status')
    unless status.nil? || status.zero?
        raise RequestError, "#{status}: #{resp['msg']}"
    end
    resp
end

_zmq_ok!(rc, msg = nil) click to toggle source

# File lib/crazyflie-zmq.rb, line 309
def _zmq_ok!(rc, msg = nil)
    if !ZMQ::Util.resultcode_ok?(rc)
        raise ZMQError, msg
    end
end