Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Dev/open source jf #291

Merged
merged 37 commits into from
Feb 18, 2024
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
37 commits
Select commit Hold shift + click to select a range
ccea569
Add new Dockerfile
mmattamala Nov 3, 2023
ed56c59
Add pre-commit, docker compose with gui
mmattamala Nov 3, 2023
e537ec5
Initial files for WVN jackal demo
mmattamala Nov 3, 2023
8b23c0f
Merge branch 'dev/open_source' into dev/open_source_mm
mmattamala Nov 3, 2023
e34fc27
Add preliminary world for sim
mmattamala Dec 1, 2023
0644e8d
Add heightmap terrain
mmattamala Jan 15, 2024
d0061dc
Fix tree positions
mmattamala Jan 15, 2024
f031f40
Fix dockerfile
mmattamala Jan 19, 2024
9d6809f
Add Piotr's interface
mmattamala Jan 25, 2024
0b67ed7
Add option to StegoInterface to use feature clustering
mmattamala Jan 26, 2024
c5ccdb3
Fix gui dockerfile
mmattamala Jan 26, 2024
b6ed9a3
Fixes for online execution
mmattamala Jan 27, 2024
afb5eff
Fix unit tests
mmattamala Jan 31, 2024
2dc8509
Add missing testing file
mmattamala Feb 3, 2024
6e0fb8b
Add camera scheduler
mmattamala Feb 3, 2024
9e3c01e
Fix scheduler, fix naming of private variables
mmattamala Feb 3, 2024
5814cd0
Add scheduler weight as param
mmattamala Feb 3, 2024
b617745
Add automatic configuration of model input given the selected features
mmattamala Feb 3, 2024
c4b06d3
updates for new sem
JonasFrey96 Feb 7, 2024
b9b7816
first reordering attempt
JonasFrey96 Feb 7, 2024
8b7c6eb
cleaned up repo
JonasFrey96 Feb 7, 2024
f06e079
running on the robot
JonasFrey96 Feb 8, 2024
e80d803
failing learning node
JonasFrey96 Feb 8, 2024
96dadfa
updated rviz
JonasFrey96 Feb 8, 2024
6c73d70
removed trav and cleaned up config
JonasFrey96 Feb 17, 2024
dd7e084
correct camera configs
JonasFrey96 Feb 17, 2024
781747d
moved to deprecated
JonasFrey96 Feb 17, 2024
a7e5ea6
started updating readme
JonasFrey96 Feb 17, 2024
af50296
confidence generation certainly broken
JonasFrey96 Feb 17, 2024
ae0d176
confidence and traversability look good
JonasFrey96 Feb 17, 2024
c9ed5e8
match mm branch
JonasFrey96 Feb 18, 2024
fd32604
delete rosparams by default
JonasFrey96 Feb 18, 2024
39b31e0
wvn headers
JonasFrey96 Feb 18, 2024
2cd6e05
wvn_ros wvn_anymal header
JonasFrey96 Feb 18, 2024
14b3d67
cmake copy right
JonasFrey96 Feb 18, 2024
403d053
readme and license
JonasFrey96 Feb 18, 2024
f47c6c9
Merge pull request #290 from leggedrobotics/dev/open_source_header
JonasFrey96 Feb 18, 2024
File filter

Filter by extension

Filter by extension


Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions .deprecated/dataset/__init__.py
Original file line number Diff line number Diff line change
@@ -0,0 +1 @@
from .graph_trav_dataset import get_ablation_module
Original file line number Diff line number Diff line change
@@ -1,14 +1,13 @@
from torch_geometric.data import InMemoryDataset, DataListLoader, DataLoader
from torch_geometric.data import LightningDataset
# TODO
# from torch_geometric.data import InMemoryDataset, DataLoader
# from torch_geometric.data import LightningDataset
# from torch_geometric.data import Dataset

from wild_visual_navigation import WVN_ROOT_DIR
import os
import torch
from pathlib import Path
from torch_geometric.data import Dataset
from torchvision import transforms as T
from typing import Optional, Callable
from torch_geometric.data import Data
import random


Expand Down Expand Up @@ -186,13 +185,28 @@ def get_test_dataset(perugia_root, env, feature_key, test_all_datasets, training
)
]
else:
val_dataset = get_test_dataset(perugia_root, env, feature_key, test_all_datasets, training_data_percentage)
val_dataset = get_test_dataset(
perugia_root,
env,
feature_key,
test_all_datasets,
training_data_percentage,
)

train_loader = DataLoader(
dataset=train_dataset, batch_size=batch_size, num_workers=num_workers, pin_memory=False
dataset=train_dataset,
batch_size=batch_size,
num_workers=num_workers,
pin_memory=False,
)
val_loader = [
DataLoader(dataset=v, batch_size=batch_size, num_workers=num_workers, pin_memory=False) for v in val_dataset
DataLoader(
dataset=v,
batch_size=batch_size,
num_workers=num_workers,
pin_memory=False,
)
for v in val_dataset
]

else:
Expand All @@ -212,9 +226,20 @@ def get_test_dataset(perugia_root, env, feature_key, test_all_datasets, training
)
]
else:
test_dataset = get_test_dataset(perugia_root, env, feature_key, test_all_datasets, training_data_percentage)
test_dataset = get_test_dataset(
perugia_root,
env,
feature_key,
test_all_datasets,
training_data_percentage,
)
test_loader = [
DataLoader(dataset=t, batch_size=batch_size, num_workers=num_workers, pin_memory=False)
DataLoader(
dataset=t,
batch_size=batch_size,
num_workers=num_workers,
pin_memory=False,
)
for t in test_dataset
]
else:
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -7,7 +7,6 @@

# Frameworks
import torch
import pytorch_lightning
from pytorch_lightning import seed_everything, Trainer
from pytorch_lightning.callbacks import EarlyStopping
from pytorch_lightning.callbacks import ModelCheckpoint
Expand Down Expand Up @@ -60,7 +59,11 @@ def training_routine(exp: ExperimentParams, seed=42) -> torch.Tensor:

if exp.cb_checkpoint.active:
checkpoint_callback = ModelCheckpoint(
dirpath=model_path, save_top_k=1, monitor="epoch", mode="max", save_last=True
dirpath=model_path,
save_top_k=1,
monitor="epoch",
mode="max",
save_last=True,
)
cb_ls.append(checkpoint_callback)

Expand Down Expand Up @@ -93,7 +96,7 @@ def training_routine(exp: ExperimentParams, seed=42) -> torch.Tensor:
ckpt = torch.load(exp.model.load_ckpt)
try:
res = model.load_state_dict(ckpt.state_dict, strict=False)
except:
except Exception:
res = model.load_state_dict(ckpt, strict=False)
print("Loaded model checkpoint:", res)
trainer = Trainer(**exp.trainer, callbacks=cb_ls, logger=logger)
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -2,15 +2,14 @@
import pytorch_lightning as pl
import torch
import torch.nn.functional as F
from os.path import join
from wild_visual_navigation.visu import LearningVisualizer
from pytorch_lightning.utilities.types import EPOCH_OUTPUT
from torch_geometric.data import Data

# from torch_geometric.data import Data
from torchmetrics import ROC

from wild_visual_navigation.utils import TraversabilityLoss, MetricLogger
import os
import pickle


class LightningTrav(pl.LightningModule):
Expand Down Expand Up @@ -70,8 +69,20 @@ def training_step(self, batch: any, batch_idx: int) -> torch.Tensor:

for k, v in loss_aux.items():
if k.find("loss") != -1:
self.log(f"{self._mode}_{k}", v.item(), on_epoch=True, prog_bar=True, batch_size=BS)
self.log(f"{self._mode}_loss", loss.item(), on_epoch=True, prog_bar=True, batch_size=BS)
self.log(
f"{self._mode}_{k}",
v.item(),
on_epoch=True,
prog_bar=True,
batch_size=BS,
)
self.log(
f"{self._mode}_loss",
loss.item(),
on_epoch=True,
prog_bar=True,
batch_size=BS,
)

