Plotting while motion commander
Posted: Mon Mar 14, 2022 7:45 am
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
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()