Plotting while motion commander

Discussions about all things Bitcraze
Post Reply
kbada05
Beginner
Posts: 2
Joined: Mon Mar 14, 2022 7:28 am

Plotting while motion commander

Post by kbada05 »

Hi I'm in step by step tutorial and modifying tutorial codes from web

from motion command tutorial, i add some plot code using animation from matplotlib for drawing position

i add plotting (scatter plot) code above motion command code
but i didnt realize that code can't operate in parallel (after close plotting window, motion starts)

i'm finding more information about controlling and plotting method for realtime positioning of drone

so is there any tip or help for method can i use for drawing ?

my code is down there

Code: Select all

import logging
import sys
import time
import matplotlib.pyplot as plt
import numpy as np
from matplotlib import animation
from matplotlib.animation import FuncAnimation
from threading import Event
from mpl_toolkits import mplot3d
import multiprocessing

import cflib.crtp
from cflib.crazyflie import Crazyflie
from cflib.crazyflie.log import LogConfig
from cflib.crazyflie.syncCrazyflie import SyncCrazyflie
from cflib.crazyflie.syncLogger import SyncLogger

from cflib.positioning.motion_commander import MotionCommander
from cflib.utils import uri_helper

DEFAULT_HEIGHT = 0.2
BOX_LIMIT = 0.3

URI = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7')
deck_attached_event = Event()
logging.basicConfig(level=logging.ERROR)

position_estimate = [0, 0, 0]
x_val=[]
y_val=[]
z_val=[]

fig=plt.figure(figsize=(6,6))
ax=fig.add_subplot(projection='3d')

ax.set_xlim3d([-5,5])
ax.set_ylim3d([-5,5])
ax.set_zlim3d([0,5])

def take_off_simple(scf):
    with MotionCommander(scf, default_height=DEFAULT_HEIGHT) as mc:
        time.sleep(3)
        mc.stop()

def move_box_limit_draw(scf):
    with MotionCommander(scf, default_height=DEFAULT_HEIGHT) as mc:
        body_x_cmd = 0.2
        body_y_cmd = 0.1
        max_vel = 0.2
        
        while (1):
            # if position_estimate[0] > BOX_LIMIT:
            #    mc.start_back()
            # elif position_estimate[0] < - BOX_LIMIT:
            #    mc.start_forward()

            if position_estimate[0] > BOX_LIMIT:
                body_x_cmd = -max_vel
            elif position_estimate[0] < -BOX_LIMIT:
                body_x_cmd = max_vel
            if position_estimate[1] > BOX_LIMIT:
                body_y_cmd = -max_vel
            elif position_estimate[1] < -BOX_LIMIT:
                body_y_cmd = max_vel
            
            mc.start_linear_motion(body_x_cmd, body_y_cmd, 0)

def move_linear_simple(scf):
    with MotionCommander(scf, default_height=DEFAULT_HEIGHT) as mc:
        time.sleep(1)
        mc.forward(0.5)
        time.sleep(1)
        mc.turn_left(180)
        time.sleep(1)
        mc.turn_right(180)
        time.sleep(1)
        mc.back(0.5)
        time.sleep(1)

def log_pos_callback(timestamp, data, logconf):
    print(data)
    global position_estimate
    position_estimate[0] = data['stateEstimate.x']
    position_estimate[1] = data['stateEstimate.y']
    position_estimate[2] = data['stateEstimate.z']

def param_deck_flow(name, value_str):
    value = int(value_str)
    print(value)
    if value:
        deck_attached_event.set()
        print('Deck is attached!')
    else:
        print('Deck is NOT attached!')

def animate(i):
    x_val.append(position_estimate[0])
    y_val.append(position_estimate[1])
    z_val.append(position_estimate[2])
    
    plt.cla()
    
    ax.set_xlim3d([-5,5])
    ax.set_ylim3d([-5,5])
    ax.set_zlim3d([0,5])
    ax.scatter(x_val,y_val,z_val, marker='o', s=15, cmap='Greens')

def anim(i):
    anim = animation.FuncAnimation(plt.gcf(), animate, interval=100, blit=False)
    plt.show()

if __name__ == '__main__':
    cflib.crtp.init_drivers()

    with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf:

        scf.cf.param.add_update_callback(group="deck", name="bcFlow2",
                                                cb=param_deck_flow)
        time.sleep(1)

        logconf = LogConfig(name='Position', period_in_ms=10)
        logconf.add_variable('stateEstimate.x', 'float')
        logconf.add_variable('stateEstimate.y', 'float')
        logconf.add_variable('stateEstimate.z', 'float')
        scf.cf.log.add_config(logconf)
        logconf.data_received_cb.add_callback(log_pos_callback)

        if not deck_attached_event.wait(timeout=5):
            print('No flow deck detected!')
            sys.exit(1)

        logconf.start()
        animation.FuncAnimation(plt.gcf(), animate, interval=100, blit=False)
        plt.show()
        move_linear_simple(scf)
        #move_box_limit_draw(scf)
        logconf.stop()
        
kimberly
Bitcraze
Posts: 1050
Joined: Fri Jul 06, 2018 11:13 am

Re: Plotting while motion commander

Post by kimberly »

Hi!

This seems to be an issue with matplot lib, but a quick 'google to nonblocking plotting with matplotlib in python' already got me on the following stack overflow

https://stackoverflow.com/questions/282 ... matplotlib

This seems to be still valid because I dont thing that the matplotlib api has changed that much.

We have an multiranger plot point cload example here: https://github.com/bitcraze/crazyflie-l ... ntcloud.py

It is using vispy, not matplotlib, but perhaps also worth to try.
Post Reply