self.visu(graph, res_updated, loss_aux["confidence"])

Expand Down Expand Up @@ -112,9 +123,21 @@ def validation_step(self, batch: any, batch_idx: int, dataloader_id: int = 0) ->

for k, v in loss_aux.items():
if k.find("loss") != -1:
self.log(f"{self._mode}_{k}", v.item(), on_epoch=True, prog_bar=True, batch_size=BS)
self.log(
f"{self._mode}_{k}",
v.item(),
on_epoch=True,
prog_bar=True,
batch_size=BS,
)

self.log(f"{self._mode}_loss", loss.item(), on_epoch=True, prog_bar=True, batch_size=BS)
self.log(
f"{self._mode}_loss",
loss.item(),
on_epoch=True,
prog_bar=True,
batch_size=BS,
)
self.visu(graph, res_updated, loss_aux["confidence"])

return loss
Expand All @@ -133,7 +156,7 @@ def update_threshold(self):
fpr, tpr, thresholds = self._auxiliary_training_roc.compute()
index = torch.where(fpr > 0.15)[0][0]
self.threshold[0] = thresholds[index]
except:
except Exception:
pass
else:
self.threshold[0] = 0.5
Expand All @@ -158,13 +181,25 @@ def test_step(self, batch: any, batch_idx: int, dataloader_id: int = 0) -> torch

for k, v in loss_aux.items():
if k.find("loss") != -1:
self.log(f"{self._mode}_{k}", v.item(), on_epoch=True, prog_bar=True, batch_size=BS)
self.log(f"{self._mode}_loss", loss.item(), on_epoch=True, prog_bar=True, batch_size=BS)
self.log(
f"{self._mode}_{k}",
v.item(),
on_epoch=True,
prog_bar=True,
batch_size=BS,
)
self.log(
f"{self._mode}_loss",
loss.item(),
on_epoch=True,
prog_bar=True,
batch_size=BS,
)
self.visu(graph, res_updated, loss_aux["confidence"])
return loss

def test_epoch_end(self, outputs: any, plot=False):
################ NEW VERSION ################
# NEW VERSION
res = self._metric_logger.get_epoch_results("test")
dic2 = {
"trainer_logged_metric" + k: v.item()
Expand Down Expand Up @@ -199,7 +234,7 @@ def visu(self, graph: Data, res: torch.tensor, confidence: torch.tensor):
on_epoch=True,
batch_size=graph.ptr.shape[0] - 1,
)
except:
except Exception:
pass

for b in range(graph.ptr.shape[0] - 1):
Expand Down Expand Up @@ -232,14 +267,22 @@ def visu(self, graph: Data, res: torch.tensor, confidence: torch.tensor):

# Visualize Graph with Segmentation
t1 = self._visualizer.plot_traversability_graph_on_seg(
pred[:, 0], seg, graph[b], center, img, not_log=True, colorize_invalid_centers=True
pred[:, 0],
seg,
graph[b],
center,
img,
not_log=True,
colorize_invalid_centers=True,
)
t2 = self._visualizer.plot_traversability_graph_on_seg(
graph[b].y, seg, graph[b], center, img, not_log=True
)
t_img = self._visualizer.plot_image(img, not_log=True)
self._visualizer.plot_list(
imgs=[t1, t2, t_img], tag=f"C{c}_{self._mode}_GraphTrav", store_folder=f"{self._mode}/graph_trav"
imgs=[t1, t2, t_img],
tag=f"C{c}_{self._mode}_GraphTrav",
store_folder=f"{self._mode}/graph_trav",
)

nr_channel_reco = graph[b].x.shape[1]
Expand All @@ -248,14 +291,27 @@ def visu(self, graph: Data, res: torch.tensor, confidence: torch.tensor):

