In [1]:
from __future__ import division, print_function
import glob
In [2]:
import matplotlib.pyplot as plt
import numpy as np
import pandas as pd
import rosbag_pandas
from scipy.spatial import Voronoi, voronoi_plot_2d
%matplotlib inline
In [3]:
from collections import namedtuple
Coords = namedtuple("Coords", "x y")
In [4]:
from enum import Enum
DroneState = Enum("DroneState",
"Emergency Inited Landed Flying Hovering Test TakingOff GotoHover Landing Looping",
start=0)
In [5]:
from record_bagfiles import ROSBAG_TOPICS
In [6]:
def remove_image_topics(topics):
return [topic for topic in topics if topic not in ("/ardrone/past_image", "/ardrone/slow_image_raw")]
In [7]:
TOPICS_TO_LOAD = remove_image_topics(ROSBAG_TOPICS)
TOPICS_TO_LOAD
Out[7]:
In [8]:
TOPICS_SUMMARIZED = [
"/ardrone/arrived",
"/ardrone/ground_pose",
"/ardrone/pose"
]
In [9]:
TARGET_COORD_OFFSETS = [Coords(0, 0), Coords(-1, 3.6), Coords(-2, 1.6)]
In [10]:
def pad_bag(df):
df.fillna(method="pad", inplace=True)
df.fillna(method="backfill", inplace=True)
In [11]:
def load_bag(filename, include=TOPICS_TO_LOAD):
df = rosbag_pandas.bag_to_dataframe(filename, include=include)
pad_bag(df)
return df
In [12]:
def plot_xy(df, x="x", y="y", invert_x=True, invert_y=True, s=None, c=None,
cmap="viridis", alpha=1, show_cbar=True, label=None, line=False):
"""
Coords in real frame. +x is towards doors, +y is away from table.
"""
if s is not None:
s = df[s]*25
if c is not None:
label = c
try:
df[c]
except KeyError:
pass
else:
c = df[c]
else:
if line:
c = "C0"
else:
c = df.index
label="time"
if line:
plt.plot(df[x] * (-1 if invert_x else 1), df[y] * (-1 if invert_y else 1), c=c, alpha=alpha)
else:
plt.scatter(df[x] * (-1 if invert_x else 1), df[y] * (-1 if invert_y else 1), s=s, c=c, cmap=cmap, alpha=alpha)
if show_cbar:
plt.colorbar(label=label)
plt.axis("equal")
In [13]:
def plot_xy_conditional(df, condition=None, x="x", y="y", invert_x=True, invert_y=True,
s=None, c=None, c_condition=None, cmap_condition="viridis", cmap_not_condition="gray",
alpha_condition=1, alpha_not_condition=1, show_cbar=True, label=None, line=False):
if condition is None:
cmap_not_condition = cmap_condition
plot_xy(df, x=x, y=y, invert_x=invert_x, invert_y=invert_y,
s=s, c=c, cmap=cmap_not_condition, alpha=alpha_not_condition, show_cbar=show_cbar, label=label, line=line)
if condition is not None:
if line:
show_cbar = False
if c_condition is None:
c = "C0"
else:
c = c_condition
plot_xy(df[condition], x=x, y=y, invert_x=invert_x, invert_y=invert_y,
s=s, c=c, cmap=cmap_condition, alpha=alpha_condition, show_cbar=show_cbar, label=label)
In [14]:
def plot_targets(df, x="x", y="y", invert_x=True, invert_y=True,
target_coords=None, target_coord_offsets=None, target_color="r",
show_start=True, show_final=True,
scale=100):
if target_coords is not None and target_coord_offsets is not None:
raise ValueError("Use either target_coords or target_coord_offsets")
p_init = Coords(df[x].iloc[0], df[y].iloc[0])
p_final = Coords(df[x].iloc[-1], df[y].iloc[-1])
if target_coords is None:
if target_coord_offsets is None:
target_coord_offsets = TARGET_COORD_OFFSETS
target_coords = [Coords(p_init.x - offset.x, p_init.y - offset.y) for offset in target_coord_offsets]
if show_start:
plt.scatter(-p_init.x, -p_init.y, marker=(5, 0), s=100, c="g")
for coord in target_coords:
plt.scatter(coord.x * (-1 if invert_x else 1), coord.y * (-1 if invert_y else 1), marker=(5, 1), s=scale, c="m")
if show_final:
plt.scatter(p_final.x * (-1 if invert_x else 1), p_final.y * (-1 if invert_y else 1), marker=(3, 0), s=scale, c="r")
plt.axis("equal")
In [15]:
def rename_columns(df):
df.columns = ["arrived", "angle", "gx", "gy", "qa", "qb", "qc", "qd", "x", "y", "z"]
In [16]:
def normalize_columns(df):
df["xn"] = df.x - df.x[0]
df["yn"] = df.y - df.y[0]
In [17]:
plot_xy(df0, "ardrone_ground_pose__x", "ardrone_ground_pose__y")
plot_targets(df0, "ardrone_ground_pose__x", "ardrone_ground_pose__y")
In [ ]:
plot_xy(df0, "ardrone_ground_pose__x", "ardrone_ground_pose__y",
s="ardrone_pose__pose_position_z", c="ardrone_pose__pose_position_z")
plot_targets(df0, "ardrone_ground_pose__x", "ardrone_ground_pose__y")
In [128]:
plot_xy(df0, "ardrone_ground_pose__x", "ardrone_ground_pose__y",
s="ardrone_pose__pose_position_z", c="ardrone_pose__pose_position_z", cmap="gray", alpha=0.002, show_cbar=False)
plot_xy(df0[df0.ardrone_navdata__state==DroneState.Hovering.value], "ardrone_ground_pose__x", "ardrone_ground_pose__y",
s="ardrone_pose__pose_position_z", c="ardrone_pose__pose_position_z")
plot_targets(df0, "ardrone_ground_pose__x", "ardrone_ground_pose__y")
In [129]:
plot_xy(df0[df0.ardrone_navdata__state==DroneState.Hovering.value], "ardrone_ground_pose__x", "ardrone_ground_pose__y",
s="ardrone_pose__pose_position_z", c="ardrone_pose__pose_position_z")
plot_targets(df0, "ardrone_ground_pose__x", "ardrone_ground_pose__y")
In [15]:
df = load_bag("../data/raw/experiment-2_user-00_run-01.bag")
In [16]:
df.columns
Out[16]:
In [18]:
target_coords = [Coords(df.ardrone_ground_pose__x[0] - offset.x, df.ardrone_ground_pose__y[0] - offset.y)
for offset in TARGET_COORD_OFFSETS]
In [19]:
target_coords
Out[19]:
In [20]:
plot_xy(df, "ardrone_ground_pose__x", "ardrone_ground_pose__y", c="ardrone_pose__pose_position_z",
s="ardrone_pose__pose_position_z")
plot_targets(df, "ardrone_ground_pose__x", "ardrone_ground_pose__y")
In [21]:
plot_xy(df, "ardrone_ground_pose__x", "ardrone_ground_pose__y",
s="ardrone_pose__pose_position_z", c="ardrone_pose__pose_position_z", cmap="gray", alpha=0.002, show_cbar=False)
plot_xy(df[df.ardrone_arrived__data==1],
"ardrone_ground_pose__x", "ardrone_ground_pose__y",
c="ardrone_pose__pose_position_z", s="ardrone_pose__pose_position_z", cmap="magma")
plot_targets(df, "ardrone_ground_pose__x", "ardrone_ground_pose__y")
In [22]:
plot_xy(df[df.ardrone_arrived__data==1],
"ardrone_ground_pose__x", "ardrone_ground_pose__y",
c="ardrone_pose__pose_position_z", s="ardrone_pose__pose_position_z")
In [23]:
arrived_periods = df[df.ardrone_arrived__data == 1]
In [24]:
duration = df.index[-1] - df.index[0]
duration
Out[24]:
In [25]:
df.ardrone_arrived__data.plot()
Out[25]:
In [26]:
len(arrived_periods)
Out[26]:
In [28]:
a = np.where(np.diff([0, 0, 1, 1, 0, 1, 1, 1, 0, 0]))[0]
In [29]:
a
Out[29]:
In [30]:
zip(a[::2], a[1::2])
Out[30]:
So, the first of each pair is the index of the zero, and the second is that of the last 1.
In [31]:
np.where(np.diff(df.ardrone_arrived__data))
Out[31]:
In [32]:
df["angle_degree"] = np.rad2deg(df.ardrone_ground_pose__theta)
In [33]:
df["tag"] = (df.ardrone_arrived__data.diff(1) != 0).astype('int').cumsum()
In [34]:
plot_xy(df, "ardrone_ground_pose__x", "ardrone_ground_pose__y", c="angle_degree", s="ardrone_pose__pose_position_z", cmap="gray")
plot_xy(df[df.ardrone_arrived__data == 1], "ardrone_ground_pose__x", "ardrone_ground_pose__y", c="angle_degree", s="ardrone_pose__pose_position_z")
plot_targets(df, "ardrone_ground_pose__x", "ardrone_ground_pose__y")
In [35]:
df0 = df[["ardrone_arrived__data", "ardrone_ground_pose__x", "ardrone_ground_pose__y", "ardrone_pose__pose_position_z", "angle_degree", "tag"]]
df0.columns = ["arrived", "x", "y", "z", "angle", "region"]
In [36]:
plot_xy(df0, "x", "y", s="z", c="angle", cmap="gray")
plot_xy(df0[df0.arrived==1], "x", "y", s="z", c="angle")
plot_targets(df0, "x", "y")
In [37]:
plot_xy_conditional(df0, df0.arrived == 1, s="z", c="angle")
plot_targets(df0, "x", "y")
In [38]:
from scipy.spatial import Voronoi, voronoi_plot_2d
In [46]:
voronoi_plot_2d(Voronoi([[-x, -y] for x, y in target_coords]))
Out[46]:
In [40]:
voronoi_plot_2d(Voronoi([[-x, -y] for x, y in target_coords]))
plot_targets(df0, "x", "y")
In [41]:
voronoi_plot_2d(Voronoi([[-x, -y] for x, y in target_coords]))
plot_xy_conditional(df0, df0.arrived == 1, s="z", c="angle")
plot_targets(df0, "x", "y")
In [42]:
target_coords
Out[42]:
In [69]:
for region, coord in zip(range(2, max(df0.region)+1, 2), target_coords):
found = df0[df0.region == region]
diff = Coords(found.x.mean() - coord.x, found.y.mean() - coord.y)
print("{:.0f}: {} m difference; (dx={:.2f}, dy={:.2f})".format(region/2, np.linalg.norm(diff), diff.x, diff.y))
What to do with random presses of arrived? Set a minimum time pressed? Minimum sequence length? Something else?
In [49]:
found = df0[df0.region == 4]
coords = target_coords[1]
In [67]:
rms = np.array([np.sqrt((x - coords.x)**2 + (y - coords.y)**2) for x, y in zip(found.x.values, found.y.values)])
In [73]:
rms.mean()
Out[73]:
In [72]:
rms.std()
Out[72]:
In [ ]:
In [ ]:
In [17]:
df = load_bag("../data/raw/experiment-1_user-04_run-03.bag", include=TOPICS_SUMMARIZED)
rename_columns(df)
normalize_columns(df)
df.head()
Out[17]:
In [86]:
plot_targets(df, x="xn", y="yn", target_coords=[Coords(0, -6)])
plot_xy_conditional(df, df.arrived == 1, x="xn", y="yn", line=True)
In [19]:
plot_xy(df)
In [26]:
df = load_bag("../data/raw/experiment-2_user-08_run-01.bag", include=TOPICS_SUMMARIZED)
rename_columns(df)
normalize_columns(df)
plot_xy_conditional(df, df.arrived == 1, x="xn", y="yn", s="z", c="angle")
plot_targets(df, x="xn", y="yn", target_coords=[Coords(0, 0), Coords(0, -6)], show_final=False)
In [26]:
plot_xy(df, x="yn", y="z", invert_y=False, c="x")
In [27]:
df["distance"] = np.sqrt(df.xn**2 + df.yn**2)
In [28]:
plt.plot(df.distance, df.z)
Out[28]:
In [29]:
df.xn.plot(label="x")
plt.plot(df.index, -df.yn, label="y")
df.z.plot(label="z")
plt.legend()
Out[29]:
In [ ]:
In [37]:
for filename in glob.glob("../data/raw/experiment-0_*.bag"):
if "user-99" not in filename:
df = load_bag(filename, include=TOPICS_SUMMARIZED)
rename_columns(df)
if any(df.arrived):
normalize_columns(df)
plot_xy_conditional(df, df.arrived==1, x="xn", y="yn", alpha_not_condition=0.2, line=True)
plot_targets(df, x="xn", y="yn", target_coords=[Coords(0, -6)], show_final=False)
plt.xlabel(r"$x$")
plt.ylabel(r"$y$")
plt.title("Onboard view")
plt.ylim(-1, 8)
Out[37]:
In [38]:
for filename in glob.glob("../data/raw/experiment-1_*.bag"):
if "user-99" not in filename:
df = load_bag(filename, include=TOPICS_SUMMARIZED)
rename_columns(df)
if any(df.arrived):
normalize_columns(df)
plot_xy_conditional(df, df.arrived == 1, x="xn", y="yn", alpha_not_condition=0.2, line=True)
plot_targets(df, x="xn", y="yn", target_coords=[Coords(0, -6)], show_final=False)
plt.xlabel(r"$x$")
plt.ylabel(r"$y$")
plt.title("SPIRIT view")
plt.ylim(-1, 8)
Out[38]:
In [34]:
for filename in glob.glob("../data/raw/experiment-0_*.bag"):
if "user-99" not in filename:
df = load_bag(filename, include=TOPICS_SUMMARIZED)
rename_columns(df)
if any(df.arrived):
normalize_columns(df)
plot_xy_conditional(df, df.arrived==1, x="xn", y="yn", c="C0", c_condition="C0",
alpha_not_condition=0.2, alpha_condition=0.5, line=True)
for filename in glob.glob("../data/raw/experiment-1_*.bag"):
if "user-99" not in filename:
df = load_bag(filename, include=TOPICS_SUMMARIZED)
rename_columns(df)
if any(df.arrived):
normalize_columns(df)
plot_xy_conditional(df, df.arrived==1, x="xn", y="yn", c="C1", c_condition="C1",
alpha_not_condition=0.2, alpha_condition=0.5, line=True)
plot_targets(df, x="xn", y="yn", target_coords=[Coords(0, -6)], show_final=False)
plt.xlabel(r"$x$")
plt.ylabel(r"$y$")
Out[34]:
In [ ]:
In [ ]:
In [153]:
import seaborn as sns
In [155]:
df = load_bag("../data/raw/experiment-1_user-05_run-02.bag", include=TOPICS_SUMMARIZED)
rename_columns(df)
normalize_columns(df)
In [160]:
experiments = pd.DataFrame(columns=["user_id", "experiment_type", "run", "rms_mean", "rms_std",
"rms_x_mean", "rms_x_std", "rms_z_mean", "rms_z_std"])
In [ ]:
experiments = pd.DataFrame
In [ ]:
experiments = pd.Dataframe
In [166]:
df.to_csv("../data/interim/experiment-1_user-05_run-02.csv", index_label="time")
In [202]:
df0 = pd.read_csv("../data/interim/experiment-1_user-05_run-02.csv", parse_dates=["time"])
df0.head()
Out[202]:
In [203]:
plot_xy(df0, x="xn", y="yn", c="time")
In [ ]:
In [36]:
df = pd.read_csv("../data/interim/experiment-1_user-04_run-02.csv", parse_dates=["time"])
df.head()
Out[36]:
In [61]:
for filename in glob.glob("../data/interim/experiment-0_*.csv"):
if "user-99" not in filename:
df = pd.read_csv(filename, parse_dates=["time"])
if any(df.arrived):
plot_xy_conditional(df, df.arrived==1, x="xn", y="yn", alpha_not_condition=0.2, line=True)
plot_targets(df, x="xn", y="yn", target_coords=[Coords(0, -6)], show_final=False)
plt.xlabel(r"$x$")
plt.ylabel(r"$y$")
plt.title("Onboard view")
plt.ylim(-1, 8)
Out[61]:
In [64]:
for filename in glob.glob("../data/interim/experiment-1_*.csv"):
if "user-99" not in filename:
df = pd.read_csv(filename, parse_dates=["time"])
if any(df.arrived):
plot_xy_conditional(df, df.arrived==1, x="xn", y="yn", alpha_not_condition=0.2, line=True)
plot_targets(df, x="xn", y="yn", target_coords=[Coords(0, -6)], show_final=False)
plt.xlabel(r"$x$")
plt.ylabel(r"$y$")
plt.title("Onboard view")
plt.ylim(-1, 8)
Out[64]:
In [63]:
for filename in glob.glob("../data/interim/experiment-0_*.csv"):
if "user-99" not in filename:
df = pd.read_csv(filename, parse_dates=["time"])
if any(df.arrived):
plot_xy_conditional(df, df.arrived==1, x="xn", y="yn", c="C0", c_condition="C0",
alpha_not_condition=0.2, alpha_condition=0.5, line=True)
for filename in glob.glob("../data/interim/experiment-1_*.csv"):
if "user-99" not in filename:
df = pd.read_csv(filename, parse_dates=["time"])
if any(df.arrived):
plot_xy_conditional(df, df.arrived==1, x="xn", y="yn", c="C1", c_condition="C1",
alpha_not_condition=0.2, alpha_condition=0.5, line=True)
plot_targets(df, x="xn", y="yn", target_coords=[Coords(0, -6)], show_final=False)
plt.xlabel(r"$x$")
plt.ylabel(r"$y$")
Out[63]:
In [ ]:
In [ ]:
In [ ]:
In [ ]:
In [ ]:
In [ ]:
In [ ]:
In [ ]:
In [ ]:
In [ ]:
In [ ]:
In [ ]:
In [ ]:
In [ ]: