From f06e07908b1d28b4b0965240fd056b4d0abac97f Mon Sep 17 00:00:00 2001 From: Jonas Frey Date: Thu, 8 Feb 2024 18:03:12 +0100 Subject: [PATCH] running on the robot --- .../dataset_generation/create_gnn_dataset.py | 4 +- .../extract_features_for_dataset.py | 4 +- wild_visual_navigation/cfg/global_params.py | 1 + .../dataset/graph_trav_dataset.py | 7 +- .../lightning/lightning_module.py | 3 +- wild_visual_navigation/model/linear_rnvp.py | 2 +- wild_visual_navigation/model/simple_gcn.py | 5 +- wild_visual_navigation/model/simple_mlp.py | 2 +- .../traversability_estimator/nodes.py | 7 +- .../traversability_estimator.py | 20 ++- wild_visual_navigation/utils/__init__.py | 1 + wild_visual_navigation/utils/data.py | 68 ++++++++ wild_visual_navigation/utils/loss.py | 3 +- wild_visual_navigation_anymal/CMakeLists.txt | 6 +- wild_visual_navigation_anymal/README.md | 5 +- .../anymal_sensor_parameter.yaml | 18 +-- .../config/procman/replay.pmd | 2 +- .../config/recording/dodo.yaml | 15 ++ .../config/recording/start_recording.sh | 2 +- .../config/rviz/anymal.rviz | 151 ++++++++---------- .../inputs/wide_angle_dual.yaml | 18 +++ .../launch/overlay_images.launch | 28 ++++ .../launch/robot_jetson.launch | 16 ++ .../launch/robot_npc.launch | 10 ++ .../{robot.launch => robot_replay.launch} | 6 +- .../launch/wild_visual_navigation.launch | 3 - wild_visual_navigation_anymal/package.xml | 1 - .../convert_to_public_format.py | 0 .../encoding_fixer.py | 0 .../scripts/rosbag_play.sh | 15 +- .../scripts/smart_carrot.py | 2 +- .../scripts/wvn_feature_extractor_node.py | 8 +- .../scripts/wvn_learning_node.py | 20 +-- 33 files changed, 309 insertions(+), 144 deletions(-) create mode 100644 wild_visual_navigation/utils/data.py create mode 100644 wild_visual_navigation_anymal/config/wild_visual_navigation/inputs/wide_angle_dual.yaml create mode 100644 wild_visual_navigation_anymal/launch/overlay_images.launch create mode 100644 wild_visual_navigation_anymal/launch/robot_jetson.launch create mode 100644 wild_visual_navigation_anymal/launch/robot_npc.launch rename wild_visual_navigation_anymal/launch/{robot.launch => robot_replay.launch} (79%) rename {wild_visual_navigation_ros => wild_visual_navigation_anymal}/scripts/open_source_rosbag_converter/convert_to_public_format.py (100%) rename {wild_visual_navigation_ros => wild_visual_navigation_anymal}/scripts/open_source_rosbag_converter/encoding_fixer.py (100%) diff --git a/scripts/dataset_generation/create_gnn_dataset.py b/scripts/dataset_generation/create_gnn_dataset.py index 5b51f10d..e4d5b7aa 100644 --- a/scripts/dataset_generation/create_gnn_dataset.py +++ b/scripts/dataset_generation/create_gnn_dataset.py @@ -7,8 +7,8 @@ import argparse import cv2 - -from torch_geometric.data import Data +# TODO +# from torch_geometric.data import Data if __name__ == "__main__": """Converts a folder with images to a torch_geometric dataformat. diff --git a/scripts/dataset_generation/extract_features_for_dataset.py b/scripts/dataset_generation/extract_features_for_dataset.py index cb2bfdd8..617919ea 100644 --- a/scripts/dataset_generation/extract_features_for_dataset.py +++ b/scripts/dataset_generation/extract_features_for_dataset.py @@ -3,7 +3,9 @@ import os from pathlib import Path import torch -from torch_geometric.data import Data + +# TODO +# from torch_geometric.data import Data from wild_visual_navigation.utils import KLTTrackerOpenCV import numpy as np from tqdm import tqdm diff --git a/wild_visual_navigation/cfg/global_params.py b/wild_visual_navigation/cfg/global_params.py index c504da01..a6d8fbe1 100644 --- a/wild_visual_navigation/cfg/global_params.py +++ b/wild_visual_navigation/cfg/global_params.py @@ -11,5 +11,6 @@ def get_global_env_params(name): configs = { "default": GlobalEnvironmentParams(perugia_root="TBD", results="results"), "ge76": GlobalEnvironmentParams(perugia_root="TBD", results="results"), + "jetson": GlobalEnvironmentParams(perugia_root="TBD", results="results"), } return configs[name] diff --git a/wild_visual_navigation/dataset/graph_trav_dataset.py b/wild_visual_navigation/dataset/graph_trav_dataset.py index f39ab5d9..54348a78 100644 --- a/wild_visual_navigation/dataset/graph_trav_dataset.py +++ b/wild_visual_navigation/dataset/graph_trav_dataset.py @@ -1,10 +1,11 @@ -from torch_geometric.data import InMemoryDataset, 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 torch_geometric.data import Dataset from torchvision import transforms as T from typing import Optional, Callable import random diff --git a/wild_visual_navigation/lightning/lightning_module.py b/wild_visual_navigation/lightning/lightning_module.py index 9c88db87..5e49bea8 100644 --- a/wild_visual_navigation/lightning/lightning_module.py +++ b/wild_visual_navigation/lightning/lightning_module.py @@ -4,7 +4,8 @@ import torch.nn.functional as F 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 diff --git a/wild_visual_navigation/model/linear_rnvp.py b/wild_visual_navigation/model/linear_rnvp.py index d91eac47..2d2b3ba4 100644 --- a/wild_visual_navigation/model/linear_rnvp.py +++ b/wild_visual_navigation/model/linear_rnvp.py @@ -1,7 +1,7 @@ import copy import torch from torch import nn, distributions -from torch_geometric.data import Data +from wild_visual_navigation.utils import Data class LinearBatchNorm(nn.Module): diff --git a/wild_visual_navigation/model/simple_gcn.py b/wild_visual_navigation/model/simple_gcn.py index 4beb7955..583ee2ea 100644 --- a/wild_visual_navigation/model/simple_gcn.py +++ b/wild_visual_navigation/model/simple_gcn.py @@ -1,7 +1,8 @@ import torch import torch.nn.functional as F -from torch_geometric.nn import GCNConv -from torch_geometric.data import Data + +# from torch_geometric.nn import GCNConv +from wild_visual_navigation.utils import Data class SimpleGCN(torch.nn.Module): diff --git a/wild_visual_navigation/model/simple_mlp.py b/wild_visual_navigation/model/simple_mlp.py index 96bb27e8..e913a7e1 100644 --- a/wild_visual_navigation/model/simple_mlp.py +++ b/wild_visual_navigation/model/simple_mlp.py @@ -1,5 +1,5 @@ import torch -from torch_geometric.data import Data +from wild_visual_navigation.utils import Data class SimpleMLP(torch.nn.Module): diff --git a/wild_visual_navigation/traversability_estimator/nodes.py b/wild_visual_navigation/traversability_estimator/nodes.py index 8d6c09cf..17aa5a22 100644 --- a/wild_visual_navigation/traversability_estimator/nodes.py +++ b/wild_visual_navigation/traversability_estimator/nodes.py @@ -6,7 +6,8 @@ make_dense_plane, ) from liegroups.torch import SE3, SO3 -from torch_geometric.data import Data +from wild_visual_navigation.utils import Data + import os import torch from typing import Optional @@ -166,10 +167,12 @@ def change_device(self, device): """ super().change_device(device) self._image_projector.change_device(device) - self._image = self._image.to(device) + self._pose_cam_in_base = self._pose_cam_in_base.to(device) self._pose_cam_in_world = self._pose_cam_in_world.to(device) + if self._image is not None: + self._image = self._image.to(device) if self._features is not None: self._features = self._features.to(device) if self._feature_edges is not None: diff --git a/wild_visual_navigation/traversability_estimator/traversability_estimator.py b/wild_visual_navigation/traversability_estimator/traversability_estimator.py index 705827aa..209128ca 100644 --- a/wild_visual_navigation/traversability_estimator/traversability_estimator.py +++ b/wild_visual_navigation/traversability_estimator/traversability_estimator.py @@ -15,7 +15,7 @@ from wild_visual_navigation.visu import LearningVisualizer from pytorch_lightning import seed_everything -from torch_geometric.data import Data, Batch +from wild_visual_navigation.utils import Data, Batch from threading import Lock import os import pickle @@ -124,6 +124,7 @@ def __init__( self._optimizer = torch.optim.Adam(self._model.parameters(), lr=self._params["optimizer"]["lr"]) self._loss = torch.tensor([torch.inf]) self._step = 0 + self._debug_info_node_count = 0 torch.set_grad_enabled(True) @@ -288,9 +289,9 @@ def add_mission_node(self, node: MissionNode, verbose: bool = False, update_feat s += " " * (48 - len(s)) + f"total nodes [{total_nodes}]" if verbose: print(s) - + h, w = node._feature_segments.shape[0], node._feature_segments.shape[1] # Project past footprints on current image - supervision_mask = torch.ones(node.image.shape).to(self._device) * torch.nan + supervision_mask = torch.ones((3, h, w)).to(self._device) * torch.nan # Finally overwrite the current mask node.supervision_mask = supervision_mask @@ -334,6 +335,7 @@ def add_supervision_node(self, pnode: SupervisionNode): return False else: + # If the previous node doesn't exist or it's invalid, we do nothing if last_pnode is None or not last_pnode.is_valid(): return False @@ -348,6 +350,18 @@ def add_supervision_node(self, pnode: SupervisionNode): if (not hasattr(last_mission_node, "supervision_mask")) or (last_mission_node.supervision_mask is None): return False + for j, ele in enumerate( + list(self._mission_graph._graph.nodes._nodes.items())[self._debug_info_node_count :] + ): + node, values = ele + if last_mission_node.timestamp - values["timestamp"] > 20: + node.clear_debug_data() + self._debug_info_node_count += 1 + length = len(list(self._mission_graph._graph.nodes._nodes.items())) + print( + f"cleaned node {self._debug_info_node_count} nodes {self._debug_info_node_count}, length {length}" + ) + # Get all mission nodes within a range mission_nodes = self._mission_graph.get_nodes_within_radius_range( last_mission_node, 0, self._supervision_graph.max_distance diff --git a/wild_visual_navigation/utils/__init__.py b/wild_visual_navigation/utils/__init__.py index 35d04975..3b76f1bd 100644 --- a/wild_visual_navigation/utils/__init__.py +++ b/wild_visual_navigation/utils/__init__.py @@ -1,3 +1,4 @@ +from .data import Data, Batch from .flatten_dict import * from .get_logger import get_logger, get_neptune_run from .loading import load_yaml, file_path, save_omega_cfg diff --git a/wild_visual_navigation/utils/data.py b/wild_visual_navigation/utils/data.py new file mode 100644 index 00000000..ea8a7f4c --- /dev/null +++ b/wild_visual_navigation/utils/data.py @@ -0,0 +1,68 @@ +from typing import Any, List, Optional, Type, Union +from typing_extensions import Self +import torch + + +class Data: + def __init__(self, **kwargs): + for k, v in kwargs.items(): + setattr(self, k, v) + + +class Batch: + def __init__(self): + pass + + @classmethod + def from_data_list(cls, list_of_data: List[Data]) -> Self: + if len(list_of_data) == 0: + return None + + base = ["x"] + + tensors_to_concatenate = [ + k for k in dir(list_of_data[0]) if k[0] != "_" and getattr(list_of_data[0], k) is not None and not k in base + ] + base = base + tensors_to_concatenate + + for k in base: + if k == "edge_index": + ls = [] + for j, data in enumerate(list_of_data): + ls.append(getattr(data, k) + cls.ptr[j]) + + cls.edge_index = torch.cat(ls, dim=-1) + else: + + if k == "x": + running = 0 + ptrs = [running] + batches = [] + + for j, data in enumerate(list_of_data): + running = running + getattr(data, k).shape[0] + ptrs.append(running) + batches += [j] * int(getattr(data, k).shape[0]) + + cls.ptr = torch.tensor(ptrs, dtype=torch.long) + cls.batch = torch.tensor(batches, dtype=torch.long) + + setattr(cls, k, torch.cat([getattr(data, k) for data in list_of_data], dim=0)) + + cls.ba = cls.x.shape[0] + return cls + + +if __name__ == "__main__": + from torch_geometric.data import Data as DataTorchGeometric + from torch_geometric.data import Batch as BatchTorchGeometric + + edge_index = torch.tensor([[0, 1, 1, 2], [1, 0, 2, 1]], dtype=torch.long) + x = torch.tensor([[-1], [0], [1]], dtype=torch.float) + data_tg1 = DataTorchGeometric(x=x, edge_index=edge_index) + data1 = Data(x=x, edge_index=edge_index) + edge_index2 = torch.tensor([[0, 1, 1, 2], [1, 0, 2, 1]], dtype=torch.long) + x2 = torch.tensor([[-1], [12], [1]], dtype=torch.float) + data_tg2 = DataTorchGeometric(x=x2, edge_index=edge_index2) + data2 = Data(x=x2, edge_index=edge_index2) + batch = BatchTorchGeometric.from_data_list([data_tg1, data_tg2]) diff --git a/wild_visual_navigation/utils/loss.py b/wild_visual_navigation/utils/loss.py index 2db05908..ea2769a5 100644 --- a/wild_visual_navigation/utils/loss.py +++ b/wild_visual_navigation/utils/loss.py @@ -1,7 +1,8 @@ from wild_visual_navigation.utils import ConfidenceGenerator import torch.nn.functional as F -from torch_geometric.data import Data +from wild_visual_navigation.utils import Data + import torch from typing import Optional from torch import nn diff --git a/wild_visual_navigation_anymal/CMakeLists.txt b/wild_visual_navigation_anymal/CMakeLists.txt index e8de00f0..72f1a53e 100644 --- a/wild_visual_navigation_anymal/CMakeLists.txt +++ b/wild_visual_navigation_anymal/CMakeLists.txt @@ -5,7 +5,6 @@ set (CMAKE_CXX_STANDARD 14) set(CATKIN_PACKAGE_LIST rospy roscpp - anymal_msgs sensor_msgs std_msgs wild_visual_navigation_msgs @@ -30,9 +29,10 @@ include_directories( ) if(BUILD_ANYMAL) + find_package( anymal_msgs ) # Declare node - add_executable(anymal_msg_converter_cpp_node - src/anymal_msg_converter_cpp_node.cpp) + add_executable( anymal_msg_converter_cpp_node + src/anymal_msg_converter_cpp_node.cpp) target_link_libraries(anymal_msg_converter_cpp_node ${catkin_LIBRARIES} diff --git a/wild_visual_navigation_anymal/README.md b/wild_visual_navigation_anymal/README.md index edef23e4..292b50b4 100644 --- a/wild_visual_navigation_anymal/README.md +++ b/wild_visual_navigation_anymal/README.md @@ -1,6 +1,5 @@ Setup on ANYmal: - NPC: ``` sudo apt-get install ros-noetic-anymal-msgs-dev @@ -11,4 +10,6 @@ catkin build wild_visual_navigation_anymal --cmake-args -DBUILD_ANYMAL=1 Jetson: ``` -``` \ No newline at end of file +``` + +Currently the resizing is not working which is pretty bad for the wide angle camera. \ No newline at end of file diff --git a/wild_visual_navigation_anymal/config/elevation_mapping_cupy/anymal_sensor_parameter.yaml b/wild_visual_navigation_anymal/config/elevation_mapping_cupy/anymal_sensor_parameter.yaml index 5bfd92c5..a9f93e9a 100644 --- a/wild_visual_navigation_anymal/config/elevation_mapping_cupy/anymal_sensor_parameter.yaml +++ b/wild_visual_navigation_anymal/config/elevation_mapping_cupy/anymal_sensor_parameter.yaml @@ -29,15 +29,15 @@ subscribers: data_type: pointcloud # Semantics - front_wide_angle: - topic_name: /wide_angle_camera_front/image_color_rect - camera_info_topic_name: /wide_angle_camera_front/camera_info - data_type: image - - rear_wide_angle: - topic_name: /wide_angle_camera_rear/image_color_rect - camera_info_topic_name: /wide_angle_camera_rear/camera_info - data_type: image + # front_wide_angle: + # topic_name: /wide_angle_camera_front/image_color_rect + # camera_info_topic_name: /wide_angle_camera_front/camera_info + # data_type: image + + # rear_wide_angle: + # topic_name: /wide_angle_camera_rear/image_color_rect + # camera_info_topic_name: /wide_angle_camera_rear/camera_info + # data_type: image # Traversability # channels: ["visual_traversability"] diff --git a/wild_visual_navigation_anymal/config/procman/replay.pmd b/wild_visual_navigation_anymal/config/procman/replay.pmd index 43e175ca..a9f4a7c2 100644 --- a/wild_visual_navigation_anymal/config/procman/replay.pmd +++ b/wild_visual_navigation_anymal/config/procman/replay.pmd @@ -36,7 +36,7 @@ group "1 - WVN" { } cmd "1.2 - overlay" { - exec = "roslaunch wild_visual_navigation_ros overlay_images.launch"; + exec = "roslaunch wild_visual_navigation_anymal overlay_images.launch"; host = "localhost"; } cmd "1.3 - elevation_mapping_cupy" { diff --git a/wild_visual_navigation_anymal/config/recording/dodo.yaml b/wild_visual_navigation_anymal/config/recording/dodo.yaml index 898d3da1..203a100f 100644 --- a/wild_visual_navigation_anymal/config/recording/dodo.yaml +++ b/wild_visual_navigation_anymal/config/recording/dodo.yaml @@ -124,6 +124,7 @@ npc: jetson: elevation_mapping: - /elevation_mapping/elevation_map_raw + - /elevation_mapping/semantic_map generic: - /chrony_monitor_jetson/status - /cpu_loupe_jetson/cpu_loupe @@ -140,3 +141,17 @@ jetson: hdr_camera: - /hdr_camera/image_raw/compressed - /hdr_camera/camera_info + wvn: + - /wild_visual_navigation_node/front/camera_info + - /wild_visual_navigation_node/front/confidence + - /wild_visual_navigation_node/front/feat + - /wild_visual_navigation_node/front/image_input + - /wild_visual_navigation_node/front/traversability + - /wild_visual_navigation_node/graph_footprints + - /wild_visual_navigation_node/graph_footprints_array + - /wild_visual_navigation_node/rear/camera_info + - /wild_visual_navigation_node/rear/confidence + - /wild_visual_navigation_node/rear/image_input + - /wild_visual_navigation_node/rear/traversability + - /wild_visual_navigation_node/robot_state + - /wild_visual_navigation_node/supervision_graph \ No newline at end of file diff --git a/wild_visual_navigation_anymal/config/recording/start_recording.sh b/wild_visual_navigation_anymal/config/recording/start_recording.sh index 348af60f..383db851 100755 --- a/wild_visual_navigation_anymal/config/recording/start_recording.sh +++ b/wild_visual_navigation_anymal/config/recording/start_recording.sh @@ -1 +1 @@ -rosservice call /rosbag_record_coordinator/start_recording "yaml_file: '/home/jonfrey/git/wild_visual_navigation/wild_visual_navigation_ros/config/recording/dodo.yaml'" \ No newline at end of file +rosservice call /rosbag_record_coordinator/start_recording "yaml_file: '/home/jonfrey/git/wild_visual_navigation/wild_visual_navigation_anymal/config/recording/dodo.yaml'" \ No newline at end of file diff --git a/wild_visual_navigation_anymal/config/rviz/anymal.rviz b/wild_visual_navigation_anymal/config/rviz/anymal.rviz index 8316fca5..155514fe 100644 --- a/wild_visual_navigation_anymal/config/rviz/anymal.rviz +++ b/wild_visual_navigation_anymal/config/rviz/anymal.rviz @@ -6,12 +6,13 @@ Panels: Expanded: - /Global Options1 - /TF1/Tree1 + - /Robot Info1 - /Wild Visual Navigation1 - - /Wild Visual Navigation1/Cameras1 - - /Wild Visual Navigation1/Cameras1/AB Wide Angle Front1/Status1 + - /Wild Visual Navigation1/Self Supervision1 + - /Wild Visual Navigation1/Prediction1 - /GridMaps1 - Splitter Ratio: 0.6777777671813965 - Tree Height: 664 + Splitter Ratio: 0.6433260440826416 + Tree Height: 1342 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -41,7 +42,7 @@ Visualization Manager: Cell Size: 1 Class: rviz/Grid Color: 160; 160; 164 - Enabled: true + Enabled: false Line Style: Line Width: 0.029999999329447746 Value: Lines @@ -54,7 +55,7 @@ Visualization Manager: Plane: XY Plane Cell Count: 20 Reference Frame: - Value: true + Value: false - Class: rviz/TF Enabled: false Frame Timeout: 1000 @@ -481,7 +482,7 @@ Visualization Manager: - Alpha: 1 Buffer Length: 1 Class: rviz/Path - Color: 40; 87; 244 + Color: 67; 110; 176 Enabled: true Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 @@ -502,6 +503,30 @@ Visualization Manager: Topic: /wild_visual_navigation_node/supervision_graph Unreliable: false Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 25; 255; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.029999999329447746 + Name: Mission Graph + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: Axes + Queue Size: 10 + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /wild_visual_navigation_node/mission_graph + Unreliable: false + Value: true Enabled: true Name: Self Supervision - Class: rviz/Group @@ -767,7 +792,7 @@ Visualization Manager: - Class: rviz/Group Displays: - Class: rviz/Image - Enabled: true + Enabled: false Image Topic: /wild_visual_navigation_node/front/image_input Max Value: 1 Median window: 5 @@ -777,50 +802,14 @@ Visualization Manager: Queue Size: 2 Transport Hint: raw Unreliable: false - Value: true - - Class: rviz/Image - Enabled: true - Image Topic: /wild_visual_navigation_node/front/traversability - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: Traversability Raw - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: true - - Class: rviz/Image - Enabled: true - Image Topic: /wild_visual_navigation_node/front/debug/last_node_image_overlay - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: Learning Mask - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: true - - Class: rviz/Image - Enabled: false - Image Topic: /wild_visual_navigation_visu_traversability/traversability_overlayed - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: F Traversability Overlay - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false Value: false - Class: rviz/Image Enabled: false - Image Topic: /wild_visual_navigation_visu_0/traversability_overlayed + Image Topic: /wild_visual_navigation_node/front/traversability Max Value: 1 Median window: 5 Min Value: 0 - Name: F Traversability Overlay Replay + Name: Traversability Raw Normalize Range: true Queue Size: 2 Transport Hint: raw @@ -828,35 +817,35 @@ Visualization Manager: Value: false - Class: rviz/Image Enabled: false - Image Topic: /wild_visual_navigation_visu_confidence/confidence_overlayed + Image Topic: /wild_visual_navigation_node/front/debug/last_node_image_overlay Max Value: 1 Median window: 5 Min Value: 0 - Name: F Confidence Overlay + Name: Learning Mask Normalize Range: true Queue Size: 2 Transport Hint: raw Unreliable: false Value: false - Class: rviz/Image - Enabled: false - Image Topic: /wild_visual_navigation_visu_1/confidence_overlayed + Enabled: true + Image Topic: /wild_visual_navigation_visu_front_trav/traversability Max Value: 1 Median window: 5 Min Value: 0 - Name: F Confidence Overlay Replay + Name: F Traversability Overlay Normalize Range: true Queue Size: 2 Transport Hint: raw Unreliable: false - Value: false + Value: true - Class: rviz/Image Enabled: true - Image Topic: /wild_visual_navigation_visu_0/traversability_overlayed + Image Topic: /wild_visual_navigation_visu_rear_trav/traversability Max Value: 1 Median window: 5 Min Value: 0 - Name: Traversability Overlay + Name: R Traversability Overlay Normalize Range: true Queue Size: 2 Transport Hint: raw @@ -1018,7 +1007,7 @@ Visualization Manager: Color Layer: elevation Color Transformer: GridMapLayer ColorMap: terrain - Enabled: true + Enabled: false Grid Cell Decimation: 1 Grid Line Thickness: 0.10000000149011612 Height Layer: elevation @@ -1034,7 +1023,7 @@ Visualization Manager: Topic: /elevation_mapping/semantic_map Unreliable: false Use ColorMap: true - Value: true + Value: false - Alpha: 1 Autocompute Intensity Bounds: false Class: grid_map_rviz_plugin/GridMap @@ -1093,7 +1082,7 @@ Visualization Manager: Enabled: false Grid Cell Decimation: 1 Grid Line Thickness: 0.10000000149011612 - Height Layer: elevation + Height Layer: elevation_with_semantics Height Transformer: "" History Length: 1 Invert ColorMap: true @@ -1108,21 +1097,21 @@ Visualization Manager: Use ColorMap: true Value: false - Alpha: 1 - Autocompute Intensity Bounds: true + Autocompute Intensity Bounds: false Class: grid_map_rviz_plugin/GridMap Color: 200; 200; 200 Color Layer: visual_traversability Color Transformer: "" ColorMap: coolwarm - Enabled: false + Enabled: true Grid Cell Decimation: 1 Grid Line Thickness: 0.10000000149011612 Height Layer: elevation_with_semantics Height Transformer: GridMapLayer History Length: 1 - Invert ColorMap: false + Invert ColorMap: true Max Color: 255; 255; 255 - Max Intensity: 10 + Max Intensity: 1 Min Color: 0; 0; 0 Min Intensity: 0 Name: Visual Traversability @@ -1130,9 +1119,9 @@ Visualization Manager: Topic: /elevation_mapping/semantic_map Unreliable: false Use ColorMap: true - Value: false + Value: true - Alpha: 1 - Autocompute Intensity Bounds: true + Autocompute Intensity Bounds: false Class: grid_map_rviz_plugin/GridMap Color: 200; 200; 200 Color Layer: traversability @@ -1146,7 +1135,7 @@ Visualization Manager: History Length: 1 Invert ColorMap: true Max Color: 255; 255; 255 - Max Intensity: 10 + Max Intensity: 1 Min Color: 0; 0; 0 Min Intensity: 0 Name: Geom Traversability @@ -1161,7 +1150,7 @@ Visualization Manager: Global Options: Background Color: 255; 255; 255 Default Light: true - Fixed Frame: base + Fixed Frame: odom Frame Rate: 30 Name: root Tools: @@ -1185,7 +1174,7 @@ Visualization Manager: Views: Current: Class: rviz/ThirdPersonFollower - Distance: 15.472325325012207 + Distance: 20.380605697631836 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -1193,17 +1182,17 @@ Visualization Manager: Value: false Field of View: 0.7853981852531433 Focal Point: - X: 1.027165174484253 - Y: 0.3078951835632324 - Z: -5.593323707580566 + X: -1.2219188213348389 + Y: -0.08437561988830566 + Z: -5.593324184417725 Focal Shape Fixed Size: false Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.6147975325584412 + Pitch: 0.6297970414161682 Target Frame: base - Yaw: 4.243528842926025 + Yaw: 0.43035051226615906 Saved: ~ Window Geometry: AB Wide Angle Front: @@ -1212,15 +1201,9 @@ Window Geometry: collapsed: false Displays: collapsed: false - F Confidence Overlay: - collapsed: false - F Confidence Overlay Replay: - collapsed: false F Traversability Overlay: collapsed: false - F Traversability Overlay Replay: - collapsed: false - Height: 1043 + Height: 2096 Hide Left Dock: false Hide Right Dock: true Input Image: @@ -1235,11 +1218,13 @@ Window Geometry: collapsed: false Placeholder: collapsed: false - QMainWindow State: 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 + QMainWindow State: 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 R Confidence Raw: collapsed: false R Input Image: collapsed: false + R Traversability Overlay: + collapsed: false R Traversability Raw: collapsed: false Reprojected Path: @@ -1254,14 +1239,12 @@ Window Geometry: collapsed: false Traversability: collapsed: false - Traversability Overlay: - collapsed: false Traversability Raw: collapsed: false Uncertainty: collapsed: false Views: collapsed: true - Width: 1920 - X: 0 - Y: 172 + Width: 3768 + X: 1992 + Y: 27 diff --git a/wild_visual_navigation_anymal/config/wild_visual_navigation/inputs/wide_angle_dual.yaml b/wild_visual_navigation_anymal/config/wild_visual_navigation/inputs/wide_angle_dual.yaml new file mode 100644 index 00000000..b202988a --- /dev/null +++ b/wild_visual_navigation_anymal/config/wild_visual_navigation/inputs/wide_angle_dual.yaml @@ -0,0 +1,18 @@ +camera_topics: + front: + image_topic: "/wide_angle_camera_front/image_color_rect" + info_topic: "/wide_angle_camera_front/camera_info" + use_for_training: true + publish_confidence: true + publish_input_image: true + scheduler_weight: 1 + rear: + image_topic: "/wide_angle_camera_rear/image_color_rect" + info_topic: "/wide_angle_camera_rear/camera_info" + use_for_training: false + publish_confidence: true + publish_input_image: true + scheduler_weight: 1 +# Provides 1080 (height) x 1920 (width) images +network_input_image_height: 224 # 448 +network_input_image_width: 224 # 448 \ No newline at end of file diff --git a/wild_visual_navigation_anymal/launch/overlay_images.launch b/wild_visual_navigation_anymal/launch/overlay_images.launch new file mode 100644 index 00000000..bf323deb --- /dev/null +++ b/wild_visual_navigation_anymal/launch/overlay_images.launch @@ -0,0 +1,28 @@ + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/wild_visual_navigation_anymal/launch/robot_jetson.launch b/wild_visual_navigation_anymal/launch/robot_jetson.launch new file mode 100644 index 00000000..8f152d76 --- /dev/null +++ b/wild_visual_navigation_anymal/launch/robot_jetson.launch @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/wild_visual_navigation_anymal/launch/robot_npc.launch b/wild_visual_navigation_anymal/launch/robot_npc.launch new file mode 100644 index 00000000..7007dedf --- /dev/null +++ b/wild_visual_navigation_anymal/launch/robot_npc.launch @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/wild_visual_navigation_anymal/launch/robot.launch b/wild_visual_navigation_anymal/launch/robot_replay.launch similarity index 79% rename from wild_visual_navigation_anymal/launch/robot.launch rename to wild_visual_navigation_anymal/launch/robot_replay.launch index fd210cea..394bcab7 100644 --- a/wild_visual_navigation_anymal/launch/robot.launch +++ b/wild_visual_navigation_anymal/launch/robot_replay.launch @@ -1,17 +1,17 @@ - + - - + + diff --git a/wild_visual_navigation_anymal/launch/wild_visual_navigation.launch b/wild_visual_navigation_anymal/launch/wild_visual_navigation.launch index 0af623f8..26831988 100644 --- a/wild_visual_navigation_anymal/launch/wild_visual_navigation.launch +++ b/wild_visual_navigation_anymal/launch/wild_visual_navigation.launch @@ -3,7 +3,6 @@ - @@ -25,6 +24,4 @@ - - diff --git a/wild_visual_navigation_anymal/package.xml b/wild_visual_navigation_anymal/package.xml index 6bba4f74..ad4523b2 100644 --- a/wild_visual_navigation_anymal/package.xml +++ b/wild_visual_navigation_anymal/package.xml @@ -14,7 +14,6 @@ rospy roscpp - anymal_msgs nav_msgs sensor_msgs std_msgs diff --git a/wild_visual_navigation_ros/scripts/open_source_rosbag_converter/convert_to_public_format.py b/wild_visual_navigation_anymal/scripts/open_source_rosbag_converter/convert_to_public_format.py similarity index 100% rename from wild_visual_navigation_ros/scripts/open_source_rosbag_converter/convert_to_public_format.py rename to wild_visual_navigation_anymal/scripts/open_source_rosbag_converter/convert_to_public_format.py diff --git a/wild_visual_navigation_ros/scripts/open_source_rosbag_converter/encoding_fixer.py b/wild_visual_navigation_anymal/scripts/open_source_rosbag_converter/encoding_fixer.py similarity index 100% rename from wild_visual_navigation_ros/scripts/open_source_rosbag_converter/encoding_fixer.py rename to wild_visual_navigation_anymal/scripts/open_source_rosbag_converter/encoding_fixer.py diff --git a/wild_visual_navigation_ros/scripts/rosbag_play.sh b/wild_visual_navigation_ros/scripts/rosbag_play.sh index cc7be15f..befd94a5 100755 --- a/wild_visual_navigation_ros/scripts/rosbag_play.sh +++ b/wild_visual_navigation_ros/scripts/rosbag_play.sh @@ -8,15 +8,20 @@ for option in "$@"; do elif [ "$option" == "--wvn" ]; then args="$args /wild_visual_navigation_node/front/camera_info:=/recorded/wild_visual_navigation_node/front/camera_info \ /wild_visual_navigation_node/front/confidence:=/recorded/wild_visual_navigation_node/front/confidence \ + /wild_visual_navigation_node/front/feat:=/recorded/wild_visual_navigation_node/front/feat \ /wild_visual_navigation_node/front/image_input:=/recorded/wild_visual_navigation_node/front/image_input \ /wild_visual_navigation_node/front/traversability:=/recorded/wild_visual_navigation_node/front/traversability \ /wild_visual_navigation_node/graph_footprints:=/recorded/wild_visual_navigation_node/graph_footprints \ - /wild_visual_navigation_node/instant_traversability:=/recorded/wild_visual_navigation_node/instant_traversability \ - /wild_visual_navigation_node/supervision_graph:=/recorded/wild_visual_navigation_node/supervision_graph \ + /wild_visual_navigation_node/graph_footprints_array:=/recorded/wild_visual_navigation_node/graph_footprints_array \ + /wild_visual_navigation_node/mission_graph:=/recorded/wild_visual_navigation_node/mission_graph \ + /wild_visual_navigation_node/rear/camera_info:=/recorded/wild_visual_navigation_node/rear/camera_info \ + /wild_visual_navigation_node/rear/confidence:=/recorded/wild_visual_navigation_node/rear/confidence \ + /wild_visual_navigation_node/rear/image_input:=/recorded/wild_visual_navigation_node/rear/image_input \ + /wild_visual_navigation_node/rear/traversability:=/recorded/wild_visual_navigation_node/rear/traversability \ /wild_visual_navigation_node/robot_state:=/recorded/wild_visual_navigation_node/robot_state \ - /wild_visual_navigation_node/system_state:=/recorded/wild_visual_navigation_node/system_state \ - /wild_visual_navigation_visu_confidence/confidence_overlayed:=/recorded/wild_visual_navigation_visu_confidence/confidence_overlayed \ - /wild_visual_navigation_visu_traversability/traversability_overlayed:=/recorded/wild_visual_navigation_visu_traversability/traversability_overlayed" + /wild_visual_navigation_node/supervision_graph:=/recorded/wild_visual_navigation_node/supervision_graph \ + /wild_visual_navigation_visu_front_trav/traversability:=/recorded/wild_visual_navigation_visu_front_trav/traversability \ + /wild_visual_navigation_visu_rear_trav/traversability:=/recorded/wild_visual_navigation_visu_rear_trav/traversability" elif [ "$option" == "--flp" ]; then args="$args /field_local_planner/action_server/status:=/recorded/field_local_planner/action_server/status \ /field_local_planner/current_base:=/recorded/field_local_planner/current_base \ diff --git a/wild_visual_navigation_ros/scripts/smart_carrot.py b/wild_visual_navigation_ros/scripts/smart_carrot.py index 7f93dd84..8e0cf7d0 100644 --- a/wild_visual_navigation_ros/scripts/smart_carrot.py +++ b/wild_visual_navigation_ros/scripts/smart_carrot.py @@ -10,7 +10,7 @@ class SmartCarrotNode: def __init__(self): - self.gridmap_sub_topic = "/elevation_mapping/elevation_map_wifi" # rospy.get_param("~gridmap_sub_topic", "/elevation_mapping/elevation_map_wifi") + self.gridmap_sub_topic = "/elevation_mapping/elevation_map_wifi" self.pub = rospy.Publisher(f"~binary_mask", GridMap, queue_size=5) self.pub_goal = rospy.Publisher(f"/initialpose", PoseWithCovarianceStamped, queue_size=5) self.tf_buffer = tf2_ros.Buffer(cache_time=rospy.Duration(3.0)) diff --git a/wild_visual_navigation_ros/scripts/wvn_feature_extractor_node.py b/wild_visual_navigation_ros/scripts/wvn_feature_extractor_node.py index 93663c8e..bde01c27 100644 --- a/wild_visual_navigation_ros/scripts/wvn_feature_extractor_node.py +++ b/wild_visual_navigation_ros/scripts/wvn_feature_extractor_node.py @@ -21,7 +21,8 @@ import sys import traceback from omegaconf import OmegaConf, read_write -from torch_geometric.data import Data +from wild_visual_navigation.utils import Data + from threading import Thread, Event from prettytable import PrettyTable from termcolor import colored @@ -476,9 +477,8 @@ def load_model(self, stamp): rospack = rospkg.RosPack() wvn_path = rospack.get_path("wild_visual_navigation_ros") os.system(f"rosparam load {wvn_path}/config/wild_visual_navigation/default.yaml {node_name}") - os.system( - f"rosparam load {wvn_path}/config/wild_visual_navigation/inputs/wide_angle_dual_resize.yaml {node_name}" - ) + wvn_anymal = rospack.get_path("wild_visual_navigation_anymal") + os.system(f"rosparam load {wvn_anymal}/config/wild_visual_navigation/inputs/wide_angle_dual.yaml {node_name}") wvn = WvnFeatureExtractor(node_name) rospy.spin() diff --git a/wild_visual_navigation_ros/scripts/wvn_learning_node.py b/wild_visual_navigation_ros/scripts/wvn_learning_node.py index a3fa27f7..f4489f05 100644 --- a/wild_visual_navigation_ros/scripts/wvn_learning_node.py +++ b/wild_visual_navigation_ros/scripts/wvn_learning_node.py @@ -630,12 +630,7 @@ def imagefeat_callback(self, *args): ).clone() h_small, w_small = feature_segments.shape[1:3] - torch_image = torch.zeros( - (3, h_small, w_small), - device=self._ros_params.device, - dtype=torch.float32, - ) - + torch_image = None # convert image message to torch image if self._ros_params.mode == WVNMode.DEBUG: torch_image = rc.ros_image_to_torch( @@ -666,13 +661,15 @@ def imagefeat_callback(self, *args): if self._ros_params.mode == WVNMode.DEBUG: # Publish current predictions - self.visualize_mission_graph() + # Publish supervision data depending on the mode self.visualize_image_overlay() if added_new_node: self._traversability_estimator.update_visualization_node() + self.visualize_mission_graph() + # Print callback time if required if self._ros_params.print_image_callback_time: rospy.loginfo(f"[{self._node_name}]\n{self._timer}") @@ -954,6 +951,10 @@ def query_tf(self, parent_frame: str, child_frame: str, stamp: Optional[rospy.Ti if __name__ == "__main__": + fn = os.path.join(WVN_ROOT_DIR, ".tmp_state_dict.pt") + if os.path.exists(fn): + os.remove(fn) + node_name = "wvn_learning_node" rospy.init_node(node_name) if rospy.get_param("~reload_default_params", True): @@ -962,9 +963,8 @@ def query_tf(self, parent_frame: str, child_frame: str, stamp: Optional[rospy.Ti rospack = rospkg.RosPack() wvn_path = rospack.get_path("wild_visual_navigation_ros") os.system(f"rosparam load {wvn_path}/config/wild_visual_navigation/default.yaml {node_name}") - os.system( - f"rosparam load {wvn_path}/config/wild_visual_navigation/inputs/wide_angle_dual_resize.yaml {node_name}" - ) + wvn_anymal = rospack.get_path("wild_visual_navigation_anymal") + os.system(f"rosparam load {wvn_anymal}/config/wild_visual_navigation/inputs/wide_angle_dual.yaml {node_name}") wvn = WvnLearning(node_name) rospy.spin()