# # Visualize Graph with Segmentation
c1 = self._visualizer.plot_traversability_graph_on_seg(
conf, seg, graph[b], center, img, not_log=True, colorize_invalid_centers=True
conf,
seg,
graph[b],
center,
img,
not_log=True,
colorize_invalid_centers=True,
)
c2 = self._visualizer.plot_traversability_graph_on_seg(
graph[b].y_valid.type(torch.float32), seg, graph[b], center, img, not_log=True
graph[b].y_valid.type(torch.float32),
seg,
graph[b],
center,
img,
not_log=True,
)
c_img = self._visualizer.plot_image(img, not_log=True)
self._visualizer.plot_list(
imgs=[c1, c2, c_img], tag=f"C{c}_{self._mode}_GraphConf", store_folder=f"{self._mode}/graph_conf"
imgs=[c1, c2, c_img],
tag=f"C{c}_{self._mode}_GraphConf",
store_folder=f"{self._mode}/graph_conf",
)

# if self._mode == "test":
Expand Down Expand Up @@ -316,10 +372,18 @@ def visu(self, graph: Data, res: torch.tensor, confidence: torch.tensor):
reco_loss = F.mse_loss(pred[:, -nr_channel_reco:], graph[b].x, reduction="none").mean(dim=1)

self._visualizer.plot_histogram(
reco_loss, graph[b].y, mean, std, tag=f"C{c}_{self._mode}__confidence_generator_prop"
reco_loss,
graph[b].y,
mean,
std,
tag=f"C{c}_{self._mode}__confidence_generator_prop",
)

if hasattr(graph[b], "y_gt"):
self._visualizer.plot_histogram(
reco_loss, graph[b].y_gt, mean, std, tag=f"C{c}_{self._mode}__confidence_generator_gt"
reco_loss,
graph[b].y_gt,
mean,
std,
tag=f"C{c}_{self._mode}__confidence_generator_gt",
)
Original file line number Diff line number Diff line change
@@ -1,7 +1,6 @@
raise ValueError("TODO: Not tested with new configuration!")
# Use the same config to load the data using the dataloader
from wild_visual_navigation.dataset import get_ablation_module
from wild_visual_navigation import WVN_ROOT_DIR
from wild_visual_navigation.cfg import ExperimentParams
import torch
from torchmetrics import Accuracy, AUROC
Expand All @@ -10,10 +9,8 @@

patch_sklearn()

from sklearn.neighbors import KNeighborsClassifier
from sklearn.svm import SVC
from sklearn.ensemble import RandomForestClassifier
from sklearn.neural_network import MLPClassifier
import numpy as np
import copy
import os
Expand Down Expand Up @@ -125,8 +122,14 @@

for features_test, y_gt, y_prop, test_scene in zip(features_tests, y_gts, y_props, test_scenes):
y_pred = v.predict_proba(features_test)
test_auroc_gt.update(torch.from_numpy(y_pred[:, 1]), torch.from_numpy(y_gt).type(torch.long))
test_auroc_prop.update(torch.from_numpy(y_pred[:, 1]), torch.from_numpy(y_prop).type(torch.long))
test_auroc_gt.update(
torch.from_numpy(y_pred[:, 1]),
torch.from_numpy(y_gt).type(torch.long),
)
test_auroc_prop.update(
torch.from_numpy(y_pred[:, 1]),
torch.from_numpy(y_prop).type(torch.long),
)

res = {
"test_auroc_gt_seg": test_auroc_gt.compute().item(),
Expand All @@ -146,15 +149,16 @@
ws = os.environ.get("ENV_WORKSTATION_NAME", "default")
# Store epoch output to disk.
p = os.path.join(
exp.env.results, f"ablations/classicial_learning_ablation_{ws}/classicial_learning_ablation_test_results.pkl"
exp.env.results,
f"ablations/classicial_learning_ablation_{ws}/classicial_learning_ablation_test_results.pkl",
)
print(results_epoch)
Path(p).parent.mkdir(parents=True, exist_ok=True)

try:
os.remove(p)
except OSError as error:
pass
print(error)

with open(p, "wb") as handle:
pickle.dump(results_epoch, handle, protocol=pickle.HIGHEST_PROTOCOL)
Loading
Loading