From 467149ecd0123d3276b66fd8b17e69fcf797f413 Mon Sep 17 00:00:00 2001 From: Jonas Frey Date: Wed, 16 Aug 2023 10:07:34 +0200 Subject: [PATCH 01/43] formatting problems --- .../extract_binary_images_and_labels.py | 25 +++++++++++++++---- .../scripts/rotate_image.py | 1 - .../scripts/wild_visual_navigation_node.py | 19 +++++++++++--- 3 files changed, 35 insertions(+), 10 deletions(-) diff --git a/scripts/dataset_generation/extract_binary_images_and_labels.py b/scripts/dataset_generation/extract_binary_images_and_labels.py index f8d4e845..a807eaf1 100644 --- a/scripts/dataset_generation/extract_binary_images_and_labels.py +++ b/scripts/dataset_generation/extract_binary_images_and_labels.py @@ -66,9 +66,11 @@ def do(n, dry_run): valid_topics = ["/state_estimator/anymal_state", "/wide_angle_camera_front/img_out"] - rosbags = ["/home/rschmid/RosBags/6/images.bag", - "/home/rschmid/RosBags/6/2023-03-02-11-13-08_anymal-d020-lpc_mission_0.bag", - "/home/rschmid/RosBags/6/2023-03-02-11-13-08_anymal-d020-lpc_mission_1.bag"] + rosbags = [ + "/home/rschmid/RosBags/6/images.bag", + "/home/rschmid/RosBags/6/2023-03-02-11-13-08_anymal-d020-lpc_mission_0.bag", + "/home/rschmid/RosBags/6/2023-03-02-11-13-08_anymal-d020-lpc_mission_1.bag", + ] output_bag_wvn = s + "_wvn.bag" output_bag_tf = s + "_tf.bag" @@ -115,7 +117,20 @@ def do(n, dry_run): info_msg.distortion_model = "equidistant" info_msg.D = [0.4316922809468283, 0.09279900476637248, -0.4010909691803734, 0.4756163338479413] info_msg.K = [575.6050407221768, 0.0, 745.7312198525915, 0.0, 578.564849365178, 519.5207040671075, 0.0, 0.0, 1.0] - info_msg.P = [575.6050407221768, 0.0, 745.7312198525915, 0.0, 0.0, 578.564849365178, 519.5207040671075, 0.0, 0.0, 0.0, 1.0, 0.0] + info_msg.P = [ + 575.6050407221768, + 0.0, + 745.7312198525915, + 0.0, + 0.0, + 578.564849365178, + 519.5207040671075, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + ] info_msg.R = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] rosbag_info_dict = get_bag_info(output_bag_wvn) @@ -150,7 +165,7 @@ def do(n, dry_run): info_msg.header = msg.header camera_options = {} - camera_options['name'] = "wide_angle_camera_front" + camera_options["name"] = "wide_angle_camera_front" camera_options["use_for_training"] = True info_msg.header = msg.header diff --git a/wild_visual_navigation_ros/scripts/rotate_image.py b/wild_visual_navigation_ros/scripts/rotate_image.py index bed08971..8238f3cb 100644 --- a/wild_visual_navigation_ros/scripts/rotate_image.py +++ b/wild_visual_navigation_ros/scripts/rotate_image.py @@ -6,7 +6,6 @@ from cv_bridge import CvBridge, CvBridgeError - class ImageRotator: def __init__(self): self.bridge = CvBridge() diff --git a/wild_visual_navigation_ros/scripts/wild_visual_navigation_node.py b/wild_visual_navigation_ros/scripts/wild_visual_navigation_node.py index bce0d02b..0d97ac7b 100644 --- a/wild_visual_navigation_ros/scripts/wild_visual_navigation_node.py +++ b/wild_visual_navigation_ros/scripts/wild_visual_navigation_node.py @@ -276,7 +276,9 @@ def read_params(self): # Select mode: # debug, online, extract_labels self.use_debug_for_desired = rospy.get_param("~use_debug_for_desired") # Note: Unused parameter - self.use_binary_only = rospy.get_param("~use_binary_only") # Only extract binary labels, do not update traversability + self.use_binary_only = rospy.get_param( + "~use_binary_only" + ) # Only extract binary labels, do not update traversability self.mode = WVNMode.from_string(rospy.get_param("~mode", "debug")) self.extraction_store_folder = rospy.get_param("~extraction_store_folder") @@ -510,7 +512,12 @@ def query_tf(self, parent_frame: str, child_frame: str, stamp: Optional[rospy.Ti res = self.tf_buffer.lookup_transform(parent_frame, child_frame, stamp) trans = (res.transform.translation.x, res.transform.translation.y, res.transform.translation.z) rot = np.array( - [res.transform.rotation.x, res.transform.rotation.y, res.transform.rotation.z, res.transform.rotation.w] + [ + res.transform.rotation.x, + res.transform.rotation.y, + res.transform.rotation.z, + res.transform.rotation.w, + ] ) rot /= np.linalg.norm(rot) return (trans, tuple(rot)) @@ -593,7 +600,11 @@ def robot_state_callback(self, state_msg, desired_twist_msg: TwistStamped): desired_twist_tensor = rc.twist_stamped_to_torch(desired_twist_msg, device=self.device) # Update traversability - traversability, traversability_var, is_untraversable = self.supervision_generator.update_velocity_tracking( + ( + traversability, + traversability_var, + is_untraversable, + ) = self.supervision_generator.update_velocity_tracking( current_twist_tensor, desired_twist_tensor, velocities=["vx", "vy"] ) @@ -706,7 +717,7 @@ def image_callback(self, image_msg: Image, info_msg: CameraInfo, camera_options: # Add node to graph added_new_node = self.traversability_estimator.add_mission_node(mission_node) - + if not self.use_binary_only: with SystemLevelContextGpuMonitor(self, "update_prediction"): with SystemLevelContextTimer(self, "update_prediction"): From a4268d35043470fbdc45e448519af2d227712a25 Mon Sep 17 00:00:00 2001 From: Jonas Frey Date: Wed, 16 Aug 2023 10:08:10 +0200 Subject: [PATCH 02/43] improved feature extraction node to cont. log --- .../wild_visual_navigation/default.yaml | 3 +- .../scripts/wvn_feature_extractor_node.py | 78 ++++++++++++++++--- 2 files changed, 70 insertions(+), 11 deletions(-) diff --git a/wild_visual_navigation_ros/config/wild_visual_navigation/default.yaml b/wild_visual_navigation_ros/config/wild_visual_navigation/default.yaml index 76310cf1..fb09d7ec 100644 --- a/wild_visual_navigation_ros/config/wild_visual_navigation/default.yaml +++ b/wild_visual_navigation_ros/config/wild_visual_navigation/default.yaml @@ -45,6 +45,7 @@ image_callback_rate: 10 # hertz proprio_callback_rate: 4 # hertz learning_thread_rate: 10 # hertz logging_thread_rate: 2 # hertz +status_thread_rate: 0.5 # hertz # Runtime options device: "cuda" @@ -55,7 +56,7 @@ print_image_callback_time: false print_proprio_callback_time: false log_time: false log_confidence: false -verbose: false +verbose: true debug_supervision_node_index_from_last: 10 extraction_store_folder: "nan" 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 349f6ffa..61fd3df0 100644 --- a/wild_visual_navigation_ros/scripts/wvn_feature_extractor_node.py +++ b/wild_visual_navigation_ros/scripts/wvn_feature_extractor_node.py @@ -11,17 +11,17 @@ import rospy from sensor_msgs.msg import Image, CameraInfo, CompressedImage -from std_msgs.msg import Float32, MultiArrayDimension -from rospy.numpy_msg import numpy_msg -import message_filters +from std_msgs.msg import MultiArrayDimension -from pytictac import Timer, CpuTimer import os import torch import numpy as np import dataclasses from torch_geometric.data import Data import torch.nn.functional as F +from threading import Thread, Event +from prettytable import PrettyTable +from termcolor import colored class WvnFeatureExtractor: @@ -34,6 +34,7 @@ def __init__(self): feature_type=self.feature_type, input_size=self.network_input_image_height, ) + self.i = 0 self.setup_ros() self.model = get_model(self.exp_cfg["model"]).to(self.device) @@ -45,7 +46,38 @@ def __init__(self): self.scale_traversability = True self.traversability_thershold = 0.5 - self.i = 0 + if self.verbose: + self.status_thread = Thread(target=self.status_thread_loop, name="status") + self.status_thread.start() + + def status_thread_loop(self): + rate = rospy.Rate(self.status_thread_rate) + # Learning loop + while True: + t = rospy.get_time() + x = PrettyTable() + x.field_names = ["Key", "Value"] + + for k, v in self.log_data.items(): + if "time" in k: + d = t - v + if d < 0: + c = "red" + if d < 0.2: + c = "green" + elif d < 1.0: + c = "yellow" + else: + c = "red" + x.add_row([k, colored(round(d, 2), c)]) + else: + x.add_row([k, v]) + print(x) + try: + rate.sleep() + except Exception as e: + rate = rospy.Rate(self.status_thread_rate) + print("Ignored jump pack in time!") def read_params(self): """Reads all the parameters from the parameter server""" @@ -65,6 +97,7 @@ def read_params(self): self.confidence_std_factor = rospy.get_param("~confidence_std_factor") self.scale_traversability = rospy.get_param("~scale_traversability") self.scale_traversability_max_fpr = rospy.get_param("~scale_traversability_max_fpr") + self.status_thread_rate = rospy.get_param("~status_thread_rate") # Initialize traversability estimator parameters # Experiment file @@ -80,8 +113,21 @@ def read_params(self): def setup_ros(self, setup_fully=True): """Main function to setup ROS-related stuff: publishers, subscribers and services""" # Image callback + self.camera_handler = {} + + if self.verbose: + # DEBUG Logging + self.log_data = {} + self.log_data[f"time_last_model"] = -1 + self.log_data[f"nr_model_updates"] = -1 + for cam in self.camera_topics: + if self.verbose: + # DEBUG Logging + self.log_data[f"nr_images_{cam}"] = 0 + self.log_data[f"time_last_image_{cam}"] = -1 + # Initialize camera handler for given cam self.camera_handler[cam] = {} # Store camera name @@ -160,7 +206,9 @@ def image_callback(self, image_msg: Image, cam: str): # info_msg: CameraInfo cam (str): Camera name """ if self.verbose: - print("Processing Camera: ", cam) + # DEBUG Logging + self.log_data[f"nr_images_{cam}"] += 1 + self.log_data[f"time_last_image_{cam}"] = rospy.get_time() # Update model from file if possible self.load_model() @@ -254,7 +302,9 @@ def load_model(self): res = torch.load(f"{WVN_ROOT_DIR}/tmp_state_dict2.pt") if (self.model.state_dict()["layers.0.weight"] != res["layers.0.weight"]).any(): if self.verbose: - print("Model updated.") + self.log_data[f"time_last_model"] = rospy.get_time() + self.log_data[f"nr_model_updates"] += 1 + self.model.load_state_dict(res, strict=False) self.traversability_thershold = res["traversability_thershold"] self.confidence_generator_state = res["confidence_generator"] @@ -262,9 +312,6 @@ def load_model(self): self.confidence_generator.var = self.confidence_generator_state["var"] self.confidence_generator.mean = self.confidence_generator_state["mean"] self.confidence_generator.std = self.confidence_generator_state["std"] - else: - if self.verbose: - print("Model did not change.") except Exception as e: if self.verbose: print(f"Model Loading Failed: {e}") @@ -273,5 +320,16 @@ def load_model(self): if __name__ == "__main__": node_name = "wvn_feature_extractor_node" rospy.init_node(node_name) + + if True: + import rospkg + + 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 wvn_feature_extractor_node") + os.system( + f"rosparam load {wvn_path}/config/wild_visual_navigation/inputs/alphasense_compressed.yaml wvn_feature_extractor_node" + ) + wvn = WvnFeatureExtractor() rospy.spin() From 81825ba44ab5770112a2720068746a2a5fb7b8c4 Mon Sep 17 00:00:00 2001 From: Jonas Frey Date: Wed, 16 Aug 2023 12:46:26 +0200 Subject: [PATCH 03/43] added valid nodes to state - dino working --- .../feature_extractor/dino_interface.py | 6 +- .../feature_extractor/feature_extractor.py | 16 +- .../traversability_estimator.py | 9 +- .../msg/SystemState.msg | 2 + .../config/rviz/anymal.rviz | 424 +++++++++++++++++- .../wild_visual_navigation/default.yaml | 8 +- .../scripts/wvn_feature_extractor_node.py | 27 +- .../scripts/wvn_learning_node.py | 10 + 8 files changed, 465 insertions(+), 37 deletions(-) diff --git a/wild_visual_navigation/feature_extractor/dino_interface.py b/wild_visual_navigation/feature_extractor/dino_interface.py index 539fba6d..97a8ec4a 100644 --- a/wild_visual_navigation/feature_extractor/dino_interface.py +++ b/wild_visual_navigation/feature_extractor/dino_interface.py @@ -6,6 +6,7 @@ from omegaconf import DictConfig from torchvision import transforms as T from stego.src.train_segmentation import DinoFeaturizer +from kornia.filters import filter2d class DinoInterface: @@ -68,6 +69,8 @@ def __init__( # Just normalization self.norm = T.Normalize([0.485, 0.456, 0.406], [0.229, 0.224, 0.225]) + self.mean_kernel = torch.ones((1, 5, 5), device=device) / 25 + def change_device(self, device): """Changes the device of all the class members @@ -136,7 +139,8 @@ def inference(self, img: torch.tensor, interpolate: bool = False): pad = int((W - H) / 2) features = F.interpolate(features, new_size, mode="bilinear", align_corners=True) features = F.pad(features, pad=[pad, pad, 0, 0]) - + # Optionally turn on image feature smoothing + # features = filter2d(features, self.mean_kernel, "replicate") return features @property diff --git a/wild_visual_navigation/feature_extractor/feature_extractor.py b/wild_visual_navigation/feature_extractor/feature_extractor.py index 42d30844..8d638a79 100644 --- a/wild_visual_navigation/feature_extractor/feature_extractor.py +++ b/wild_visual_navigation/feature_extractor/feature_extractor.py @@ -66,7 +66,7 @@ def __init__( pass def extract(self, img, **kwargs): - if kwargs.get("fast_random", False): + if self._segmentation_type == "random": dense_feat = self.compute_features(img, None, None, **kwargs) H, W = img.shape[2:] @@ -83,16 +83,16 @@ def extract(self, img, **kwargs): return None, feat, seg, None # Compute segments, their centers, and edges connecting them (graph structure) - with Timer("feature_extractor - compute_segments"): - edges, seg, center = self.compute_segments(img, **kwargs) + # with Timer("feature_extractor - compute_segments"): + edges, seg, center = self.compute_segments(img, **kwargs) # Compute features - with Timer("feature_extractor - compute_features"): - dense_feat = self.compute_features(img, seg, center, **kwargs) + # with Timer("feature_extractor - compute_features"): + dense_feat = self.compute_features(img, seg, center, **kwargs) - with Timer("feature_extractor - compute_features"): - # Sparsify features to match the centers if required - feat = self.sparsify_features(dense_feat, seg) + # with Timer("feature_extractor - compute_features"): + # Sparsify features to match the centers if required + feat = self.sparsify_features(dense_feat, seg) if kwargs.get("return_dense_features", False): return edges, feat, seg, center, dense_feat diff --git a/wild_visual_navigation/traversability_estimator/traversability_estimator.py b/wild_visual_navigation/traversability_estimator/traversability_estimator.py index 40e18b53..8ba55be6 100644 --- a/wild_visual_navigation/traversability_estimator/traversability_estimator.py +++ b/wild_visual_navigation/traversability_estimator/traversability_estimator.py @@ -692,8 +692,9 @@ def train(self): if self._pause_training: return {} - return_dict = {} - if self._mission_graph.get_num_valid_nodes() > self._min_samples_for_training: + num_valid_nodes = self._mission_graph.get_num_valid_nodes() + return_dict = {"mission_graph_num_valid_node": num_valid_nodes} + if num_valid_nodes > self._min_samples_for_training: # Prepare new batch graph = self.make_batch(self._exp_cfg["ablation_data_module"]["batch_size"]) @@ -737,8 +738,10 @@ def train(self): return_dict["loss_total"] = self._loss.item() return_dict["loss_trav"] = loss_aux["loss_trav"].item() return_dict["loss_reco"] = loss_aux["loss_reco"].item() + return return_dict - return {"loss_total": -1} + return_dict["loss_total"] = -1 + return return_dict @accumulate_time def plot_mission_node_prediction(self, node: MissionNode): diff --git a/wild_visual_navigation_msgs/msg/SystemState.msg b/wild_visual_navigation_msgs/msg/SystemState.msg index 2b1e314d..bbfd3c0d 100644 --- a/wild_visual_navigation_msgs/msg/SystemState.msg +++ b/wild_visual_navigation_msgs/msg/SystemState.msg @@ -1,5 +1,7 @@ # Pipeline operation mode uint32 mode +# Current valid samples +uint32 mission_graph_num_valid_node # Training loss float32 loss_total # Training loss diff --git a/wild_visual_navigation_ros/config/rviz/anymal.rviz b/wild_visual_navigation_ros/config/rviz/anymal.rviz index d00e36a2..2bfde7c3 100644 --- a/wild_visual_navigation_ros/config/rviz/anymal.rviz +++ b/wild_visual_navigation_ros/config/rviz/anymal.rviz @@ -10,7 +10,7 @@ Panels: - /Elevation Map WIFI1/Local Planner (SDF)1 - /Elevation Map RAW1 Splitter Ratio: 0.4659898579120636 - Tree Height: 465 + Tree Height: 492 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -116,7 +116,383 @@ Visualization Manager: Expand Joint Details: false Expand Link Details: false Expand Tree: false + LF_FOOT: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + LF_HAA: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + LF_HFE: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + LF_HIP: + Alpha: 1 + Show Axes: false + Show Trail: false + LF_KFE: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + LF_SHANK: + Alpha: 1 + Show Axes: false + Show Trail: false + LF_THIGH: + Alpha: 1 + Show Axes: false + Show Trail: false + LF_hip_fixed: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + LF_shank_fixed: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + LF_thigh_fixed: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + LH_FOOT: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + LH_HAA: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + LH_HFE: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + LH_HIP: + Alpha: 1 + Show Axes: false + Show Trail: false + LH_KFE: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + LH_SHANK: + Alpha: 1 + Show Axes: false + Show Trail: false + LH_THIGH: + Alpha: 1 + Show Axes: false + Show Trail: false + LH_hip_fixed: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + LH_shank_fixed: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + LH_thigh_fixed: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true Link Tree Style: Links in Alphabetic Order + RF_FOOT: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RF_HAA: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RF_HFE: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RF_HIP: + Alpha: 1 + Show Axes: false + Show Trail: false + RF_KFE: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RF_SHANK: + Alpha: 1 + Show Axes: false + Show Trail: false + RF_THIGH: + Alpha: 1 + Show Axes: false + Show Trail: false + RF_hip_fixed: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RF_shank_fixed: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RF_thigh_fixed: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RH_FOOT: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RH_HAA: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RH_HFE: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RH_HIP: + Alpha: 1 + Show Axes: false + Show Trail: false + RH_KFE: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RH_SHANK: + Alpha: 1 + Show Axes: false + Show Trail: false + RH_THIGH: + Alpha: 1 + Show Axes: false + Show Trail: false + RH_hip_fixed: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RH_shank_fixed: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RH_thigh_fixed: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + alphasense_mesh: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_inertia: + Alpha: 1 + Show Axes: false + Show Trail: false + battery: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + body_top: + Alpha: 1 + Show Axes: false + Show Trail: false + bottom_shell: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + cam0_sensor_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + cam1_sensor_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + cam2_sensor_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + cam3_sensor_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + cam3_sensor_frame_helper: + Alpha: 1 + Show Axes: false + Show Trail: false + cam4_sensor_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + cam4_sensor_frame_helper: + Alpha: 1 + Show Axes: false + Show Trail: false + cam5_sensor_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + cam5_sensor_frame_helper: + Alpha: 1 + Show Axes: false + Show Trail: false + cam6_sensor_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + cam6_sensor_frame_helper: + Alpha: 1 + Show Axes: false + Show Trail: false + depth_camera_front_camera: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + depth_camera_front_camera_parent: + Alpha: 1 + Show Axes: false + Show Trail: false + depth_camera_left_camera: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + depth_camera_left_camera_parent: + Alpha: 1 + Show Axes: false + Show Trail: false + depth_camera_rear_camera: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + depth_camera_rear_camera_parent: + Alpha: 1 + Show Axes: false + Show Trail: false + depth_camera_right_camera: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + depth_camera_right_camera_parent: + Alpha: 1 + Show Axes: false + Show Trail: false + docking_hatch_cover: + Alpha: 1 + Show Axes: false + Show Trail: false + face_front: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + face_rear: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_handle: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + handle: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + hatch: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + imu_link: + Alpha: 1 + Show Axes: false + Show Trail: false + imu_sensor_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + interface_hatch: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lidar: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + perception_head_cage: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + remote: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + top_shell: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wide_angle_camera_front_camera: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wide_angle_camera_front_camera_parent: + Alpha: 1 + Show Axes: false + Show Trail: false + wide_angle_camera_rear_camera: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wide_angle_camera_rear_camera_parent: + Alpha: 1 + Show Axes: false + Show Trail: false Name: RobotModel Robot Description: anymal_description TF Prefix: "" @@ -488,8 +864,8 @@ Visualization Manager: - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: 11.413318634033203 - Min Value: -0.9144046306610107 + Max Value: 11.379964828491211 + Min Value: -1.0356265306472778 Value: true Axis: Z Channel Name: intensity @@ -518,7 +894,7 @@ Visualization Manager: - Class: rviz/Group Displays: - Class: rviz/Image - Enabled: false + Enabled: true Image Topic: /wild_visual_navigation_node/front/image_input Max Value: 1 Median window: 5 @@ -528,7 +904,7 @@ Visualization Manager: Queue Size: 2 Transport Hint: raw Unreliable: false - Value: false + Value: true - Class: rviz/Image Enabled: false Image Topic: /wild_visual_navigation_node/front/traversability @@ -673,7 +1049,7 @@ Visualization Manager: - Class: rviz/Group Displays: - Class: rviz/Image - Enabled: true + Enabled: false Image Topic: /wild_visual_navigation_node/left/image_input Max Value: 1 Median window: 5 @@ -683,9 +1059,9 @@ Visualization Manager: Queue Size: 2 Transport Hint: raw Unreliable: false - Value: true + Value: false - Class: rviz/Image - Enabled: true + Enabled: false Image Topic: /wild_visual_navigation_node/left/traversability Max Value: 1 Median window: 5 @@ -695,7 +1071,7 @@ Visualization Manager: Queue Size: 2 Transport Hint: raw Unreliable: false - Value: true + Value: false - Class: rviz/Image Enabled: false Image Topic: /wild_visual_navigation_node/left/confidence @@ -1068,6 +1444,18 @@ Visualization Manager: Value: false Enabled: true Name: Elevation Map RAW + - Class: rviz/Image + Enabled: true + Image Topic: /wild_visual_navigation_visu_0/traversability_overlayed + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: F Trav Over + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: true Enabled: true Global Options: Background Color: 255; 255; 255 @@ -1096,7 +1484,7 @@ Visualization Manager: Views: Current: Class: rviz/ThirdPersonFollower - Distance: 15.3408784866333 + Distance: 15.707101821899414 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -1112,9 +1500,9 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.734797477722168 + Pitch: 0.8647973537445068 Target Frame: base - Yaw: 5.81669807434082 + Yaw: 5.881691932678223 Saved: ~ Window Geometry: AB Wide Angle Front: @@ -1135,13 +1523,15 @@ Window Geometry: collapsed: false F Input Image: collapsed: false + F Trav Over: + collapsed: false F Traversability Overlay: collapsed: false F Traversability Overlay Replay: collapsed: false F Traversability Raw: collapsed: false - Height: 1016 + Height: 1043 Hide Left Dock: false Hide Right Dock: true L Confidence Raw: @@ -1152,7 +1542,7 @@ 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: @@ -1175,6 +1565,6 @@ Window Geometry: collapsed: false Views: collapsed: true - Width: 1848 - X: 72 - Y: 27 + Width: 1920 + X: 299 + Y: 122 diff --git a/wild_visual_navigation_ros/config/wild_visual_navigation/default.yaml b/wild_visual_navigation_ros/config/wild_visual_navigation/default.yaml index fb09d7ec..afc237cf 100644 --- a/wild_visual_navigation_ros/config/wild_visual_navigation/default.yaml +++ b/wild_visual_navigation_ros/config/wild_visual_navigation/default.yaml @@ -9,8 +9,8 @@ base_frame: base footprint_frame: footprint # Robot size -robot_length: 1.0 -robot_width: 0.6 +robot_length: 0.8 +robot_width: 0.4 robot_height: 0.3 # Robot specs @@ -22,14 +22,14 @@ image_graph_dist_thr: 0.2 # meters proprio_graph_dist_thr: 0.1 # meters network_input_image_height: 224 # 448 network_input_image_width: 224 # 448 -segmentation_type: "random" +segmentation_type: "slic" feature_type: "dino" dino_patch_size: 16 # DINO only confidence_std_factor: 4.0 scale_traversability: True scale_traversability_max_fpr: 0.25 min_samples_for_training: 5 - +prediction_per_pixel: False # Optical flow params optical_flow_estimator_type: "none" 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 61fd3df0..fe06cc3a 100644 --- a/wild_visual_navigation_ros/scripts/wvn_feature_extractor_node.py +++ b/wild_visual_navigation_ros/scripts/wvn_feature_extractor_node.py @@ -22,6 +22,8 @@ from threading import Thread, Event from prettytable import PrettyTable from termcolor import colored +import signal +import sys class WvnFeatureExtractor: @@ -48,12 +50,23 @@ def __init__(self): if self.verbose: self.status_thread = Thread(target=self.status_thread_loop, name="status") + self.run_status_thread = True self.status_thread.start() + rospy.on_shutdown(self.shutdown_callback) + signal.signal(signal.SIGINT, self.shutdown_callback) + signal.signal(signal.SIGTERM, self.shutdown_callback) + + def shutdown_callback(self, *args, **kwargs): + self.run_status_thread = False + self.status_thread.join() + rospy.signal_shutdown(f"Wild Visual Navigation Feature Extraction killed {args}") + sys.exit(0) + def status_thread_loop(self): rate = rospy.Rate(self.status_thread_rate) # Learning loop - while True: + while self.run_status_thread: t = rospy.get_time() x = PrettyTable() x.field_names = ["Key", "Value"] @@ -98,7 +111,7 @@ def read_params(self): self.scale_traversability = rospy.get_param("~scale_traversability") self.scale_traversability_max_fpr = rospy.get_param("~scale_traversability_max_fpr") self.status_thread_rate = rospy.get_param("~status_thread_rate") - + self.prediction_per_pixel = rospy.get_param("~prediction_per_pixel") # Initialize traversability estimator parameters # Experiment file exp_file = rospy.get_param("~exp") @@ -222,11 +235,17 @@ def image_callback(self, image_msg: Image, cam: str): # info_msg: CameraInfo return_centers=False, return_dense_features=True, n_random_pixels=100, - fast_random=True, ) + if self.prediction_per_pixel: + # Evaluate traversability + data = Data(x=dense_feat[0].permute(1, 2, 0).reshape(-1, dense_feat.shape[1])) + else: + input_feat = dense_feat[0].permute(1, 2, 0).reshape(-1, dense_feat.shape[1]) + input_feat = feat[seg.reshape(-1)] + data = Data(x=input_feat) + # Evaluate traversability - data = Data(x=dense_feat[0].permute(1, 2, 0).reshape(-1, dense_feat.shape[1])) prediction = self.model.forward(data) out_trav = prediction.reshape(H, W, -1)[:, :, 0] diff --git a/wild_visual_navigation_ros/scripts/wvn_learning_node.py b/wild_visual_navigation_ros/scripts/wvn_learning_node.py index f35bc114..4a63f9e7 100644 --- a/wild_visual_navigation_ros/scripts/wvn_learning_node.py +++ b/wild_visual_navigation_ros/scripts/wvn_learning_node.py @@ -814,5 +814,15 @@ def visualize_mission_graph(self): if __name__ == "__main__": node_name = "wvn_learning_node" rospy.init_node(node_name) + if True: + import rospkg + + 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 wvn_learning_node") + os.system( + f"rosparam load {wvn_path}/config/wild_visual_navigation/inputs/alphasense_compressed.yaml wvn_learning_node" + ) + wvn = WvnLearning() rospy.spin() From 42055b627f2385ac0e85ced5dfadd736c8f948fb Mon Sep 17 00:00:00 2001 From: Jonas Frey Date: Wed, 16 Aug 2023 17:57:16 +0200 Subject: [PATCH 04/43] integrated anomaly detection --- .../cfg/experiment_params.py | 20 +- .../learning/model/__init__.py | 1 + .../learning/model/linear_rnvp.py | 283 ++++++++++++++++++ .../learning/utils/__init__.py | 2 +- wild_visual_navigation/learning/utils/loss.py | 39 +++ .../traversability_estimator/nodes.py | 14 - .../traversability_estimator.py | 191 ++---------- .../wild_visual_navigation/default.yaml | 9 +- .../scripts/wild_visual_navigation_node.py | 9 - .../scripts/wvn_feature_extractor_node.py | 61 ++-- .../scripts/wvn_learning_node.py | 32 +- 11 files changed, 421 insertions(+), 240 deletions(-) create mode 100644 wild_visual_navigation/learning/model/linear_rnvp.py diff --git a/wild_visual_navigation/cfg/experiment_params.py b/wild_visual_navigation/cfg/experiment_params.py index f786e995..d9c6e5c7 100644 --- a/wild_visual_navigation/cfg/experiment_params.py +++ b/wild_visual_navigation/cfg/experiment_params.py @@ -43,11 +43,18 @@ class LossParams: w_reco: float = 0.5 w_temp: float = 0.0 # 0.75 method: str = "latest_measurment" - confidence_std_factor: float = 1.0 + confidence_std_factor: float = 0.5 trav_cross_entropy: bool = False loss: LossParams = LossParams() + @dataclass + class LossAnomalyParams: + method: str = "latest_measurment" + confidence_std_factor: float = 0.5 + + loss_anomaly: LossAnomalyParams = LossAnomalyParams() + @dataclass class TrainerParams: precision: int = 32 @@ -110,6 +117,17 @@ class SimpleGcnCfgParams: simple_gcn_cfg: SimpleGcnCfgParams = SimpleGcnCfgParams() + @dataclass + class LinearRnvpCfgParams: + input_dim: int = 90 + coupling_topology: List[int] = field(default_factory=lambda: [200]) + mask_type: str = "odds" + conditioning_size: int = 0 + use_permutation: bool = True + single_function: bool = True + + linear_rnvp_cfg: LinearRnvpCfgParams = LinearRnvpCfgParams() + model: ModelParams = ModelParams() @dataclass diff --git a/wild_visual_navigation/learning/model/__init__.py b/wild_visual_navigation/learning/model/__init__.py index 5fc47920..cdceb237 100644 --- a/wild_visual_navigation/learning/model/__init__.py +++ b/wild_visual_navigation/learning/model/__init__.py @@ -1,3 +1,4 @@ from .simple_mlp import SimpleMLP, DoubleMLP from .simple_gcn import SimpleGCN +from .linear_rnvp import LinearRnvp from .network_register import get_model diff --git a/wild_visual_navigation/learning/model/linear_rnvp.py b/wild_visual_navigation/learning/model/linear_rnvp.py new file mode 100644 index 00000000..89125307 --- /dev/null +++ b/wild_visual_navigation/learning/model/linear_rnvp.py @@ -0,0 +1,283 @@ +import copy +import itertools +from abc import abstractmethod, ABC +import torch +from torch import nn, distributions +import numpy as np +from torch_geometric.data import Data + + +class LinearBatchNorm(nn.Module): + """ + An (invertible) batch normalization layer. + This class is mostly inspired from this one: + https://github.com/kamenbliznashki/normalizing_flows/blob/master/maf.py + """ + + def __init__(self, input_size, momentum=0.9, eps=1e-5): + super().__init__() + self.momentum = momentum + self.eps = eps + + self.log_gamma = nn.Parameter(torch.zeros(input_size)) + self.beta = nn.Parameter(torch.zeros(input_size)) + + self.register_buffer("running_mean", torch.zeros(input_size)) + self.register_buffer("running_var", torch.ones(input_size)) + + def forward(self, x, **kwargs): + if self.training: + self.batch_mean = x.mean(0) + self.batch_var = x.var(0) + + self.running_mean.mul_(self.momentum).add_(self.batch_mean.data * (1 - self.momentum)) + self.running_var.mul_(self.momentum).add_(self.batch_var.data * (1 - self.momentum)) + + mean = self.batch_mean + var = self.batch_var + else: + mean = self.running_mean + var = self.running_var + + x_hat = (x - mean) / torch.sqrt(var + self.eps) + y = self.log_gamma.exp() * x_hat + self.beta + + log_det = self.log_gamma - 0.5 * torch.log(var + self.eps) + + return y, log_det.expand_as(x).sum(1) + + def backward(self, x, **kwargs): + if self.training: + mean = self.batch_mean + var = self.batch_var + else: + mean = self.running_mean + var = self.running_var + + x_hat = (x - self.beta) * torch.exp(-self.log_gamma) + x = x_hat * torch.sqrt(var + self.eps) + mean + + log_det = 0.5 * torch.log(var + self.eps) - self.log_gamma + + return x, log_det.expand_as(x).sum(1) + + +class LinearCouplingLayer(nn.Module): + """ + Linear coupling layer. + (i) Split the input x into 2 parts x1 and x2 according to a given mask. + (ii) Compute s(x2) and t(x2) with given neural network. + (iii) Final output is [exp(s(x2))*x1 + t(x2); x2]. + The inverse is trivially [(x1 - t(x2))*exp(-s(x2)); x2]. + """ + + def __init__(self, input_dim, mask, network_topology, conditioning_size=None, single_function=True): + super().__init__() + + if conditioning_size is None: + conditioning_size = 0 + + if network_topology is None or len(network_topology) == 0: + network_topology = [input_dim] + + self.register_buffer("mask", mask) + + self.dim = input_dim + + self.s = [nn.Linear(input_dim + conditioning_size, network_topology[0]), nn.ReLU()] + + for i in range(len(network_topology)): + t = network_topology[i] + t_p = network_topology[i - 1] + self.s.extend([nn.Linear(t_p, t), nn.ReLU()]) + + if single_function: + input_dim = input_dim * 2 + + ll = nn.Linear(network_topology[-1], input_dim) + + self.s.append(ll) + self.s = nn.Sequential(*self.s) + + if single_function: + self.st = lambda x: (self.s(x).chunk(2, 1)) + else: + self.t = copy.deepcopy(self.s) + self.st = lambda x: (self.s(x), self.t(x)) + + def backward(self, x, y=None): + mx = x * self.mask + + if y is not None: + _mx = torch.cat([y, mx], dim=1) + else: + _mx = mx + + s, t = self.st(_mx) + s = torch.tanh(s) + + u = mx + (1 - self.mask) * (x - t) * torch.exp(-s) + + log_abs_det_jacobian = -(1 - self.mask) * s + + return u, log_abs_det_jacobian.sum(1) + + def forward(self, u, y=None): + mu = u * self.mask + + if y is not None: + _mu = torch.cat([y, mu], dim=1) + else: + _mu = mu + + s, t = self.st(_mu) + s = torch.tanh(s) + + x = mu + (1 - self.mask) * (u * s.exp() + t) + + log_abs_det_jacobian = (1 - self.mask) * s + + return x, log_abs_det_jacobian.sum(1) + + +class Permutation(nn.Module): + """ + A permutation layer. + """ + + def __init__(self, in_ch): + super().__init__() + self.in_ch = in_ch + self.register_buffer("p", torch.randperm(in_ch)) + self.register_buffer("invp", torch.argsort(self.p)) + + def forward(self, x, y=None): + assert x.shape[1] == self.in_ch + out = x[:, self.p] + return out, 0 + + def backward(self, x, y=None): + assert x.shape[1] == self.in_ch + out = x[:, self.invp] + return out, 0 + + +class SequentialFlow(nn.Sequential): + """ + Utility class to build a normalizing flow from a sequence of base transformations. + During forward and inverse steps, aggregates the sum of the log determinants of the Jacobians. + """ + + def forward(self, x, y=None): + log_det = 0 + for module in self: + x, _log_det = module(x, y=y) + log_det = log_det + _log_det + return x, log_det + + def backward(self, u, y=None): + log_det = 0 + for module in reversed(self): + u, _log_det = module.backward(u, y=y) + log_det = log_det + _log_det + return u, log_det + + def forward_steps(self, x, y=None): + log_det = 0 + xs = [x] + for module in self: + x, _log_det = module(x, y=y) + xs.append(x) + log_det = log_det + _log_det + return xs, log_det + + def backward_steps(self, u, y=None): + log_det = 0 + us = [u] + for module in reversed(self): + u, _log_det = module.backward(u, y=y) + us.append(u) + log_det = log_det + _log_det + return us, log_det + + +class LinearRnvp(nn.Module): + """ + Main RNVP model, alternating affine coupling layers + with permutations and/or batch normalization steps. + """ + + def __init__( + self, + input_dim, + coupling_topology, + flow_n=2, + use_permutation=False, + batch_norm=False, + mask_type="odds", + conditioning_size=None, + single_function=False, + **kwargs + ): + super().__init__() + + self.register_buffer("prior_mean", torch.zeros(input_dim)) + self.register_buffer("prior_var", torch.ones(input_dim)) + + if mask_type == "odds": + mask = torch.arange(0, input_dim).float() % 2 + elif mask_type == "half": + mask = torch.zeros(input_dim) + mask[: input_dim // 2] = 1 + else: + assert False + + if coupling_topology is None: + coupling_topology = [input_dim // 2, input_dim // 2] + + blocks = [] + + for i in range(flow_n): + blocks.append( + LinearCouplingLayer( + input_dim, + mask, + network_topology=coupling_topology, + conditioning_size=conditioning_size, + single_function=single_function, + ) + ) + if use_permutation: + blocks.append(Permutation(input_dim)) + else: + mask = 1 - mask + + if batch_norm: + blocks.append(LinearBatchNorm(input_dim)) + + self.flows = SequentialFlow(*blocks) + + def logprob(self, x): + return self.prior.log_prob(x) + + @property + def prior(self): + return distributions.Normal(self.prior_mean, self.prior_var) + + def forward(self, data: Data): + x = data.x + z, log_det = self.flows.forward(x, None) + return {"z": z, "log_det": log_det, "logprob": self.logprob(z)} + + def backward(self, u, y=None, return_step=False): + if return_step: + return self.flows.backward_steps(u, y) + return self.flows.backward(u, y) + + def sample(self, samples=1, y=None, return_step=False, return_logdet=False): + u = self.prior.sample((samples,)) + z, d = self.backward(u, y=y, return_step=return_step) + if return_logdet: + d = self.logprob(u).sum(1) + d + return z, d + return z diff --git a/wild_visual_navigation/learning/utils/__init__.py b/wild_visual_navigation/learning/utils/__init__.py index 5872fd1d..ba3c8a1e 100644 --- a/wild_visual_navigation/learning/utils/__init__.py +++ b/wild_visual_navigation/learning/utils/__init__.py @@ -3,5 +3,5 @@ from .loading import load_env, load_yaml, file_path from .create_experiment_folder import create_experiment_folder from .get_confidence import get_confidence -from .loss import TraversabilityLoss +from .loss import TraversabilityLoss, AnomalyLoss from .metric_logger import MetricLogger diff --git a/wild_visual_navigation/learning/utils/loss.py b/wild_visual_navigation/learning/utils/loss.py index f68ce8d5..7e79b5ef 100644 --- a/wild_visual_navigation/learning/utils/loss.py +++ b/wild_visual_navigation/learning/utils/loss.py @@ -7,6 +7,41 @@ from torchmetrics import ROC, AUROC, Accuracy +class AnomalyLoss(nn.Module): + def __init__(self, confidence_std_factor, method): + super(AnomalyLoss, self).__init__() + # self._confidence_generator = ConfidenceGenerator( + # std_factor=confidence_std_factor, + # method=method, + # log_enabled=False, + # log_folder="/tmp", + # ) + + def forward( + self, graph: Optional[Data], res: dict, update_generator: bool = True, step: int = 0, log_step: bool = False + ): + loss_aux = {} + loss_aux["loss_trav"] = torch.tensor([0.0]) + loss_aux["loss_reco"] = torch.tensor([0.0]) + loss_aux["confidence"] = torch.tensor([0.0]) + + losses = res["logprob"].sum(1) + res["log_det"] + + std = 400 + mean = 550 + + # Clip the losses + l_clip = torch.clip(losses, mean - 2 * std, mean + 2 * std) + + # Normalize between 0 and 1 + l_norm = (losses - torch.min(l_clip)) / (torch.max(l_clip) - torch.min(l_clip)) + loss_aux["loss_trav"] = -torch.mean(losses) + return -torch.mean(losses), loss_aux, l_norm + + def update_node_confidence(self, node): + node.confidence = 0 + + class TraversabilityLoss(nn.Module): def __init__( self, @@ -104,3 +139,7 @@ def forward( }, res_updated, ) + + def update_node_confidence(self, node): + reco_loss = F.mse_loss(node.prediction[:, 1:], node.features, reduction="none").mean(dim=1) + node.confidence = self._confidence_generator.inference_without_update(reco_loss) diff --git a/wild_visual_navigation/traversability_estimator/nodes.py b/wild_visual_navigation/traversability_estimator/nodes.py index 2addb156..90849c0c 100644 --- a/wild_visual_navigation/traversability_estimator/nodes.py +++ b/wild_visual_navigation/traversability_estimator/nodes.py @@ -114,7 +114,6 @@ def __init__( pose_cam_in_world: torch.tensor = None, image: torch.tensor = None, image_projector: ImageProjector = None, - correspondence=None, camera_name="cam", use_for_training=True, ): @@ -139,7 +138,6 @@ def __init__( self._supervision_mask = None self._supervision_signal = None self._supervision_signal_valid = None - self._correspondence = correspondence self._confidence = None def clear_debug_data(self): @@ -181,8 +179,6 @@ def change_device(self, device): self._supervision_signal = self._supervision_signal.to(device) if self._supervision_signal_valid is not None: self._supervision_signal_valid = self._supervision_signal_valid.to(device) - if self._correspondence is not None: - self._correspondence = self._correspondence.to(device) if self._confidence is not None: self._confidence = self._confidence.to(device) @@ -204,7 +200,6 @@ def as_pyg_data(self, previous_node: Optional[BaseNode] = None, aux: bool = Fals y_valid=self._supervision_signal_valid, x_previous=previous_node.features, edge_index_previous=previous_node._feature_edges, - correspondence=self._correspondence, ) def is_valid(self): @@ -212,7 +207,6 @@ def is_valid(self): valid_members = ( isinstance(self._features, torch.Tensor) and isinstance(self._supervision_signal, torch.Tensor) - and isinstance(self._correspondence, torch.Tensor) and isinstance(self._supervision_signal_valid, torch.Tensor) ) valid_signals = self._supervision_signal_valid.any() if valid_members else False @@ -227,10 +221,6 @@ def camera_name(self): def confidence(self): return self._confidence - @property - def correspondence(self): - return self._correspondence - @property def features(self): return self._features @@ -291,10 +281,6 @@ def camera_name(self, camera_name): def confidence(self, confidence): self._confidence = confidence - @correspondence.setter - def correspondence(self, correspondence): - self._correspondence = correspondence - @features.setter def features(self, features): self._features = features diff --git a/wild_visual_navigation/traversability_estimator/traversability_estimator.py b/wild_visual_navigation/traversability_estimator/traversability_estimator.py index 8ba55be6..edb03a3d 100644 --- a/wild_visual_navigation/traversability_estimator/traversability_estimator.py +++ b/wild_visual_navigation/traversability_estimator/traversability_estimator.py @@ -11,7 +11,7 @@ MaxElementsGraph, ) from wild_visual_navigation.utils import WVNMode -from wild_visual_navigation.learning.utils import TraversabilityLoss +from wild_visual_navigation.learning.utils import TraversabilityLoss, AnomalyLoss from wild_visual_navigation.utils import make_polygon_from_points from wild_visual_navigation.visu import LearningVisualizer from wild_visual_navigation.utils import KLTTracker, KLTTrackerOpenCV @@ -45,7 +45,6 @@ def __init__( proprio_distance_thr: float = None, segmentation_type: str = "slic", feature_type: str = "dino", - optical_flow_estimator_type: str = "none", min_samples_for_training: int = 10, vis_node_index: int = 10, mode: bool = False, @@ -89,11 +88,6 @@ def __init__( input_size=image_size, **kwargs, ) - # Optical flow - self._optical_flow_estimator_type = optical_flow_estimator_type - - if optical_flow_estimator_type == "sparse": - self._optical_flow_estimator = KLTTrackerOpenCV(device=self._device) # Mutex self._learning_lock = Lock() @@ -112,13 +106,18 @@ def __init__( self._model = get_model(self._exp_cfg["model"]).to(self._device) self._model.train() - self._traversability_loss = TraversabilityLoss( - **self._exp_cfg["loss"], - model=self._model, - log_enabled=self._exp_cfg["general"]["log_confidence"], - log_folder=self._exp_cfg["general"]["model_path"], - ) - self._traversability_loss.to(self._device) + if self._exp_cfg["model"]["name"] == "LinearRnvp": + self._traversability_loss = AnomalyLoss(**self._exp_cfg["loss_anomaly"]) + self._traversability_loss.to(self._device) + + else: + self._traversability_loss = TraversabilityLoss( + **self._exp_cfg["loss"], + model=self._model, + log_enabled=self._exp_cfg["general"]["log_confidence"], + log_folder=self._exp_cfg["general"]["model_path"], + ) + self._traversability_loss.to(self._device) self._optimizer = torch.optim.Adam(self._model.parameters(), lr=self._exp_cfg["optimizer"]["lr"]) self._loss = torch.tensor([torch.inf]) @@ -211,8 +210,6 @@ def change_device(self, device: str): self._mission_graph.change_device(device) self._feature_extractor.change_device(device) self._model = self._model.to(device) - if self._optical_flow_estimator_type != "none": - self._optical_flow_estimator = self._optical_flow_estimator.to(device) if self._scale_traversability: # Use 500 bins for constant memory usuage @@ -243,8 +240,8 @@ def update_prediction(self, node: MissionNode): with torch.inference_mode(): with self._learning_lock: node.prediction = self._model(data) - reco_loss = F.mse_loss(node.prediction[:, 1:], node.features, reduction="none").mean(dim=1) - node.confidence = self._traversability_loss._confidence_generator.inference_without_update(reco_loss) + # TODO Check where node confidence is actually used + self._traversability_loss.update_node_confidence(node) @accumulate_time def update_visualization_node(self): @@ -258,116 +255,6 @@ def update_visualization_node(self): self._vis_mission_node = self._mission_graph.get_nodes()[-self._vis_node_index] - @accumulate_time - def add_correspondences_dense_optical_flow(self, node: MissionNode, previous_node: MissionNode, debug=False): - flow_previous_to_current = self._optical_flow_estimator.forward(previous_node.image.clone(), node.image.clone()) - grid_x, grid_y = torch.meshgrid( - torch.arange(0, previous_node.image.shape[1], device=self._device, dtype=torch.float32), - torch.arange(0, previous_node.image.shape[2], device=self._device, dtype=torch.float32), - indexing="ij", - ) - positions = torch.stack([grid_x, grid_y]) - positions += flow_previous_to_current - positions = positions.type(torch.long) - start_seg = torch.unique(previous_node.feature_segments) - - previous = [] - current = [] - for el in start_seg: - m = previous_node.feature_segments == el - candidates = positions[:, m] - m = ( - (candidates[0, :] >= 0) - * (candidates[0, :] < m.shape[0]) - * (candidates[1, :] >= 0) - * (candidates[1, :] < m.shape[1]) - ) - if m.sum() == 0: - continue - candidates = candidates[:, m] - res = node.feature_segments[candidates[0, :], candidates[1, :]] - previous.append(el) - current.append(torch.unique(res, sorted=True)[0]) - - if len(current) != 0: - current = torch.stack(current) - previous = torch.stack(previous) - correspondence = torch.stack([previous, current], dim=1) - node.correspondence = correspondence - - if debug: - from wild_visual_navigation import WVN_ROOT_DIR - from wild_visual_navigation.visu import LearningVisualizer - - visu = LearningVisualizer(p_visu=os.path.join(WVN_ROOT_DIR, "results/test_visu"), store=True) - visu.plot_correspondence_segment( - seg_prev=previous_node.feature_segments, - seg_current=node.feature_segments, - img_prev=previous_node.image, - img_current=node.image, - center_prev=previous_node.feature_positions, - center_current=node.feature_positions, - correspondence=node.correspondence, - tag="centers", - ) - - visu.plot_optical_flow( - flow=flow_previous_to_current, img1=previous_node.image, img2=node.image, tag="flow", s=50 - ) - - @accumulate_time - def add_correspondences_sparse_optical_flow(self, node: MissionNode, previous_node: MissionNode, debug=False): - # Transform previous_nodes feature locations into current image using KLT - pre_pos = previous_node.feature_positions - cur_pos = self._optical_flow_estimator( - previous_node.feature_positions[:, 0].clone(), - previous_node.feature_positions[:, 1].clone(), - previous_node.image, - node.image, - ) - cur_pos = torch.stack(cur_pos, dim=1) - # Use forward propagated cluster centers to get segment index of current image - - # Only compute for forward propagated centers that fall onto current image plane - m = ( - (cur_pos[:, 0] >= 0) - * (cur_pos[:, 1] >= 0) - * (cur_pos[:, 0] < node.image.shape[1]) - * (cur_pos[:, 1] < node.image.shape[2]) - ) - - # Can enumerate previous segments and use mask to get segment index - cor_pre = torch.arange(previous_node.feature_positions.shape[0], device=self._device)[m] - # cor_pre = pre[m] - - # Check feature_segmentation mask to index correct segment index - cur_pos = cur_pos[m].type(torch.long) - cor_cur = node.feature_segments[cur_pos[:, 1], cur_pos[:, 0]] - node.correspondence = torch.stack([cor_pre, cor_cur], dim=1) - - if debug: - from wild_visual_navigation import WVN_ROOT_DIR - from wild_visual_navigation.visu import LearningVisualizer - - visu = LearningVisualizer(p_visu=os.path.join(WVN_ROOT_DIR, "results/test_visu"), store=True) - visu.plot_sparse_optical_flow( - pre_pos, - cur_pos, - img1=previous_node.image, - img2=node.image, - tag="flow", - ) - visu.plot_correspondence_segment( - seg_prev=previous_node.feature_segments, - seg_current=node.feature_segments, - img_prev=previous_node.image, - img_current=node.image, - center_prev=previous_node.feature_positions, - center_current=node.feature_positions, - correspondence=node.correspondence, - tag="centers", - ) - @accumulate_time def add_mission_node(self, node: MissionNode, verbose: bool = False, update_features: bool = True): """Adds a node to the mission graph to images and training info @@ -383,9 +270,6 @@ def add_mission_node(self, node: MissionNode, verbose: bool = False, update_feat # Compute image features self.update_features(node) - # Get last added node - previous_node = self._mission_graph.get_last_node() - # Add image node success = self._mission_graph.add_node(node) @@ -397,15 +281,6 @@ def add_mission_node(self, node: MissionNode, verbose: bool = False, update_feat if verbose: print(s) - if self._mode != WVNMode.EXTRACT_LABELS: - # Set optical flow - if self._optical_flow_estimator_type == "dense" and previous_node is not None: - raise Exception("Not working") - self.add_correspondences_dense_optical_flow(node, previous_node, debug=False) - - elif self._optical_flow_estimator_type == "sparse" and previous_node is not None: - self.add_correspondences_sparse_optical_flow(node, previous_node, debug=False) - # Project past footprints on current image supervision_mask = torch.ones(node.image.shape).to(self._device) * torch.nan @@ -653,32 +528,9 @@ def make_batch(self, batch_size: int = 8): batch_size (int): Size of the batch """ - if self._optical_flow_estimator_type != "none": - raise ValueError( - "Currently not supported the auxilary graph implementation is not needed and the features and edge index of previous node should be added to the mission graph directly." - ) - # Sample a batch of nodes and their previous node, for temporal consistency - mission_nodes = self._mission_graph.get_n_random_valid_nodes(n=batch_size) - ls = [ - x.as_pyg_data(self._mission_graph.get_previous_node(x)) - for x in mission_nodes - if x.correspondence is not None - ] - - ls_aux = [self._mission_graph.get_previous_node(x).as_pyg_data(aux=True) for x in mission_nodes] - - # Make sure to only use nodes with valid correspondences - while len(ls) < batch_size: - mn = self._mission_graph.get_n_random_valid_nodes(n=1)[0] - if mn.correspondence is not None: - ls.append(mn.as_pyg_data(self._mission_graph.get_previous_node(mn))) - ls_aux.append(self._mission_graph.get_previous_node(mn).as_pyg_data(aux=True)) - - batch = Batch.from_data_list(ls) - else: - # Just sample N random nodes - mission_nodes = self._mission_graph.get_n_random_valid_nodes(n=batch_size) - batch = Batch.from_data_list([x.as_pyg_data() for x in mission_nodes]) + # Just sample N random nodes + mission_nodes = self._mission_graph.get_n_random_valid_nodes(n=batch_size) + batch = Batch.from_data_list([x.as_pyg_data() for x in mission_nodes]) return batch @@ -703,9 +555,7 @@ def train(self): res = self._model(graph) log_step = (self._step % 20) == 0 - self._loss, loss_aux, res_updated = self._traversability_loss( - graph, res, step=self._step, log_step=log_step - ) + self._loss, loss_aux, trav = self._traversability_loss(graph, res, step=self._step, log_step=log_step) # Keep track of ROC during training for rescaling the loss when publishing if self._scale_traversability: @@ -718,6 +568,7 @@ def train(self): mask_valid = mask_anomaly | mask_proprioceptive self._auxiliary_training_roc(res[mask_valid, 0], graph.y[mask_valid].type(torch.long)) return_dict["scale_traversability_threshold"] = self._scale_traversability_threshold + # Backprop self._optimizer.zero_grad() self._loss.backward() @@ -728,7 +579,7 @@ def train(self): loss_trav = loss_aux["loss_trav"] loss_reco = loss_aux["loss_reco"] print( - f"step: {self._step} | loss: {self._loss:5f} | loss_trav: {loss_trav:5f} | loss_reco: {loss_reco:5f}" + f"step: {self._step} | loss: {self._loss.item():5f} | loss_trav: {loss_trav.item():5f} | loss_reco: {loss_reco.item():5f}" ) # Update steps diff --git a/wild_visual_navigation_ros/config/wild_visual_navigation/default.yaml b/wild_visual_navigation_ros/config/wild_visual_navigation/default.yaml index afc237cf..035bd8a4 100644 --- a/wild_visual_navigation_ros/config/wild_visual_navigation/default.yaml +++ b/wild_visual_navigation_ros/config/wild_visual_navigation/default.yaml @@ -22,17 +22,14 @@ image_graph_dist_thr: 0.2 # meters proprio_graph_dist_thr: 0.1 # meters network_input_image_height: 224 # 448 network_input_image_width: 224 # 448 -segmentation_type: "slic" +segmentation_type: "random" feature_type: "dino" dino_patch_size: 16 # DINO only confidence_std_factor: 4.0 -scale_traversability: True +scale_traversability: False # This parameter needs to be false when using the anomaly detection model scale_traversability_max_fpr: 0.25 min_samples_for_training: 5 -prediction_per_pixel: False - -# Optical flow params -optical_flow_estimator_type: "none" +prediction_per_pixel: True # Supervision Generator untraversable_thr: 0.01 diff --git a/wild_visual_navigation_ros/scripts/wild_visual_navigation_node.py b/wild_visual_navigation_ros/scripts/wild_visual_navigation_node.py index 0d97ac7b..7d5847be 100644 --- a/wild_visual_navigation_ros/scripts/wild_visual_navigation_node.py +++ b/wild_visual_navigation_ros/scripts/wild_visual_navigation_node.py @@ -67,7 +67,6 @@ def __init__(self): max_distance=self.traversability_radius, image_distance_thr=self.image_graph_dist_thr, proprio_distance_thr=self.proprio_graph_dist_thr, - optical_flow_estimator_type=self.optical_flow_estimator_type, min_samples_for_training=self.min_samples_for_training, vis_node_index=self.vis_node_index, mode=self.mode, @@ -254,9 +253,6 @@ def read_params(self): self.robot_max_velocity = rospy.get_param("~robot_max_velocity") self.untraversable_thr = rospy.get_param("~untraversable_thr") - # Optical flow params - self.optical_flow_estimator_type = rospy.get_param("~optical_flow_estimator_type") - # Threads self.image_callback_rate = rospy.get_param("~image_callback_rate") # hertz self.proprio_callback_rate = rospy.get_param("~proprio_callback_rate") # hertz @@ -289,7 +285,6 @@ def read_params(self): elif self.mode == WVNMode.EXTRACT_LABELS: self.image_callback_rate = 3 self.proprio_callback_rate = 4 - self.optical_flow_estimator_type = False self.image_graph_dist_thr = 0.2 self.proprio_graph_dist_thr = 0.1 @@ -319,9 +314,6 @@ def read_params(self): self.step = -1 self.step_time = rospy.get_time() - if self.mode != WVNMode.EXTRACT_LABELS: - assert self.optical_flow_estimator_type == "none", "Optical flow estimator not tested due to changes" - def setup_rosbag_replay(self, tf_listener): self.tf_listener = tf_listener @@ -710,7 +702,6 @@ def image_callback(self, image_msg: Image, info_msg: CameraInfo, camera_options: pose_cam_in_base=pose_cam_in_base, image=torch_image, image_projector=image_projector, - correspondence=torch.zeros((1,)) if self.optical_flow_estimator_type != "sparse" else None, camera_name=camera_options["name"], use_for_training=camera_options["use_for_training"], ) 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 fe06cc3a..87f47a32 100644 --- a/wild_visual_navigation_ros/scripts/wvn_feature_extractor_node.py +++ b/wild_visual_navigation_ros/scripts/wvn_feature_extractor_node.py @@ -8,6 +8,7 @@ import wild_visual_navigation_ros.ros_converter as rc from wild_visual_navigation.learning.model import get_model from wild_visual_navigation.utils import ConfidenceGenerator +from wild_visual_navigation.learning.utils import AnomalyLoss import rospy from sensor_msgs.msg import Image, CameraInfo, CompressedImage @@ -30,6 +31,8 @@ class WvnFeatureExtractor: def __init__(self): # Read params self.read_params() + self.anomaly_detection = self.exp_cfg["model"]["name"] == "LinearRnvp" + self.feature_extractor = FeatureExtractor( self.device, segmentation_type=self.segmentation_type, @@ -42,11 +45,16 @@ def __init__(self): self.model = get_model(self.exp_cfg["model"]).to(self.device) self.model.eval() - self.confidence_generator = ConfidenceGenerator( - method=self.exp_cfg["loss"]["method"], std_factor=self.exp_cfg["loss"]["confidence_std_factor"] - ) - self.scale_traversability = True - self.traversability_thershold = 0.5 + if not self.anomaly_detection: + self.confidence_generator = ConfidenceGenerator( + method=self.exp_cfg["loss"]["method"], std_factor=self.exp_cfg["loss"]["confidence_std_factor"] + ) + self.scale_traversability = True + self.traversability_thershold = 0.5 + else: + self.traversability_loss = AnomalyLoss(**self.exp_cfg["loss_anomaly"]) + self.traversability_loss.to(self.device) + self.scale_traversability = False if self.verbose: self.status_thread = Thread(target=self.status_thread_loop, name="status") @@ -194,6 +202,9 @@ def setup_ros(self, setup_fully=True): info_pub = rospy.Publisher(f"/wild_visual_navigation_node/{cam}/camera_info", CameraInfo, queue_size=10) self.camera_handler[cam]["trav_pub"] = trav_pub self.camera_handler[cam]["info_pub"] = info_pub + if self.anomaly_detection and self.camera_topics[cam]["publish_confidence"]: + print(colored("Warning force set public confidence to false", "red")) + self.camera_topics[cam]["publish_confidence"] = False if self.camera_topics[cam]["publish_input_image"]: input_pub = rospy.Publisher(f"/wild_visual_navigation_node/{cam}/image_input", Image, queue_size=10) @@ -247,20 +258,29 @@ def image_callback(self, image_msg: Image, cam: str): # info_msg: CameraInfo # Evaluate traversability prediction = self.model.forward(data) - out_trav = prediction.reshape(H, W, -1)[:, :, 0] - - # Publish traversability - if self.scale_traversability: - # Apply piecewise linear scaling 0->0; threshold->0.5; 1->1 - traversability = out_trav.clone() - m = traversability < self.traversability_thershold - # Scale untraversable - traversability[m] *= 0.5 / self.traversability_thershold - # Scale traversable - traversability[~m] -= self.traversability_thershold - traversability[~m] *= 0.5 / (1 - self.traversability_thershold) - traversability[~m] += 0.5 - traversability = traversability.clip(0, 1) + + if not self.anomaly_detection: + + out_trav = prediction.reshape(H, W, -1)[:, :, 0] + + # Publish traversability + if self.scale_traversability: + # Apply piecewise linear scaling 0->0; threshold->0.5; 1->1 + traversability = out_trav.clone() + m = traversability < self.traversability_thershold + # Scale untraversable + traversability[m] *= 0.5 / self.traversability_thershold + # Scale traversable + traversability[~m] -= self.traversability_thershold + traversability[~m] *= 0.5 / (1 - self.traversability_thershold) + traversability[~m] += 0.5 + traversability = traversability.clip(0, 1) + # TODO Check if this was a bug + out_trav = traversability + else: + loss, loss_aux, trav = self.traversability_loss(None, prediction) + + out_trav = trav.reshape(H, W, -1)[:, :, 0] msg = rc.numpy_to_ros_image(out_trav.cpu().numpy(), "passthrough") msg.header = image_msg.header @@ -319,7 +339,8 @@ def load_model(self): self.i += 1 if self.i % 100 == 0: res = torch.load(f"{WVN_ROOT_DIR}/tmp_state_dict2.pt") - if (self.model.state_dict()["layers.0.weight"] != res["layers.0.weight"]).any(): + k = list(self.model.state_dict().keys())[-1] + if (self.model.state_dict()[k] != res[k]).any(): if self.verbose: self.log_data[f"time_last_model"] = rospy.get_time() self.log_data[f"nr_model_updates"] += 1 diff --git a/wild_visual_navigation_ros/scripts/wvn_learning_node.py b/wild_visual_navigation_ros/scripts/wvn_learning_node.py index 4a63f9e7..e8e44050 100644 --- a/wild_visual_navigation_ros/scripts/wvn_learning_node.py +++ b/wild_visual_navigation_ros/scripts/wvn_learning_node.py @@ -68,7 +68,6 @@ def __init__(self): max_distance=self.traversability_radius, image_distance_thr=self.image_graph_dist_thr, proprio_distance_thr=self.proprio_graph_dist_thr, - optical_flow_estimator_type=self.optical_flow_estimator_type, min_samples_for_training=self.min_samples_for_training, vis_node_index=self.vis_node_index, mode=self.mode, @@ -192,19 +191,21 @@ def learning_thread_loop(self): res = self.traversability_estimator._model.state_dict() # Compute ROC Threshold - if self.traversability_estimator._auxiliary_training_roc._update_count != 0: - try: - fpr, tpr, thresholds = self.traversability_estimator._auxiliary_training_roc.compute() - index = torch.where(fpr > self.scale_traversability_max_fpr)[0][0] - traversability_thershold = thresholds[index] - except: + if self.scale_traversability: + if self.traversability_estimator._auxiliary_training_roc._update_count != 0: + try: + fpr, tpr, thresholds = self.traversability_estimator._auxiliary_training_roc.compute() + index = torch.where(fpr > self.scale_traversability_max_fpr)[0][0] + traversability_thershold = thresholds[index] + except: + traversability_thershold = 0.5 + else: traversability_thershold = 0.5 - else: - traversability_thershold = 0.5 - res["traversability_thershold"] = traversability_thershold - cg = self.traversability_estimator._traversability_loss._confidence_generator - res["confidence_generator"] = cg.get_dict() + res["traversability_thershold"] = traversability_thershold + cg = self.traversability_estimator._traversability_loss._confidence_generator + res["confidence_generator"] = cg.get_dict() + os.system(f"rm {WVN_ROOT_DIR}/tmp_state_dict2.pt") torch.save(res, f"{WVN_ROOT_DIR}/tmp_state_dict2.pt") i += 1 @@ -273,9 +274,6 @@ def read_params(self): self.robot_max_velocity = rospy.get_param("~robot_max_velocity") self.untraversable_thr = rospy.get_param("~untraversable_thr") - # Optical flow params - self.optical_flow_estimator_type = rospy.get_param("~optical_flow_estimator_type") - # Threads self.image_callback_rate = rospy.get_param("~image_callback_rate") # hertz self.proprio_callback_rate = rospy.get_param("~proprio_callback_rate") # hertz @@ -304,7 +302,6 @@ def read_params(self): elif self.mode == WVNMode.EXTRACT_LABELS: self.image_callback_rate = 3 self.proprio_callback_rate = 4 - self.optical_flow_estimator_type = False self.image_graph_dist_thr = 0.2 self.proprio_graph_dist_thr = 0.1 @@ -334,8 +331,6 @@ def read_params(self): self.step = -1 self.step_time = rospy.get_time() - assert self.optical_flow_estimator_type == "none", "Optical flow estimator not tested due to changes" - def setup_ros(self, setup_fully=True): """Main function to setup ROS-related stuff: publishers, subscribers and services""" if setup_fully: @@ -654,7 +649,6 @@ def imagefeat_callback(self, imagefeat_msg: ImageFeatures, info_msg: CameraInfo, pose_cam_in_base=pose_cam_in_base, image=torch.zeros((3, h_small, w_small), device=self.device, dtype=torch.float32), image_projector=image_projector, - correspondence=torch.zeros((1,)), camera_name=camera_options["name"], use_for_training=camera_options["use_for_training"], ) From 018310be744591ebdd3c72d0ba2d7371421ea908 Mon Sep 17 00:00:00 2001 From: RobinSchmid7 Date: Wed, 23 Aug 2023 19:01:17 +0200 Subject: [PATCH 05/43] Add fix to also extract labels with python Bagtransformer --- .../src/wild_visual_navigation_ros/ros_converter.py | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/wild_visual_navigation_ros/src/wild_visual_navigation_ros/ros_converter.py b/wild_visual_navigation_ros/src/wild_visual_navigation_ros/ros_converter.py index d1de3ba4..887ef11e 100644 --- a/wild_visual_navigation_ros/src/wild_visual_navigation_ros/ros_converter.py +++ b/wild_visual_navigation_ros/src/wild_visual_navigation_ros/ros_converter.py @@ -106,7 +106,16 @@ def ros_tf_to_torch(tf_pose, device="cpu"): def ros_image_to_torch(ros_img, desired_encoding="rgb8", device="cpu"): - if isinstance(ros_img, Image): + if type(ros_img).__name__ is "_sensor_msgs__Image": + np_image = CV_BRIDGE.imgmsg_to_cv2(ros_img, desired_encoding=desired_encoding) + + elif type(ros_img).__name__ is "_sensor_msgs__CompressedImage": + np_arr = np.fromstring(ros_img.data, np.uint8) + np_image = cv2.imdecode(np_arr, cv2.IMREAD_COLOR) + if "bgr" in ros_img.format: + np_image = cv2.cvtColor(np_image, cv2.COLOR_BGR2RGB) + + elif isinstance(ros_img, Image): np_image = CV_BRIDGE.imgmsg_to_cv2(ros_img, desired_encoding=desired_encoding) elif isinstance(ros_img, CompressedImage): From 5be5be77bef9eabbd7fa86a8c29eccf4e6e9e60c Mon Sep 17 00:00:00 2001 From: RobinSchmid7 Date: Thu, 24 Aug 2023 12:37:41 +0200 Subject: [PATCH 06/43] Do not use dropout, use 384 dim feat vector, no non-linear projection for DINO --- wild_visual_navigation/feature_extractor/dino_interface.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/wild_visual_navigation/feature_extractor/dino_interface.py b/wild_visual_navigation/feature_extractor/dino_interface.py index 97a8ec4a..163743f2 100644 --- a/wild_visual_navigation/feature_extractor/dino_interface.py +++ b/wild_visual_navigation/feature_extractor/dino_interface.py @@ -18,15 +18,15 @@ def __init__( model_type: str = "vit_small", patch_size: int = 8, ): - self.dim = 90 + self.dim = 384 # 90 self.cfg = DictConfig( { "dino_patch_size": patch_size, "dino_feat_type": "feat", "model_type": model_type, - "projection_type": "nonlinear", + "projection_type": None, # "nonlinear" "pretrained_weights": None, - "dropout": True, + "dropout": False, # True } ) From 70e2a25f5d9e69596ffa7748217ecddb9d09d0d8 Mon Sep 17 00:00:00 2001 From: RobinSchmid7 Date: Thu, 24 Aug 2023 16:15:22 +0200 Subject: [PATCH 07/43] Fixes to loss, traversability estimation --- .../cfg/experiment_params.py | 6 +- wild_visual_navigation/learning/utils/loss.py | 23 ++- .../traversability_estimator.py | 177 +++++++++++++----- .../wild_visual_navigation/default.yaml | 18 +- .../scripts/wvn_feature_extractor_node.py | 47 +++-- .../scripts/wvn_learning_node.py | 17 +- 6 files changed, 206 insertions(+), 82 deletions(-) diff --git a/wild_visual_navigation/cfg/experiment_params.py b/wild_visual_navigation/cfg/experiment_params.py index d9c6e5c7..ee6c17f0 100644 --- a/wild_visual_navigation/cfg/experiment_params.py +++ b/wild_visual_navigation/cfg/experiment_params.py @@ -91,12 +91,12 @@ class AblationDataModuleParams: @dataclass class ModelParams: - name: str = "SimpleMLP" + name: str = "LinearRnvp" # LinearRnvp, SimpleMLP, SimpleGCN, DoubleMLP load_ckpt: Optional[str] = None @dataclass class SimpleMlpCfgParams: - input_size: int = 90 + input_size: int = 384 hidden_sizes: List[int] = field(default_factory=lambda: [256, 32, 1]) reconstruction: bool = True @@ -119,7 +119,7 @@ class SimpleGcnCfgParams: @dataclass class LinearRnvpCfgParams: - input_dim: int = 90 + input_dim: int = 384 coupling_topology: List[int] = field(default_factory=lambda: [200]) mask_type: str = "odds" conditioning_size: int = 0 diff --git a/wild_visual_navigation/learning/utils/loss.py b/wild_visual_navigation/learning/utils/loss.py index 7e79b5ef..079e7403 100644 --- a/wild_visual_navigation/learning/utils/loss.py +++ b/wild_visual_navigation/learning/utils/loss.py @@ -18,25 +18,30 @@ def __init__(self, confidence_std_factor, method): # ) def forward( - self, graph: Optional[Data], res: dict, update_generator: bool = True, step: int = 0, log_step: bool = False + self, graph: Optional[Data], res: dict, loss_mean: int = None, loss_std: int = None, train: bool = False, update_generator: bool = True, step: int = 0, log_step: bool = False ): loss_aux = {} loss_aux["loss_trav"] = torch.tensor([0.0]) loss_aux["loss_reco"] = torch.tensor([0.0]) loss_aux["confidence"] = torch.tensor([0.0]) - losses = res["logprob"].sum(1) + res["log_det"] + losses = -(res["logprob"].sum(1) + res["log_det"]) - std = 400 - mean = 550 - - # Clip the losses - l_clip = torch.clip(losses, mean - 2 * std, mean + 2 * std) + # print(torch.mean(losses)) + l_clip = losses + if loss_mean is not None and loss_std is not None: + # Clip the losses + l_clip = torch.clip(losses, loss_mean - 2 * loss_std, loss_mean + 2 * loss_std) # Normalize between 0 and 1 l_norm = (losses - torch.min(l_clip)) / (torch.max(l_clip) - torch.min(l_clip)) - loss_aux["loss_trav"] = -torch.mean(losses) - return -torch.mean(losses), loss_aux, l_norm + l_trav = 1 - l_norm + + if train: + loss_aux["loss_mean"] = torch.median(losses) + loss_aux["loss_std"] = torch.std(losses) + + return torch.mean(losses), loss_aux, l_trav def update_node_confidence(self, node): node.confidence = 0 diff --git a/wild_visual_navigation/traversability_estimator/traversability_estimator.py b/wild_visual_navigation/traversability_estimator/traversability_estimator.py index edb03a3d..84fa7873 100644 --- a/wild_visual_navigation/traversability_estimator/traversability_estimator.py +++ b/wild_visual_navigation/traversability_estimator/traversability_estimator.py @@ -29,6 +29,12 @@ import torch.nn.functional as F import torchvision.transforms as transforms from torchmetrics import ROC +import numpy as np +import cv2 +import rospy +import random +from sensor_msgs.msg import Image +from cv_bridge import CvBridge to_tensor = transforms.ToTensor() @@ -49,6 +55,8 @@ def __init__( vis_node_index: int = 10, mode: bool = False, extraction_store_folder=None, + anomaly_detection: bool = False, + vis_training_samples: bool = False, **kwargs, ): self._device = device @@ -59,6 +67,8 @@ def __init__( self._scale_traversability = scale_traversability self._params = params self._scale_traversability_threshold = 0 + self._anomaly_detection = anomaly_detection + self._vis_training_samples = vis_training_samples if self._scale_traversability: # Use 500 bins for constant memory usuage @@ -99,6 +109,12 @@ def __init__( # Visualization self._visualizer = LearningVisualizer() + if self._vis_training_samples: + self._last_image_mask_pub = rospy.Publisher( + f"/wild_visual_navigation_node/last_node_image_mask", Image, queue_size=1 + ) + self._bridge = CvBridge() + # Lightning module seed_everything(42) @@ -390,6 +406,11 @@ def add_proprio_node(self, pnode: ProprioceptionNode): store = torch.nan_to_num(mnode.supervision_mask.nanmean(axis=0)) != 0 torch.save(store, p) + # if self._anomaly_detection: + # # Visualize supervision mask + # store = torch.nan_to_num(mnode.supervision_mask.nanmean(axis=0)) != 0 + # self._last_image_mask_pub.publish(self._bridge.cv2_to_imgmsg(store.cpu().numpy().astype(np.uint8) * 255, "mono8")) + return True def get_mission_nodes(self): @@ -521,16 +542,69 @@ def load_checkpoint(self, checkpoint_path: str): self._pause_training = False @accumulate_time - def make_batch(self, batch_size: int = 8): + def make_batch(self, batch_size: int = 8, anomaly_detection: bool = False, n_features: int = 200, vis_training_samples: bool = False): """Samples a batch from the mission_graph Args: batch_size (int): Size of the batch """ - # Just sample N random nodes - mission_nodes = self._mission_graph.get_n_random_valid_nodes(n=batch_size) - batch = Batch.from_data_list([x.as_pyg_data() for x in mission_nodes]) + if not anomaly_detection: + # Just sample N random nodes + mission_nodes = self._mission_graph.get_n_random_valid_nodes(n=batch_size) + batch = Batch.from_data_list([x.as_pyg_data() for x in mission_nodes]) + + else: + # TODO: how does the samplig make the most sense + # last_mission_node = self._mission_graph.get_last_node() + # + # if last_mission_node is None: + # return None + # if (not hasattr(last_mission_node, "supervision_mask")) or (last_mission_node.supervision_mask is None): + # return None + # + # mission_nodes = self._mission_graph.get_nodes_within_radius_range( + # last_mission_node, 0, self._proprio_graph.max_distance + # ) + + mission_nodes = self._mission_graph.get_n_random_valid_nodes(n=batch_size) + + p_feat = [] + + for i, mnode in enumerate(mission_nodes): + feat = mnode.features + seg = mnode.feature_segments + mask = mnode.supervision_mask + + mask = torch.nan_to_num(mask.nanmean(axis=0)) != 0 + + # Visualize supervision mask + if vis_training_samples: + self._last_image_mask_pub.publish(self._bridge.cv2_to_imgmsg(mask.cpu().numpy().astype(np.uint8) * 255, "mono8")) + + # Save mask as numpy with opencv + # cv2.imwrite(os.path.join("/home/rschmid/ext", "mask", f"{rospy.get_time()}.png"), mask.cpu().numpy().astype(np.uint8) * 255) + + for s in torch.unique(seg): + m = mask[seg == s] + pos = m.sum() # Inside + neg = (~m).sum() # Outside + + # Check if more inside features than outside features + if pos > neg: + # Check if the vector contains any NaN values + if not np.isnan(feat[s.item()].cpu()).any(): + p_feat.append(feat[s.item()]) + + # Only sample up to 200 features + if len(p_feat) > n_features: + random.shuffle(p_feat) + p_feat = p_feat[:n_features] + break + + p_feat = torch.stack(p_feat, dim=1).T + + batch = Data(x=p_feat) return batch @@ -548,49 +622,60 @@ def train(self): return_dict = {"mission_graph_num_valid_node": num_valid_nodes} if num_valid_nodes > self._min_samples_for_training: # Prepare new batch - graph = self.make_batch(self._exp_cfg["ablation_data_module"]["batch_size"]) + graph = self.make_batch(self._exp_cfg["ablation_data_module"]["batch_size"], anomaly_detection=self._anomaly_detection, vis_training_samples=self._vis_training_samples) + if graph is not None: + + self._loss_mean = None + self._loss_std = None + + with self._learning_lock: + # Forward pass + + if self._anomaly_detection: + res = self._model(graph) + else: + res = self._model(graph) + + log_step = (self._step % 20) == 0 + self._loss, loss_aux, trav = self._traversability_loss(graph, res, step=self._step, log_step=log_step, loss_mean=self._loss_mean, loss_std=self._loss_std, train=True) + + self._loss_mean = loss_aux["loss_mean"] + self._loss_std = loss_aux["loss_std"] + + # Keep track of ROC during training for rescaling the loss when publishing + if self._scale_traversability: + # This mask should contain all the segments corrosponding to trees. + mask_anomaly = loss_aux["confidence"] < 0.5 + mask_proprioceptive = graph.y == 1 + # Remove the segments that are for sure not an anomalies given that we have walked on them. + mask_anomaly[mask_proprioceptive] = False + # Elements are valid if they are either an anomaly or we have walked on them to fit the ROC + mask_valid = mask_anomaly | mask_proprioceptive + self._auxiliary_training_roc(res[mask_valid, 0], graph.y[mask_valid].type(torch.long)) + return_dict["scale_traversability_threshold"] = self._scale_traversability_threshold + + # Backprop + self._optimizer.zero_grad() + self._loss.backward() + self._optimizer.step() + + # Print losses + if log_step: + loss_trav = loss_aux["loss_trav"] + loss_reco = loss_aux["loss_reco"] + print( + f"step: {self._step} | loss: {self._loss.item():5f} | loss_trav: {loss_trav.item():5f} | loss_reco: {loss_reco.item():5f}" + ) - with self._learning_lock: - # Forward pass - res = self._model(graph) - - log_step = (self._step % 20) == 0 - self._loss, loss_aux, trav = self._traversability_loss(graph, res, step=self._step, log_step=log_step) - - # Keep track of ROC during training for rescaling the loss when publishing - if self._scale_traversability: - # This mask should contain all the segments corrosponding to trees. - mask_anomaly = loss_aux["confidence"] < 0.5 - mask_proprioceptive = graph.y == 1 - # Remove the segments that are for sure not an anomalies given that we have walked on them. - mask_anomaly[mask_proprioceptive] = False - # Elements are valid if they are either an anomaly or we have walked on them to fit the ROC - mask_valid = mask_anomaly | mask_proprioceptive - self._auxiliary_training_roc(res[mask_valid, 0], graph.y[mask_valid].type(torch.long)) - return_dict["scale_traversability_threshold"] = self._scale_traversability_threshold - - # Backprop - self._optimizer.zero_grad() - self._loss.backward() - self._optimizer.step() - - # Print losses - if log_step: - loss_trav = loss_aux["loss_trav"] - loss_reco = loss_aux["loss_reco"] - print( - f"step: {self._step} | loss: {self._loss.item():5f} | loss_trav: {loss_trav.item():5f} | loss_reco: {loss_reco.item():5f}" - ) - - # Update steps - self._step += 1 - - # Return loss - return_dict["loss_total"] = self._loss.item() - return_dict["loss_trav"] = loss_aux["loss_trav"].item() - return_dict["loss_reco"] = loss_aux["loss_reco"].item() - - return return_dict + # Update steps + self._step += 1 + + # Return loss + return_dict["loss_total"] = self._loss.item() + return_dict["loss_trav"] = loss_aux["loss_trav"].item() + return_dict["loss_reco"] = loss_aux["loss_reco"].item() + + return return_dict return_dict["loss_total"] = -1 return return_dict diff --git a/wild_visual_navigation_ros/config/wild_visual_navigation/default.yaml b/wild_visual_navigation_ros/config/wild_visual_navigation/default.yaml index 035bd8a4..b604f4c2 100644 --- a/wild_visual_navigation_ros/config/wild_visual_navigation/default.yaml +++ b/wild_visual_navigation_ros/config/wild_visual_navigation/default.yaml @@ -17,19 +17,24 @@ robot_height: 0.3 robot_max_velocity: 0.8 # Traversability estimation params -traversability_radius: 5.0 # meters +traversability_radius: 8.0 # meters image_graph_dist_thr: 0.2 # meters proprio_graph_dist_thr: 0.1 # meters -network_input_image_height: 224 # 448 -network_input_image_width: 224 # 448 -segmentation_type: "random" +network_input_image_height: 448 # 448 +network_input_image_width: 448 # 448 +segmentation_type: "slic" feature_type: "dino" -dino_patch_size: 16 # DINO only +dino_patch_size: 8 # DINO only, 16 +slic_num_components: 500 confidence_std_factor: 4.0 scale_traversability: False # This parameter needs to be false when using the anomaly detection model scale_traversability_max_fpr: 0.25 min_samples_for_training: 5 -prediction_per_pixel: True +prediction_per_pixel: False +traversability_threshold: 0.55 +clip_to_binary: False +anomaly_detection: True +vis_training_samples: True # Supervision Generator untraversable_thr: 0.01 @@ -55,6 +60,7 @@ log_time: false log_confidence: false verbose: true debug_supervision_node_index_from_last: 10 +use_debug_for_desired: false extraction_store_folder: "nan" exp: "nan" 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 87f47a32..0b2d4526 100644 --- a/wild_visual_navigation_ros/scripts/wvn_feature_extractor_node.py +++ b/wild_visual_navigation_ros/scripts/wvn_feature_extractor_node.py @@ -38,6 +38,7 @@ def __init__(self): segmentation_type=self.segmentation_type, feature_type=self.feature_type, input_size=self.network_input_image_height, + slic_num_components=self.slic_num_components, ) self.i = 0 self.setup_ros() @@ -50,7 +51,7 @@ def __init__(self): method=self.exp_cfg["loss"]["method"], std_factor=self.exp_cfg["loss"]["confidence_std_factor"] ) self.scale_traversability = True - self.traversability_thershold = 0.5 + self.traversability_threshold = 0.5 else: self.traversability_loss = AnomalyLoss(**self.exp_cfg["loss_anomaly"]) self.traversability_loss.to(self.device) @@ -114,6 +115,9 @@ def read_params(self): self.segmentation_type = rospy.get_param("~segmentation_type") self.feature_type = rospy.get_param("~feature_type") self.dino_patch_size = rospy.get_param("~dino_patch_size") + self.slic_num_components = rospy.get_param("~slic_num_components") + self.traversability_threshold = rospy.get_param("~traversability_threshold") + self.clip_to_binary = rospy.get_param("~clip_to_binary") self.confidence_std_factor = rospy.get_param("~confidence_std_factor") self.scale_traversability = rospy.get_param("~scale_traversability") @@ -267,12 +271,12 @@ def image_callback(self, image_msg: Image, cam: str): # info_msg: CameraInfo if self.scale_traversability: # Apply piecewise linear scaling 0->0; threshold->0.5; 1->1 traversability = out_trav.clone() - m = traversability < self.traversability_thershold + m = traversability < self.traversability_threshold # Scale untraversable - traversability[m] *= 0.5 / self.traversability_thershold + traversability[m] *= 0.5 / self.traversability_threshold # Scale traversable - traversability[~m] -= self.traversability_thershold - traversability[~m] *= 0.5 / (1 - self.traversability_thershold) + traversability[~m] -= self.traversability_threshold + traversability[~m] *= 0.5 / (1 - self.traversability_threshold) traversability[~m] += 0.5 traversability = traversability.clip(0, 1) # TODO Check if this was a bug @@ -282,6 +286,11 @@ def image_callback(self, image_msg: Image, cam: str): # info_msg: CameraInfo out_trav = trav.reshape(H, W, -1)[:, :, 0] + # Clip to binary output + if self.clip_to_binary: + out_trav = torch.where(out_trav.squeeze() <= self.traversability_threshold, 0.0, 1.0) + + msg = rc.numpy_to_ros_image(out_trav.cpu().numpy(), "passthrough") msg.header = image_msg.header msg.width = out_trav.shape[0] @@ -340,18 +349,29 @@ def load_model(self): if self.i % 100 == 0: res = torch.load(f"{WVN_ROOT_DIR}/tmp_state_dict2.pt") k = list(self.model.state_dict().keys())[-1] - if (self.model.state_dict()[k] != res[k]).any(): + + if (self.model.state_dict()[k] != res[k]).any(): # TODO: model params are changing if self.verbose: self.log_data[f"time_last_model"] = rospy.get_time() self.log_data[f"nr_model_updates"] += 1 - self.model.load_state_dict(res, strict=False) - self.traversability_thershold = res["traversability_thershold"] - self.confidence_generator_state = res["confidence_generator"] + self.model.load_state_dict(res, strict=False) + # if res["traversability_threshold"] is not None: + # self.traversability_threshold = res["traversability_threshold"] + # if res["confidence_generator"] is not None: + # self.confidence_generator_state = res["confidence_generator"] + + # self.traversability_threshold = 0.5 + + # self.confidence_generator_state = res["confidence_generator"] + + # self.confidence_generator.var = 0 + # self.confidence_generator.mean = 1 + # self.confidence_generator.std = 1 - self.confidence_generator.var = self.confidence_generator_state["var"] - self.confidence_generator.mean = self.confidence_generator_state["mean"] - self.confidence_generator.std = self.confidence_generator_state["std"] + # self.confidence_generator.var = self.confidence_generator_state["var"] + # self.confidence_generator.mean = self.confidence_generator_state["mean"] + # self.confidence_generator.std = self.confidence_generator_state["std"] except Exception as e: if self.verbose: print(f"Model Loading Failed: {e}") @@ -368,8 +388,9 @@ def load_model(self): wvn_path = rospack.get_path("wild_visual_navigation_ros") os.system(f"rosparam load {wvn_path}/config/wild_visual_navigation/default.yaml wvn_feature_extractor_node") os.system( - f"rosparam load {wvn_path}/config/wild_visual_navigation/inputs/alphasense_compressed.yaml wvn_feature_extractor_node" + f"rosparam load {wvn_path}/config/wild_visual_navigation/inputs/wide_angle_front_compressed.yaml wvn_feature_extractor_node" ) + print(f"rosparam load {wvn_path}/config/wild_visual_navigation/inputs/wide_angle_front_compressed.yaml wvn_feature_extractor_node") wvn = WvnFeatureExtractor() 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 e8e44050..e76795f1 100644 --- a/wild_visual_navigation_ros/scripts/wvn_learning_node.py +++ b/wild_visual_navigation_ros/scripts/wvn_learning_node.py @@ -32,6 +32,7 @@ import os import seaborn as sns import torch +import dataclasses import numpy as np from typing import Optional import traceback @@ -51,6 +52,7 @@ def __init__(self): # Read params self.read_params() + self.anomaly_detection = self.exp_cfg["model"]["name"] == "LinearRnvp" # Visualization self.color_palette = sns.color_palette(self.colormap, as_cmap=True) @@ -74,6 +76,8 @@ def __init__(self): extraction_store_folder=self.extraction_store_folder, patch_size=self.dino_patch_size, scale_traversability=self.scale_traversability, + anomaly_detection=self.anomaly_detection, + vis_training_samples=self.vis_training_samples, ) # Initialize traversability generator to process velocity commands @@ -196,13 +200,13 @@ def learning_thread_loop(self): try: fpr, tpr, thresholds = self.traversability_estimator._auxiliary_training_roc.compute() index = torch.where(fpr > self.scale_traversability_max_fpr)[0][0] - traversability_thershold = thresholds[index] + traversability_threshold = thresholds[index] except: - traversability_thershold = 0.5 + traversability_threshold = 0.5 else: - traversability_thershold = 0.5 + traversability_threshold = 0.5 - res["traversability_thershold"] = traversability_thershold + res["traversability_threshold"] = traversability_threshold cg = self.traversability_estimator._traversability_loss._confidence_generator res["confidence_generator"] = cg.get_dict() @@ -294,6 +298,7 @@ def read_params(self): # Select mode: # debug, online, extract_labels self.mode = WVNMode.from_string(rospy.get_param("~mode", "debug")) self.extraction_store_folder = rospy.get_param("~extraction_store_folder") + self.vis_training_samples = rospy.get_param("~vis_training_samples") # Parse operation modes if self.mode == WVNMode.ONLINE: @@ -323,6 +328,8 @@ def read_params(self): exp_override = load_yaml(os.path.join(WVN_ROOT_DIR, "cfg/exp", exp_file)) self.params = override_params(self.params, exp_override) + self.exp_cfg = dataclasses.asdict(self.params) + self.params.general.name = self.mission_name self.params.general.timestamp = self.mission_timestamp self.params.general.log_confidence = self.log_confidence @@ -815,7 +822,7 @@ def visualize_mission_graph(self): wvn_path = rospack.get_path("wild_visual_navigation_ros") os.system(f"rosparam load {wvn_path}/config/wild_visual_navigation/default.yaml wvn_learning_node") os.system( - f"rosparam load {wvn_path}/config/wild_visual_navigation/inputs/alphasense_compressed.yaml wvn_learning_node" + f"rosparam load {wvn_path}/config/wild_visual_navigation/inputs/wide_angle_front_compressed.yaml wvn_learning_node" ) wvn = WvnLearning() From 538524109f3cd27f4d53b829f23dec6c5de62ca0 Mon Sep 17 00:00:00 2001 From: RobinSchmid7 Date: Thu, 24 Aug 2023 16:46:38 +0200 Subject: [PATCH 08/43] Fix camera yaml files --- .../config/wild_visual_navigation/default.yaml | 2 +- .../inputs/realsense_front.yaml | 6 ++++-- .../wild_visual_navigation/inputs/realsense_rear.yaml | 2 ++ .../inputs/wide_angle_front.yaml | 2 ++ .../inputs/wide_angle_front_compressed.yaml | 11 +++++++++++ 5 files changed, 20 insertions(+), 3 deletions(-) create mode 100644 wild_visual_navigation_ros/config/wild_visual_navigation/inputs/wide_angle_front_compressed.yaml diff --git a/wild_visual_navigation_ros/config/wild_visual_navigation/default.yaml b/wild_visual_navigation_ros/config/wild_visual_navigation/default.yaml index b604f4c2..09aabf7e 100644 --- a/wild_visual_navigation_ros/config/wild_visual_navigation/default.yaml +++ b/wild_visual_navigation_ros/config/wild_visual_navigation/default.yaml @@ -17,7 +17,7 @@ robot_height: 0.3 robot_max_velocity: 0.8 # Traversability estimation params -traversability_radius: 8.0 # meters +traversability_radius: 5.0 # meters image_graph_dist_thr: 0.2 # meters proprio_graph_dist_thr: 0.1 # meters network_input_image_height: 448 # 448 diff --git a/wild_visual_navigation_ros/config/wild_visual_navigation/inputs/realsense_front.yaml b/wild_visual_navigation_ros/config/wild_visual_navigation/inputs/realsense_front.yaml index 10d880e5..3bc33ece 100644 --- a/wild_visual_navigation_ros/config/wild_visual_navigation/inputs/realsense_front.yaml +++ b/wild_visual_navigation_ros/config/wild_visual_navigation/inputs/realsense_front.yaml @@ -3,7 +3,9 @@ camera_topics: image_topic: "/depth_camera_front/color/image_raw" info_topic: "/depth_camera_front/color/camera_info" use_for_training: true + publish_confidence: true + publish_input_image: true # Provides 480 (height) x 848 (width) images -network_input_image_height: 224 # 448 -network_input_image_width: 224 # 448 \ No newline at end of file +network_input_image_height: 448 # 448 +network_input_image_width: 448 # 448 \ No newline at end of file diff --git a/wild_visual_navigation_ros/config/wild_visual_navigation/inputs/realsense_rear.yaml b/wild_visual_navigation_ros/config/wild_visual_navigation/inputs/realsense_rear.yaml index 074f5116..35ad15fc 100644 --- a/wild_visual_navigation_ros/config/wild_visual_navigation/inputs/realsense_rear.yaml +++ b/wild_visual_navigation_ros/config/wild_visual_navigation/inputs/realsense_rear.yaml @@ -3,6 +3,8 @@ camera_topics: image_topic: "/depth_camera_rear/color/image_raw" info_topic: "/depth_camera_rear/color/camera_info" use_for_training: true + publish_confidence: true + publish_input_image: true # Provides 480 (height) x 848 (width) images network_input_image_height: 224 # 448 diff --git a/wild_visual_navigation_ros/config/wild_visual_navigation/inputs/wide_angle_front.yaml b/wild_visual_navigation_ros/config/wild_visual_navigation/inputs/wide_angle_front.yaml index 970bcf6d..50b65278 100644 --- a/wild_visual_navigation_ros/config/wild_visual_navigation/inputs/wide_angle_front.yaml +++ b/wild_visual_navigation_ros/config/wild_visual_navigation/inputs/wide_angle_front.yaml @@ -3,6 +3,8 @@ camera_topics: image_topic: "/wide_angle_camera_front/image_color" info_topic: "/wide_angle_camera_front/camera_info" use_for_training: true + publish_confidence: true + publish_input_image: true # Provides 1080 (height) x 1920 (width) images network_input_image_height: 224 # 448 diff --git a/wild_visual_navigation_ros/config/wild_visual_navigation/inputs/wide_angle_front_compressed.yaml b/wild_visual_navigation_ros/config/wild_visual_navigation/inputs/wide_angle_front_compressed.yaml new file mode 100644 index 00000000..df07ccb8 --- /dev/null +++ b/wild_visual_navigation_ros/config/wild_visual_navigation/inputs/wide_angle_front_compressed.yaml @@ -0,0 +1,11 @@ +camera_topics: + front: + image_topic: "/wide_angle_camera_front/image_color/compressed" + info_topic: "/wide_angle_camera_front/camera_info" + use_for_training: true + publish_confidence: true + publish_input_image: true + +# 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 From d33f6109355669308d554f43c1644a8497d28d6b Mon Sep 17 00:00:00 2001 From: RobinSchmid7 Date: Tue, 29 Aug 2023 16:18:05 +0200 Subject: [PATCH 09/43] Added comments, changes to default rnvp config --- wild_visual_navigation/cfg/experiment_params.py | 2 +- .../learning/model/linear_rnvp.py | 15 ++++++++------- wild_visual_navigation/learning/utils/loss.py | 2 +- .../traversability_estimator.py | 5 +---- .../config/wild_visual_navigation/default.yaml | 8 ++++---- .../inputs/wide_angle_front.yaml | 4 ++-- .../inputs/wide_angle_front_compressed.yaml | 4 ++-- .../scripts/wvn_feature_extractor_node.py | 4 ++-- 8 files changed, 21 insertions(+), 23 deletions(-) diff --git a/wild_visual_navigation/cfg/experiment_params.py b/wild_visual_navigation/cfg/experiment_params.py index ee6c17f0..b7d3a0a3 100644 --- a/wild_visual_navigation/cfg/experiment_params.py +++ b/wild_visual_navigation/cfg/experiment_params.py @@ -124,7 +124,7 @@ class LinearRnvpCfgParams: mask_type: str = "odds" conditioning_size: int = 0 use_permutation: bool = True - single_function: bool = True + single_function: bool = False linear_rnvp_cfg: LinearRnvpCfgParams = LinearRnvpCfgParams() diff --git a/wild_visual_navigation/learning/model/linear_rnvp.py b/wild_visual_navigation/learning/model/linear_rnvp.py index 89125307..e2a3e1d4 100644 --- a/wild_visual_navigation/learning/model/linear_rnvp.py +++ b/wild_visual_navigation/learning/model/linear_rnvp.py @@ -114,7 +114,7 @@ def backward(self, x, y=None): _mx = mx s, t = self.st(_mx) - s = torch.tanh(s) + s = torch.tanh(s) # Adding an activation function here with non-linearities u = mx + (1 - self.mask) * (x - t) * torch.exp(-s) @@ -221,8 +221,8 @@ def __init__( ): super().__init__() - self.register_buffer("prior_mean", torch.zeros(input_dim)) - self.register_buffer("prior_var", torch.ones(input_dim)) + self.register_buffer("prior_mean", torch.zeros(input_dim)) # Normal Gaussian with zero mean + self.register_buffer("prior_var", torch.ones(input_dim)) # Normal Gaussian with unit variance if mask_type == "odds": mask = torch.arange(0, input_dim).float() % 2 @@ -258,16 +258,17 @@ def __init__( self.flows = SequentialFlow(*blocks) def logprob(self, x): - return self.prior.log_prob(x) + return self.prior.log_prob(x) # Compute log probability of the input at the Gaussian distribution @property def prior(self): - return distributions.Normal(self.prior_mean, self.prior_var) + return distributions.Normal(self.prior_mean, self.prior_var) # Normal Gaussian with zero mean and unit variance def forward(self, data: Data): x = data.x - z, log_det = self.flows.forward(x, None) - return {"z": z, "log_det": log_det, "logprob": self.logprob(z)} + z, log_det = self.flows.forward(x, y=None) + log_prob = self.logprob(z) + return {"z": z, "log_det": log_det, "logprob": log_prob} def backward(self, u, y=None, return_step=False): if return_step: diff --git a/wild_visual_navigation/learning/utils/loss.py b/wild_visual_navigation/learning/utils/loss.py index 079e7403..cb21905a 100644 --- a/wild_visual_navigation/learning/utils/loss.py +++ b/wild_visual_navigation/learning/utils/loss.py @@ -25,7 +25,7 @@ def forward( loss_aux["loss_reco"] = torch.tensor([0.0]) loss_aux["confidence"] = torch.tensor([0.0]) - losses = -(res["logprob"].sum(1) + res["log_det"]) + losses = -(res["logprob"].sum(1) + res["log_det"]) # Sum over all channels, resulting in h*w output dimensions # print(torch.mean(losses)) l_clip = losses diff --git a/wild_visual_navigation/traversability_estimator/traversability_estimator.py b/wild_visual_navigation/traversability_estimator/traversability_estimator.py index 84fa7873..695f5816 100644 --- a/wild_visual_navigation/traversability_estimator/traversability_estimator.py +++ b/wild_visual_navigation/traversability_estimator/traversability_estimator.py @@ -631,10 +631,7 @@ def train(self): with self._learning_lock: # Forward pass - if self._anomaly_detection: - res = self._model(graph) - else: - res = self._model(graph) + res = self._model(graph) log_step = (self._step % 20) == 0 self._loss, loss_aux, trav = self._traversability_loss(graph, res, step=self._step, log_step=log_step, loss_mean=self._loss_mean, loss_std=self._loss_std, train=True) diff --git a/wild_visual_navigation_ros/config/wild_visual_navigation/default.yaml b/wild_visual_navigation_ros/config/wild_visual_navigation/default.yaml index 09aabf7e..cd5d20f8 100644 --- a/wild_visual_navigation_ros/config/wild_visual_navigation/default.yaml +++ b/wild_visual_navigation_ros/config/wild_visual_navigation/default.yaml @@ -30,11 +30,11 @@ confidence_std_factor: 4.0 scale_traversability: False # This parameter needs to be false when using the anomaly detection model scale_traversability_max_fpr: 0.25 min_samples_for_training: 5 -prediction_per_pixel: False +prediction_per_pixel: false traversability_threshold: 0.55 -clip_to_binary: False -anomaly_detection: True -vis_training_samples: True +clip_to_binary: false +anomaly_detection: true +vis_training_samples: true # Supervision Generator untraversable_thr: 0.01 diff --git a/wild_visual_navigation_ros/config/wild_visual_navigation/inputs/wide_angle_front.yaml b/wild_visual_navigation_ros/config/wild_visual_navigation/inputs/wide_angle_front.yaml index 50b65278..41106092 100644 --- a/wild_visual_navigation_ros/config/wild_visual_navigation/inputs/wide_angle_front.yaml +++ b/wild_visual_navigation_ros/config/wild_visual_navigation/inputs/wide_angle_front.yaml @@ -7,5 +7,5 @@ camera_topics: publish_input_image: true # 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 +network_input_image_height: 448 # 448 +network_input_image_width: 448 # 448 \ No newline at end of file diff --git a/wild_visual_navigation_ros/config/wild_visual_navigation/inputs/wide_angle_front_compressed.yaml b/wild_visual_navigation_ros/config/wild_visual_navigation/inputs/wide_angle_front_compressed.yaml index df07ccb8..7afa0a66 100644 --- a/wild_visual_navigation_ros/config/wild_visual_navigation/inputs/wide_angle_front_compressed.yaml +++ b/wild_visual_navigation_ros/config/wild_visual_navigation/inputs/wide_angle_front_compressed.yaml @@ -7,5 +7,5 @@ camera_topics: publish_input_image: true # 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 +network_input_image_height: 448 # 448 +network_input_image_width: 448 # 448 \ No newline at end of file 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 0b2d4526..a06f5352 100644 --- a/wild_visual_navigation_ros/scripts/wvn_feature_extractor_node.py +++ b/wild_visual_navigation_ros/scripts/wvn_feature_extractor_node.py @@ -256,7 +256,7 @@ def image_callback(self, image_msg: Image, cam: str): # info_msg: CameraInfo # Evaluate traversability data = Data(x=dense_feat[0].permute(1, 2, 0).reshape(-1, dense_feat.shape[1])) else: - input_feat = dense_feat[0].permute(1, 2, 0).reshape(-1, dense_feat.shape[1]) + # input_feat = dense_feat[0].permute(1, 2, 0).reshape(-1, dense_feat.shape[1]) input_feat = feat[seg.reshape(-1)] data = Data(x=input_feat) @@ -350,7 +350,7 @@ def load_model(self): res = torch.load(f"{WVN_ROOT_DIR}/tmp_state_dict2.pt") k = list(self.model.state_dict().keys())[-1] - if (self.model.state_dict()[k] != res[k]).any(): # TODO: model params are changing + if (self.model.state_dict()[k] != res[k]).any(): # TODO: model params are changing? if self.verbose: self.log_data[f"time_last_model"] = rospy.get_time() self.log_data[f"nr_model_updates"] += 1 From d2283e28c282b46280402f5e3f4acf31e1c7ae02 Mon Sep 17 00:00:00 2001 From: RobinSchmid7 Date: Tue, 29 Aug 2023 16:49:27 +0200 Subject: [PATCH 10/43] Changed resolution to 224 --- .../feature_extractor/dino_interface.py | 9 ++++++--- .../config/wild_visual_navigation/default.yaml | 4 ++-- .../inputs/wide_angle_front_compressed.yaml | 4 ++-- 3 files changed, 10 insertions(+), 7 deletions(-) diff --git a/wild_visual_navigation/feature_extractor/dino_interface.py b/wild_visual_navigation/feature_extractor/dino_interface.py index 163743f2..130ef53b 100644 --- a/wild_visual_navigation/feature_extractor/dino_interface.py +++ b/wild_visual_navigation/feature_extractor/dino_interface.py @@ -17,16 +17,19 @@ def __init__( input_interp: str = "bilinear", model_type: str = "vit_small", patch_size: int = 8, + dim: int = 384, + projection_type: str = None, # nonlinear or None + dropout: bool = False, # True or False ): - self.dim = 384 # 90 + self.dim = dim # 90 or 384 self.cfg = DictConfig( { "dino_patch_size": patch_size, "dino_feat_type": "feat", "model_type": model_type, - "projection_type": None, # "nonlinear" + "projection_type": projection_type, "pretrained_weights": None, - "dropout": False, # True + "dropout": dropout, } ) diff --git a/wild_visual_navigation_ros/config/wild_visual_navigation/default.yaml b/wild_visual_navigation_ros/config/wild_visual_navigation/default.yaml index cd5d20f8..12bd35a1 100644 --- a/wild_visual_navigation_ros/config/wild_visual_navigation/default.yaml +++ b/wild_visual_navigation_ros/config/wild_visual_navigation/default.yaml @@ -20,8 +20,8 @@ robot_max_velocity: 0.8 traversability_radius: 5.0 # meters image_graph_dist_thr: 0.2 # meters proprio_graph_dist_thr: 0.1 # meters -network_input_image_height: 448 # 448 -network_input_image_width: 448 # 448 +network_input_image_height: 224 # 448 +network_input_image_width: 224 # 448 segmentation_type: "slic" feature_type: "dino" dino_patch_size: 8 # DINO only, 16 diff --git a/wild_visual_navigation_ros/config/wild_visual_navigation/inputs/wide_angle_front_compressed.yaml b/wild_visual_navigation_ros/config/wild_visual_navigation/inputs/wide_angle_front_compressed.yaml index 7afa0a66..df07ccb8 100644 --- a/wild_visual_navigation_ros/config/wild_visual_navigation/inputs/wide_angle_front_compressed.yaml +++ b/wild_visual_navigation_ros/config/wild_visual_navigation/inputs/wide_angle_front_compressed.yaml @@ -7,5 +7,5 @@ camera_topics: publish_input_image: true # Provides 1080 (height) x 1920 (width) images -network_input_image_height: 448 # 448 -network_input_image_width: 448 # 448 \ No newline at end of file +network_input_image_height: 224 # 448 +network_input_image_width: 224 # 448 \ No newline at end of file From 12bb9dfcbe113f6d1a564bc4f8cc59d748bf158e Mon Sep 17 00:00:00 2001 From: Jonas Frey Date: Tue, 29 Aug 2023 17:13:48 +0200 Subject: [PATCH 11/43] finalize PR --- .../cfg/experiment_params.py | 2 +- .../feature_extractor/dino_interface.py | 2 +- .../learning/model/linear_rnvp.py | 8 +++--- wild_visual_navigation/learning/utils/loss.py | 12 ++++++-- .../traversability_estimator.py | 28 ++++++++++++++++--- .../launch/robot.launch | 5 ++-- .../launch/wild_visual_navigation.launch | 3 ++ .../scripts/wvn_feature_extractor_node.py | 7 +++-- .../scripts/wvn_learning_node.py | 2 +- .../ros_converter.py | 15 ++++------ 10 files changed, 56 insertions(+), 28 deletions(-) diff --git a/wild_visual_navigation/cfg/experiment_params.py b/wild_visual_navigation/cfg/experiment_params.py index b7d3a0a3..a64d616c 100644 --- a/wild_visual_navigation/cfg/experiment_params.py +++ b/wild_visual_navigation/cfg/experiment_params.py @@ -91,7 +91,7 @@ class AblationDataModuleParams: @dataclass class ModelParams: - name: str = "LinearRnvp" # LinearRnvp, SimpleMLP, SimpleGCN, DoubleMLP + name: str = "LinearRnvp" # LinearRnvp, SimpleMLP, SimpleGCN, DoubleMLP load_ckpt: Optional[str] = None @dataclass diff --git a/wild_visual_navigation/feature_extractor/dino_interface.py b/wild_visual_navigation/feature_extractor/dino_interface.py index 130ef53b..0973af0f 100644 --- a/wild_visual_navigation/feature_extractor/dino_interface.py +++ b/wild_visual_navigation/feature_extractor/dino_interface.py @@ -18,7 +18,7 @@ def __init__( model_type: str = "vit_small", patch_size: int = 8, dim: int = 384, - projection_type: str = None, # nonlinear or None + projection_type: str = None, # nonlinear or None dropout: bool = False, # True or False ): self.dim = dim # 90 or 384 diff --git a/wild_visual_navigation/learning/model/linear_rnvp.py b/wild_visual_navigation/learning/model/linear_rnvp.py index e2a3e1d4..d706b9c6 100644 --- a/wild_visual_navigation/learning/model/linear_rnvp.py +++ b/wild_visual_navigation/learning/model/linear_rnvp.py @@ -114,7 +114,7 @@ def backward(self, x, y=None): _mx = mx s, t = self.st(_mx) - s = torch.tanh(s) # Adding an activation function here with non-linearities + s = torch.tanh(s) # Adding an activation function here with non-linearities u = mx + (1 - self.mask) * (x - t) * torch.exp(-s) @@ -222,7 +222,7 @@ def __init__( super().__init__() self.register_buffer("prior_mean", torch.zeros(input_dim)) # Normal Gaussian with zero mean - self.register_buffer("prior_var", torch.ones(input_dim)) # Normal Gaussian with unit variance + self.register_buffer("prior_var", torch.ones(input_dim)) # Normal Gaussian with unit variance if mask_type == "odds": mask = torch.arange(0, input_dim).float() % 2 @@ -258,11 +258,11 @@ def __init__( self.flows = SequentialFlow(*blocks) def logprob(self, x): - return self.prior.log_prob(x) # Compute log probability of the input at the Gaussian distribution + return self.prior.log_prob(x) # Compute log probability of the input at the Gaussian distribution @property def prior(self): - return distributions.Normal(self.prior_mean, self.prior_var) # Normal Gaussian with zero mean and unit variance + return distributions.Normal(self.prior_mean, self.prior_var) # Normal Gaussian with zero mean and unit variance def forward(self, data: Data): x = data.x diff --git a/wild_visual_navigation/learning/utils/loss.py b/wild_visual_navigation/learning/utils/loss.py index cb21905a..50f2d3b5 100644 --- a/wild_visual_navigation/learning/utils/loss.py +++ b/wild_visual_navigation/learning/utils/loss.py @@ -18,14 +18,22 @@ def __init__(self, confidence_std_factor, method): # ) def forward( - self, graph: Optional[Data], res: dict, loss_mean: int = None, loss_std: int = None, train: bool = False, update_generator: bool = True, step: int = 0, log_step: bool = False + self, + graph: Optional[Data], + res: dict, + loss_mean: int = None, + loss_std: int = None, + train: bool = False, + update_generator: bool = True, + step: int = 0, + log_step: bool = False, ): loss_aux = {} loss_aux["loss_trav"] = torch.tensor([0.0]) loss_aux["loss_reco"] = torch.tensor([0.0]) loss_aux["confidence"] = torch.tensor([0.0]) - losses = -(res["logprob"].sum(1) + res["log_det"]) # Sum over all channels, resulting in h*w output dimensions + losses = -(res["logprob"].sum(1) + res["log_det"]) # Sum over all channels, resulting in h*w output dimensions # print(torch.mean(losses)) l_clip = losses diff --git a/wild_visual_navigation/traversability_estimator/traversability_estimator.py b/wild_visual_navigation/traversability_estimator/traversability_estimator.py index 695f5816..7de385e6 100644 --- a/wild_visual_navigation/traversability_estimator/traversability_estimator.py +++ b/wild_visual_navigation/traversability_estimator/traversability_estimator.py @@ -542,7 +542,13 @@ def load_checkpoint(self, checkpoint_path: str): self._pause_training = False @accumulate_time - def make_batch(self, batch_size: int = 8, anomaly_detection: bool = False, n_features: int = 200, vis_training_samples: bool = False): + def make_batch( + self, + batch_size: int = 8, + anomaly_detection: bool = False, + n_features: int = 200, + vis_training_samples: bool = False, + ): """Samples a batch from the mission_graph Args: @@ -580,7 +586,9 @@ def make_batch(self, batch_size: int = 8, anomaly_detection: bool = False, n_fea # Visualize supervision mask if vis_training_samples: - self._last_image_mask_pub.publish(self._bridge.cv2_to_imgmsg(mask.cpu().numpy().astype(np.uint8) * 255, "mono8")) + self._last_image_mask_pub.publish( + self._bridge.cv2_to_imgmsg(mask.cpu().numpy().astype(np.uint8) * 255, "mono8") + ) # Save mask as numpy with opencv # cv2.imwrite(os.path.join("/home/rschmid/ext", "mask", f"{rospy.get_time()}.png"), mask.cpu().numpy().astype(np.uint8) * 255) @@ -622,7 +630,11 @@ def train(self): return_dict = {"mission_graph_num_valid_node": num_valid_nodes} if num_valid_nodes > self._min_samples_for_training: # Prepare new batch - graph = self.make_batch(self._exp_cfg["ablation_data_module"]["batch_size"], anomaly_detection=self._anomaly_detection, vis_training_samples=self._vis_training_samples) + graph = self.make_batch( + self._exp_cfg["ablation_data_module"]["batch_size"], + anomaly_detection=self._anomaly_detection, + vis_training_samples=self._vis_training_samples, + ) if graph is not None: self._loss_mean = None @@ -634,7 +646,15 @@ def train(self): res = self._model(graph) log_step = (self._step % 20) == 0 - self._loss, loss_aux, trav = self._traversability_loss(graph, res, step=self._step, log_step=log_step, loss_mean=self._loss_mean, loss_std=self._loss_std, train=True) + self._loss, loss_aux, trav = self._traversability_loss( + graph, + res, + step=self._step, + log_step=log_step, + loss_mean=self._loss_mean, + loss_std=self._loss_std, + train=True, + ) self._loss_mean = loss_aux["loss_mean"] self._loss_std = loss_aux["loss_std"] diff --git a/wild_visual_navigation_ros/launch/robot.launch b/wild_visual_navigation_ros/launch/robot.launch index 12997592..86ce0bc9 100644 --- a/wild_visual_navigation_ros/launch/robot.launch +++ b/wild_visual_navigation_ros/launch/robot.launch @@ -9,7 +9,8 @@ - - + + + diff --git a/wild_visual_navigation_ros/launch/wild_visual_navigation.launch b/wild_visual_navigation_ros/launch/wild_visual_navigation.launch index 8913dfbf..1bce2bdd 100644 --- a/wild_visual_navigation_ros/launch/wild_visual_navigation.launch +++ b/wild_visual_navigation_ros/launch/wild_visual_navigation.launch @@ -5,6 +5,7 @@ + @@ -19,10 +20,12 @@ + + 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 a06f5352..86e5f6b9 100644 --- a/wild_visual_navigation_ros/scripts/wvn_feature_extractor_node.py +++ b/wild_visual_navigation_ros/scripts/wvn_feature_extractor_node.py @@ -290,7 +290,6 @@ def image_callback(self, image_msg: Image, cam: str): # info_msg: CameraInfo if self.clip_to_binary: out_trav = torch.where(out_trav.squeeze() <= self.traversability_threshold, 0.0, 1.0) - msg = rc.numpy_to_ros_image(out_trav.cpu().numpy(), "passthrough") msg.header = image_msg.header msg.width = out_trav.shape[0] @@ -381,7 +380,7 @@ def load_model(self): node_name = "wvn_feature_extractor_node" rospy.init_node(node_name) - if True: + if rospy.get_param("~reload_default_params", True): import rospkg rospack = rospkg.RosPack() @@ -390,7 +389,9 @@ def load_model(self): os.system( f"rosparam load {wvn_path}/config/wild_visual_navigation/inputs/wide_angle_front_compressed.yaml wvn_feature_extractor_node" ) - print(f"rosparam load {wvn_path}/config/wild_visual_navigation/inputs/wide_angle_front_compressed.yaml wvn_feature_extractor_node") + print( + f"rosparam load {wvn_path}/config/wild_visual_navigation/inputs/wide_angle_front_compressed.yaml wvn_feature_extractor_node" + ) wvn = WvnFeatureExtractor() 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 e76795f1..f05368c9 100644 --- a/wild_visual_navigation_ros/scripts/wvn_learning_node.py +++ b/wild_visual_navigation_ros/scripts/wvn_learning_node.py @@ -815,7 +815,7 @@ def visualize_mission_graph(self): if __name__ == "__main__": node_name = "wvn_learning_node" rospy.init_node(node_name) - if True: + if rospy.get_param("~reload_default_params", True): import rospkg rospack = rospkg.RosPack() diff --git a/wild_visual_navigation_ros/src/wild_visual_navigation_ros/ros_converter.py b/wild_visual_navigation_ros/src/wild_visual_navigation_ros/ros_converter.py index 887ef11e..63bd4f52 100644 --- a/wild_visual_navigation_ros/src/wild_visual_navigation_ros/ros_converter.py +++ b/wild_visual_navigation_ros/src/wild_visual_navigation_ros/ros_converter.py @@ -106,23 +106,18 @@ def ros_tf_to_torch(tf_pose, device="cpu"): def ros_image_to_torch(ros_img, desired_encoding="rgb8", device="cpu"): - if type(ros_img).__name__ is "_sensor_msgs__Image": + if type(ros_img).__name__ is "_sensor_msgs__Image" or isinstance(ros_img, Image): np_image = CV_BRIDGE.imgmsg_to_cv2(ros_img, desired_encoding=desired_encoding) - elif type(ros_img).__name__ is "_sensor_msgs__CompressedImage": + elif type(ros_img).__name__ is "_sensor_msgs__CompressedImage" or isinstance(ros_img, CompressedImage): np_arr = np.fromstring(ros_img.data, np.uint8) np_image = cv2.imdecode(np_arr, cv2.IMREAD_COLOR) if "bgr" in ros_img.format: np_image = cv2.cvtColor(np_image, cv2.COLOR_BGR2RGB) - elif isinstance(ros_img, Image): - np_image = CV_BRIDGE.imgmsg_to_cv2(ros_img, desired_encoding=desired_encoding) - - elif isinstance(ros_img, CompressedImage): - np_arr = np.fromstring(ros_img.data, np.uint8) - np_image = cv2.imdecode(np_arr, cv2.IMREAD_COLOR) - if "bgr" in ros_img.format: - np_image = cv2.cvtColor(np_image, cv2.COLOR_BGR2RGB) + else: + raise ValueError("Image message type is not implemented.") + return TO_TENSOR(np_image).to(device) From 1cacd24c029a37eef05a163be0362be4e3194fae Mon Sep 17 00:00:00 2001 From: Jonas Frey Date: Tue, 29 Aug 2023 17:48:11 +0200 Subject: [PATCH 12/43] fixed sampling the batch --- .../traversability_estimator/nodes.py | 49 ++++++++++----- .../traversability_estimator.py | 61 +------------------ 2 files changed, 37 insertions(+), 73 deletions(-) diff --git a/wild_visual_navigation/traversability_estimator/nodes.py b/wild_visual_navigation/traversability_estimator/nodes.py index 90849c0c..dd8e9448 100644 --- a/wild_visual_navigation/traversability_estimator/nodes.py +++ b/wild_visual_navigation/traversability_estimator/nodes.py @@ -182,25 +182,44 @@ def change_device(self, device): if self._confidence is not None: self._confidence = self._confidence.to(device) - def as_pyg_data(self, previous_node: Optional[BaseNode] = None, aux: bool = False): + def as_pyg_data(self, previous_node: Optional[BaseNode] = None, anomaly_detection: bool = False, aux: bool = False): if aux: return Data(x=self.features, edge_index=self._feature_edges) if previous_node is None: - return Data( - x=self.features, - edge_index=self._feature_edges, - y=self._supervision_signal, - y_valid=self._supervision_signal_valid, - ) + if anomaly_detection: + return Data( + x=self.features[self._supervision_signal_valid], + edge_index=self._feature_edges[self._supervision_signal_valid], + y=self._supervision_signal[self._supervision_signal_valid], + y_valid=self._supervision_signal_valid[self._supervision_signal_valid], + ) + else: + return Data( + x=self.features, + edge_index=self._feature_edges, + y=self._supervision_signal, + y_valid=self._supervision_signal_valid, + ) + else: - return Data( - x=self.features, - edge_index=self._feature_edges, - y=self._supervision_signal, - y_valid=self._supervision_signal_valid, - x_previous=previous_node.features, - edge_index_previous=previous_node._feature_edges, - ) + if anomaly_detection: + return Data( + x=self.features[self._supervision_signal_valid], + edge_index=self._feature_edges[self._supervision_signal_valid], + y=self._supervision_signal[self._supervision_signal_valid], + y_valid=self._supervision_signal_valid[self._supervision_signal_valid], + x_previous=previous_node.features, + edge_index_previous=previous_node._feature_edges, + ) + else: + return Data( + x=self.features, + edge_index=self._feature_edges, + y=self._supervision_signal, + y_valid=self._supervision_signal_valid, + x_previous=previous_node.features, + edge_index_previous=previous_node._feature_edges, + ) def is_valid(self): diff --git a/wild_visual_navigation/traversability_estimator/traversability_estimator.py b/wild_visual_navigation/traversability_estimator/traversability_estimator.py index 7de385e6..49665f78 100644 --- a/wild_visual_navigation/traversability_estimator/traversability_estimator.py +++ b/wild_visual_navigation/traversability_estimator/traversability_estimator.py @@ -555,64 +555,9 @@ def make_batch( batch_size (int): Size of the batch """ - if not anomaly_detection: - # Just sample N random nodes - mission_nodes = self._mission_graph.get_n_random_valid_nodes(n=batch_size) - batch = Batch.from_data_list([x.as_pyg_data() for x in mission_nodes]) - - else: - # TODO: how does the samplig make the most sense - # last_mission_node = self._mission_graph.get_last_node() - # - # if last_mission_node is None: - # return None - # if (not hasattr(last_mission_node, "supervision_mask")) or (last_mission_node.supervision_mask is None): - # return None - # - # mission_nodes = self._mission_graph.get_nodes_within_radius_range( - # last_mission_node, 0, self._proprio_graph.max_distance - # ) - - mission_nodes = self._mission_graph.get_n_random_valid_nodes(n=batch_size) - - p_feat = [] - - for i, mnode in enumerate(mission_nodes): - feat = mnode.features - seg = mnode.feature_segments - mask = mnode.supervision_mask - - mask = torch.nan_to_num(mask.nanmean(axis=0)) != 0 - - # Visualize supervision mask - if vis_training_samples: - self._last_image_mask_pub.publish( - self._bridge.cv2_to_imgmsg(mask.cpu().numpy().astype(np.uint8) * 255, "mono8") - ) - - # Save mask as numpy with opencv - # cv2.imwrite(os.path.join("/home/rschmid/ext", "mask", f"{rospy.get_time()}.png"), mask.cpu().numpy().astype(np.uint8) * 255) - - for s in torch.unique(seg): - m = mask[seg == s] - pos = m.sum() # Inside - neg = (~m).sum() # Outside - - # Check if more inside features than outside features - if pos > neg: - # Check if the vector contains any NaN values - if not np.isnan(feat[s.item()].cpu()).any(): - p_feat.append(feat[s.item()]) - - # Only sample up to 200 features - if len(p_feat) > n_features: - random.shuffle(p_feat) - p_feat = p_feat[:n_features] - break - - p_feat = torch.stack(p_feat, dim=1).T - - batch = Data(x=p_feat) + # Just sample N random nodes + mission_nodes = self._mission_graph.get_n_random_valid_nodes(n=batch_size) + batch = Batch.from_data_list([x.as_pyg_data(anomaly_detection=anomaly_detection) for x in mission_nodes]) return batch From 85d21033a036cf96e742bf0bc05c25c71e5983f2 Mon Sep 17 00:00:00 2001 From: Jonas Frey Date: Tue, 29 Aug 2023 18:44:56 +0200 Subject: [PATCH 13/43] started to update the confidence generator --- wild_visual_navigation/learning/utils/loss.py | 55 +++++++------------ .../traversability_estimator.py | 31 +++-------- .../wild_visual_navigation/default.yaml | 1 - .../scripts/wvn_feature_extractor_node.py | 25 ++++----- 4 files changed, 40 insertions(+), 72 deletions(-) diff --git a/wild_visual_navigation/learning/utils/loss.py b/wild_visual_navigation/learning/utils/loss.py index 50f2d3b5..501e7d86 100644 --- a/wild_visual_navigation/learning/utils/loss.py +++ b/wild_visual_navigation/learning/utils/loss.py @@ -4,26 +4,23 @@ from typing import Optional from wild_visual_navigation.utils import ConfidenceGenerator from torch import nn -from torchmetrics import ROC, AUROC, Accuracy class AnomalyLoss(nn.Module): - def __init__(self, confidence_std_factor, method): + def __init__(self, confidence_std_factor: float, method: str, log_enabled: bool, log_folder: str): super(AnomalyLoss, self).__init__() - # self._confidence_generator = ConfidenceGenerator( - # std_factor=confidence_std_factor, - # method=method, - # log_enabled=False, - # log_folder="/tmp", - # ) + + self._confidence_generator = ConfidenceGenerator( + std_factor=confidence_std_factor, + method=method, + log_enabled=log_enabled, + log_folder=log_folder, + ) def forward( self, graph: Optional[Data], res: dict, - loss_mean: int = None, - loss_std: int = None, - train: bool = False, update_generator: bool = True, step: int = 0, log_step: bool = False, @@ -31,25 +28,15 @@ def forward( loss_aux = {} loss_aux["loss_trav"] = torch.tensor([0.0]) loss_aux["loss_reco"] = torch.tensor([0.0]) - loss_aux["confidence"] = torch.tensor([0.0]) losses = -(res["logprob"].sum(1) + res["log_det"]) # Sum over all channels, resulting in h*w output dimensions - # print(torch.mean(losses)) - l_clip = losses - if loss_mean is not None and loss_std is not None: - # Clip the losses - l_clip = torch.clip(losses, loss_mean - 2 * loss_std, loss_mean + 2 * loss_std) - - # Normalize between 0 and 1 - l_norm = (losses - torch.min(l_clip)) / (torch.max(l_clip) - torch.min(l_clip)) - l_trav = 1 - l_norm + if update_generator: + confidence = self._confidence_generator.update(x=losses, x_positive=losses) - if train: - loss_aux["loss_mean"] = torch.median(losses) - loss_aux["loss_std"] = torch.std(losses) + loss_aux["confidence"] = confidence - return torch.mean(losses), loss_aux, l_trav + return torch.mean(losses), loss_aux, confidence def update_node_confidence(self, node): node.confidence = 0 @@ -58,16 +45,16 @@ def update_node_confidence(self, node): class TraversabilityLoss(nn.Module): def __init__( self, - w_trav, - w_reco, - w_temp, - anomaly_balanced, - model, - method, - confidence_std_factor, + w_trav: float, + w_reco: float, + w_temp: float, + anomaly_balanced: bool, + model: nn.Module, + method: str, + confidence_std_factor: float, + log_enabled: bool, + log_folder: str, trav_cross_entropy=False, - log_enabled: bool = False, - log_folder: str = "/tmp", ): # TODO remove trav_cross_entropy default param when running in online mode super(TraversabilityLoss, self).__init__() diff --git a/wild_visual_navigation/traversability_estimator/traversability_estimator.py b/wild_visual_navigation/traversability_estimator/traversability_estimator.py index 49665f78..0417e3ad 100644 --- a/wild_visual_navigation/traversability_estimator/traversability_estimator.py +++ b/wild_visual_navigation/traversability_estimator/traversability_estimator.py @@ -123,7 +123,11 @@ def __init__( self._model.train() if self._exp_cfg["model"]["name"] == "LinearRnvp": - self._traversability_loss = AnomalyLoss(**self._exp_cfg["loss_anomaly"]) + self._traversability_loss = AnomalyLoss( + **self._exp_cfg["loss_anomaly"], + log_enabled=self._exp_cfg["general"]["log_confidence"], + log_folder=self._exp_cfg["general"]["model_path"], + ) self._traversability_loss.to(self._device) else: @@ -545,9 +549,6 @@ def load_checkpoint(self, checkpoint_path: str): def make_batch( self, batch_size: int = 8, - anomaly_detection: bool = False, - n_features: int = 200, - vis_training_samples: bool = False, ): """Samples a batch from the mission_graph @@ -557,7 +558,7 @@ def make_batch( # Just sample N random nodes mission_nodes = self._mission_graph.get_n_random_valid_nodes(n=batch_size) - batch = Batch.from_data_list([x.as_pyg_data(anomaly_detection=anomaly_detection) for x in mission_nodes]) + batch = Batch.from_data_list([x.as_pyg_data(anomaly_detection=self._anomaly_detection) for x in mission_nodes]) return batch @@ -575,16 +576,9 @@ def train(self): return_dict = {"mission_graph_num_valid_node": num_valid_nodes} if num_valid_nodes > self._min_samples_for_training: # Prepare new batch - graph = self.make_batch( - self._exp_cfg["ablation_data_module"]["batch_size"], - anomaly_detection=self._anomaly_detection, - vis_training_samples=self._vis_training_samples, - ) + graph = self.make_batch(self._exp_cfg["ablation_data_module"]["batch_size"]) if graph is not None: - self._loss_mean = None - self._loss_std = None - with self._learning_lock: # Forward pass @@ -592,18 +586,9 @@ def train(self): log_step = (self._step % 20) == 0 self._loss, loss_aux, trav = self._traversability_loss( - graph, - res, - step=self._step, - log_step=log_step, - loss_mean=self._loss_mean, - loss_std=self._loss_std, - train=True, + graph, res, step=self._step, log_step=log_step ) - self._loss_mean = loss_aux["loss_mean"] - self._loss_std = loss_aux["loss_std"] - # Keep track of ROC during training for rescaling the loss when publishing if self._scale_traversability: # This mask should contain all the segments corrosponding to trees. diff --git a/wild_visual_navigation_ros/config/wild_visual_navigation/default.yaml b/wild_visual_navigation_ros/config/wild_visual_navigation/default.yaml index 12bd35a1..754b76d8 100644 --- a/wild_visual_navigation_ros/config/wild_visual_navigation/default.yaml +++ b/wild_visual_navigation_ros/config/wild_visual_navigation/default.yaml @@ -33,7 +33,6 @@ min_samples_for_training: 5 prediction_per_pixel: false traversability_threshold: 0.55 clip_to_binary: false -anomaly_detection: true vis_training_samples: true # Supervision Generator 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 86e5f6b9..c3809603 100644 --- a/wild_visual_navigation_ros/scripts/wvn_feature_extractor_node.py +++ b/wild_visual_navigation_ros/scripts/wvn_feature_extractor_node.py @@ -51,7 +51,6 @@ def __init__(self): method=self.exp_cfg["loss"]["method"], std_factor=self.exp_cfg["loss"]["confidence_std_factor"] ) self.scale_traversability = True - self.traversability_threshold = 0.5 else: self.traversability_loss = AnomalyLoss(**self.exp_cfg["loss_anomaly"]) self.traversability_loss.to(self.device) @@ -355,22 +354,20 @@ def load_model(self): self.log_data[f"nr_model_updates"] += 1 self.model.load_state_dict(res, strict=False) - # if res["traversability_threshold"] is not None: - # self.traversability_threshold = res["traversability_threshold"] - # if res["confidence_generator"] is not None: - # self.confidence_generator_state = res["confidence_generator"] - # self.traversability_threshold = 0.5 + try: + if res["traversability_threshold"] is not None: + self.traversability_threshold = res["traversability_threshold"] + if res["confidence_generator"] is not None: + self.confidence_generator_state = res["confidence_generator"] - # self.confidence_generator_state = res["confidence_generator"] + self.confidence_generator_state = res["confidence_generator"] + self.confidence_generator.var = self.confidence_generator_state["var"] + self.confidence_generator.mean = self.confidence_generator_state["mean"] + self.confidence_generator.std = self.confidence_generator_state["std"] + except: + pass - # self.confidence_generator.var = 0 - # self.confidence_generator.mean = 1 - # self.confidence_generator.std = 1 - - # self.confidence_generator.var = self.confidence_generator_state["var"] - # self.confidence_generator.mean = self.confidence_generator_state["mean"] - # self.confidence_generator.std = self.confidence_generator_state["std"] except Exception as e: if self.verbose: print(f"Model Loading Failed: {e}") From 112aa610c3c998049b1694e77ed63520efe46728 Mon Sep 17 00:00:00 2001 From: RobinSchmid7 Date: Wed, 30 Aug 2023 12:57:24 +0200 Subject: [PATCH 14/43] Fixes to recent commits --- wild_visual_navigation/cfg/experiment_params.py | 2 +- wild_visual_navigation/learning/utils/loss.py | 6 +++--- wild_visual_navigation/traversability_estimator/nodes.py | 4 ++-- wild_visual_navigation/utils/confidence_generator.py | 5 ++++- .../scripts/wvn_feature_extractor_node.py | 6 ++++-- 5 files changed, 14 insertions(+), 9 deletions(-) diff --git a/wild_visual_navigation/cfg/experiment_params.py b/wild_visual_navigation/cfg/experiment_params.py index a64d616c..1f3768b1 100644 --- a/wild_visual_navigation/cfg/experiment_params.py +++ b/wild_visual_navigation/cfg/experiment_params.py @@ -50,7 +50,7 @@ class LossParams: @dataclass class LossAnomalyParams: - method: str = "latest_measurment" + method: str = "latest_measurment" # "latest_measurment", "running_mean" confidence_std_factor: float = 0.5 loss_anomaly: LossAnomalyParams = LossAnomalyParams() diff --git a/wild_visual_navigation/learning/utils/loss.py b/wild_visual_navigation/learning/utils/loss.py index 501e7d86..48f9302a 100644 --- a/wild_visual_navigation/learning/utils/loss.py +++ b/wild_visual_navigation/learning/utils/loss.py @@ -29,14 +29,14 @@ def forward( loss_aux["loss_trav"] = torch.tensor([0.0]) loss_aux["loss_reco"] = torch.tensor([0.0]) - losses = -(res["logprob"].sum(1) + res["log_det"]) # Sum over all channels, resulting in h*w output dimensions + losses = res["logprob"].sum(1) + res["log_det"] # Sum over all channels, resulting in h*w output dimensions if update_generator: - confidence = self._confidence_generator.update(x=losses, x_positive=losses) + confidence = self._confidence_generator.update(x=losses, x_positive=losses, step=step) loss_aux["confidence"] = confidence - return torch.mean(losses), loss_aux, confidence + return -torch.mean(losses), loss_aux, confidence def update_node_confidence(self, node): node.confidence = 0 diff --git a/wild_visual_navigation/traversability_estimator/nodes.py b/wild_visual_navigation/traversability_estimator/nodes.py index dd8e9448..f5cb4150 100644 --- a/wild_visual_navigation/traversability_estimator/nodes.py +++ b/wild_visual_navigation/traversability_estimator/nodes.py @@ -189,7 +189,7 @@ def as_pyg_data(self, previous_node: Optional[BaseNode] = None, anomaly_detectio if anomaly_detection: return Data( x=self.features[self._supervision_signal_valid], - edge_index=self._feature_edges[self._supervision_signal_valid], + edge_index=self._feature_edges, y=self._supervision_signal[self._supervision_signal_valid], y_valid=self._supervision_signal_valid[self._supervision_signal_valid], ) @@ -205,7 +205,7 @@ def as_pyg_data(self, previous_node: Optional[BaseNode] = None, anomaly_detectio if anomaly_detection: return Data( x=self.features[self._supervision_signal_valid], - edge_index=self._feature_edges[self._supervision_signal_valid], + edge_index=self._feature_edges, y=self._supervision_signal[self._supervision_signal_valid], y_valid=self._supervision_signal_valid[self._supervision_signal_valid], x_previous=previous_node.features, diff --git a/wild_visual_navigation/utils/confidence_generator.py b/wild_visual_navigation/utils/confidence_generator.py index fffbfe95..7113fe6c 100644 --- a/wild_visual_navigation/utils/confidence_generator.py +++ b/wild_visual_navigation/utils/confidence_generator.py @@ -131,7 +131,10 @@ def inference_without_update(self, x: torch.tensor): if x.device != self.mean.device: return torch.zeros_like(x) - confidence = torch.exp(-(((x - self.mean) / (self.std * self.std_factor)) ** 2) * 0.5) + # confidence = torch.exp(-(((x - self.mean) / (self.std * self.std_factor)) ** 2) * 0.5) + x = torch.clip(x, self.mean - 2 * self.std, self.mean + 2 * self.std) + confidence = (x - torch.min(x)) / (torch.max(x) - torch.min(x)) + return confidence.type(torch.float32) def forward(self, x: torch.tensor): 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 c3809603..44b806bf 100644 --- a/wild_visual_navigation_ros/scripts/wvn_feature_extractor_node.py +++ b/wild_visual_navigation_ros/scripts/wvn_feature_extractor_node.py @@ -52,7 +52,9 @@ def __init__(self): ) self.scale_traversability = True else: - self.traversability_loss = AnomalyLoss(**self.exp_cfg["loss_anomaly"]) + self.traversability_loss = AnomalyLoss(**self.exp_cfg["loss_anomaly"], + log_enabled=self.exp_cfg["general"]["log_confidence"], + log_folder=self.exp_cfg["general"]["model_path"]) self.traversability_loss.to(self.device) self.scale_traversability = False @@ -348,7 +350,7 @@ def load_model(self): res = torch.load(f"{WVN_ROOT_DIR}/tmp_state_dict2.pt") k = list(self.model.state_dict().keys())[-1] - if (self.model.state_dict()[k] != res[k]).any(): # TODO: model params are changing? + if (self.model.state_dict()[k] != res[k]).any(): if self.verbose: self.log_data[f"time_last_model"] = rospy.get_time() self.log_data[f"nr_model_updates"] += 1 From 70e49f2509bf5e9cb0c46b089b64914b9bb238f1 Mon Sep 17 00:00:00 2001 From: RobinSchmid7 Date: Wed, 30 Aug 2023 14:22:50 +0200 Subject: [PATCH 15/43] Add DINO dim param --- .../feature_extractor/feature_extractor.py | 5 ++--- .../config/wild_visual_navigation/default.yaml | 1 + .../scripts/wvn_feature_extractor_node.py | 2 ++ 3 files changed, 5 insertions(+), 3 deletions(-) diff --git a/wild_visual_navigation/feature_extractor/feature_extractor.py b/wild_visual_navigation/feature_extractor/feature_extractor.py index 8d638a79..702aca38 100644 --- a/wild_visual_navigation/feature_extractor/feature_extractor.py +++ b/wild_visual_navigation/feature_extractor/feature_extractor.py @@ -17,8 +17,7 @@ class FeatureExtractor: def __init__( - self, device: str, segmentation_type: str = "slic", feature_type: str = "dino", input_size: int = 448, **kwargs - ): + self, device: str, segmentation_type: str = "slic", feature_type: str = "dino", input_size: int = 448, **kwargs): """Feature extraction from image Args: @@ -40,7 +39,7 @@ def __init__( elif self._feature_type == "dino": self._feature_dim = 90 - self.extractor = DinoInterface(device=device, input_size=input_size, patch_size=kwargs.get("patch_size", 8)) + self.extractor = DinoInterface(device=device, input_size=input_size, patch_size=kwargs.get("patch_size", 8), dim=kwargs.get("dino_dim", 384)) elif self._feature_type == "sift": self._feature_dim = 128 self.extractor = DenseSIFTDescriptor().to(device) diff --git a/wild_visual_navigation_ros/config/wild_visual_navigation/default.yaml b/wild_visual_navigation_ros/config/wild_visual_navigation/default.yaml index 754b76d8..ad268471 100644 --- a/wild_visual_navigation_ros/config/wild_visual_navigation/default.yaml +++ b/wild_visual_navigation_ros/config/wild_visual_navigation/default.yaml @@ -25,6 +25,7 @@ network_input_image_width: 224 # 448 segmentation_type: "slic" feature_type: "dino" dino_patch_size: 8 # DINO only, 16 +dino_dim: 384 # 90, 384 slic_num_components: 500 confidence_std_factor: 4.0 scale_traversability: False # This parameter needs to be false when using the anomaly detection model 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 44b806bf..13542b19 100644 --- a/wild_visual_navigation_ros/scripts/wvn_feature_extractor_node.py +++ b/wild_visual_navigation_ros/scripts/wvn_feature_extractor_node.py @@ -39,6 +39,7 @@ def __init__(self): feature_type=self.feature_type, input_size=self.network_input_image_height, slic_num_components=self.slic_num_components, + dino_dim=self.dino_dim, ) self.i = 0 self.setup_ros() @@ -116,6 +117,7 @@ def read_params(self): self.segmentation_type = rospy.get_param("~segmentation_type") self.feature_type = rospy.get_param("~feature_type") self.dino_patch_size = rospy.get_param("~dino_patch_size") + self.dino_dim = rospy.get_param("~dino_dim") self.slic_num_components = rospy.get_param("~slic_num_components") self.traversability_threshold = rospy.get_param("~traversability_threshold") self.clip_to_binary = rospy.get_param("~clip_to_binary") From 19ba16686a12f733632c050fa82850ba248c1ac8 Mon Sep 17 00:00:00 2001 From: RobinSchmid7 Date: Wed, 30 Aug 2023 14:23:14 +0200 Subject: [PATCH 16/43] Add DINO dim param --- wild_visual_navigation/feature_extractor/dino_interface.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/wild_visual_navigation/feature_extractor/dino_interface.py b/wild_visual_navigation/feature_extractor/dino_interface.py index 0973af0f..fda40cc7 100644 --- a/wild_visual_navigation/feature_extractor/dino_interface.py +++ b/wild_visual_navigation/feature_extractor/dino_interface.py @@ -21,7 +21,7 @@ def __init__( projection_type: str = None, # nonlinear or None dropout: bool = False, # True or False ): - self.dim = dim # 90 or 384 + self.dim = dim self.cfg = DictConfig( { "dino_patch_size": patch_size, From 89746f68d726662556d2cba176943f2a31ab0b6b Mon Sep 17 00:00:00 2001 From: RobinSchmid7 Date: Wed, 30 Aug 2023 14:28:24 +0200 Subject: [PATCH 17/43] Remove mean kernel, did not help --- wild_visual_navigation/feature_extractor/dino_interface.py | 4 ---- 1 file changed, 4 deletions(-) diff --git a/wild_visual_navigation/feature_extractor/dino_interface.py b/wild_visual_navigation/feature_extractor/dino_interface.py index fda40cc7..713e06cd 100644 --- a/wild_visual_navigation/feature_extractor/dino_interface.py +++ b/wild_visual_navigation/feature_extractor/dino_interface.py @@ -72,8 +72,6 @@ def __init__( # Just normalization self.norm = T.Normalize([0.485, 0.456, 0.406], [0.229, 0.224, 0.225]) - self.mean_kernel = torch.ones((1, 5, 5), device=device) / 25 - def change_device(self, device): """Changes the device of all the class members @@ -142,8 +140,6 @@ def inference(self, img: torch.tensor, interpolate: bool = False): pad = int((W - H) / 2) features = F.interpolate(features, new_size, mode="bilinear", align_corners=True) features = F.pad(features, pad=[pad, pad, 0, 0]) - # Optionally turn on image feature smoothing - # features = filter2d(features, self.mean_kernel, "replicate") return features @property From b11a435e8c06a08f368818012fa7f8b9bfc7aec4 Mon Sep 17 00:00:00 2001 From: Robin Date: Thu, 31 Aug 2023 15:58:15 +0200 Subject: [PATCH 18/43] updates for dodo --- .../config/procman/robot_dodo.pmd | 235 ++++++++++++++++++ .../launch/resize_images.launch | 2 +- .../launch/robot.launch | 4 +- .../launch/wild_visual_navigation.launch | 4 +- .../scripts/wvn_feature_extractor_node.py | 25 +- .../scripts/wvn_learning_node.py | 2 +- 6 files changed, 260 insertions(+), 12 deletions(-) create mode 100644 wild_visual_navigation_ros/config/procman/robot_dodo.pmd diff --git a/wild_visual_navigation_ros/config/procman/robot_dodo.pmd b/wild_visual_navigation_ros/config/procman/robot_dodo.pmd new file mode 100644 index 00000000..c2f87fc8 --- /dev/null +++ b/wild_visual_navigation_ros/config/procman/robot_dodo.pmd @@ -0,0 +1,235 @@ +group "1.startup" { + cmd "1.1.chrony" { + exec = "rosrun anymal_rsl_chrony reset_chrony.sh jonfrey dodo d020 false"; + host = "localhost"; + } + cmd "1.2.depth" { + exec = "rosservice call /depth_cameras/enable 'data: true' "; + host = "localhost"; + } + cmd "1.3.lidar" { + exec = "rosservice call /lidar/enable 'data: true' "; + host = "localhost"; + } + cmd "1.4.wide angle" { + exec = "rosservice call /wide_angle_cameras/enable 'data: true' "; + host = "localhost"; + } + cmd "1.5.power jetson" { + exec = "/home/jonfrey/powerline.sh"; + host = "lpc"; + } + cmd "1.6.ping jetson" { + exec = "ping anymal-dodo-jetson"; + host = "localhost"; + } +} + +group "2.start" { + cmd "2.1.lpc" { + exec = "rosrun anymal_dodo_rsl lpc.py"; + host = "lpc"; + } + cmd "2.2.npc" { + exec = "rosrun anymal_d_rsl npc.py"; + host = "npc"; + } + cmd "2.3.jetson" { + exec = "rosrun anymal_d_rsl jetson.py"; + host = "jetson"; + } + cmd "2.4.opc" { + exec = "rosrun anymal_d_rsl opc.py"; + host = "localhost"; + } +} + +group "3.recording" { + cmd "3.1.rosbag_record" { + exec = "rosservice call /rosbag_record_robot_coordinator/record_bag 'yaml_file: /home/jonfrey/git/diffusion_navigation/diffusion_navigation_ros/config/recording/dodo.yaml' "; + host = "localhost"; + } + cmd "3.2.rosbag_record_state" { + exec = "rosnode list | grep record"; + host = "localhost"; + } + cmd "3.3.rosbag_stop" { + exec = "rosservice call /rosbag_record_robot_coordinator/stop_bag 'verbose: false' "; + host = "localhost"; + } + cmd "3.4.fetch_bags" { + exec = "/home/jonfrey/git/anymal_rsl/anymal_rsl/anymal_rsl_utils/anymal_rsl_recording/anymal_rsl_recording/bin/copy_mission_data_from_robot.sh jonfrey dodo /home/jonfrey/mission_data"; + host = "localhost"; + } + cmd "3.5.delete_bags" { + exec = "/home/jonfrey/git/anymal_rsl/anymal_rsl/anymal_rsl_utils/anymal_rsl_recording/anymal_rsl_recording/bin/remove_mission_data_from_robot.sh jonfrey dodo"; + host = "localhost"; + } + cmd "3.6.reset_recording" { + exec = "rosnode kill rosbag_record_coordinator rosbag_record_robot_jetson rosbag_record_robot_npc rosbag_record_robot_lpc"; + host = "localhost"; + } +} + +group "4.monitoring" { + cmd "4.1.lpc_disk" { + exec = "rostopic echo /disk_monitor_lpc/status/disks[1]"; + host = "localhost"; + } + cmd "4.2.npc_disk" { + exec = "rostopic echo /disk_monitor_npc/status/disks[1]"; + host = "localhost"; + } + cmd "4.3.jetson_disk" { + exec = "rostopic echo /disk_monitor_jetson/status/disks[1]"; + host = "localhost"; + } +} + +group "5.network" { + cmd "5.1.switch_to_access_point" { + exec = "rosrun anymal_router_utils router_utils.bash -i 192.168.0.173 -p d020 -m 1"; + host = "localhost"; + } + cmd "5.2.switch_to_client_mode" { + exec = "rosrun anymal_router_utils router_utils.bash -i 192.168.0.173 -p d020 -m 3"; + host = "localhost"; + } +} + + +group "6.disk_free" { + cmd "6.1.lpc" { + exec = "df -h /dev/sda4"; + host = "lpc"; + } + cmd "6.2.npc" { + exec = " df -h /dev/sda4"; + host = "npc"; + } + cmd "6.3.jetson" { + exec = "df -h /dev/nvme0n1p1"; + host = "jetson"; + } + cmd "6.3.opc" { + exec = "df -h /dev/nvme0n1p5"; + host = "localhost"; + } + +} + +group "7.tools for bags" { + cmd "7.1.collect_lpc" { + exec = " rsync -rzPav anymal-dodo-lpc:/home/jonfrey/catkin_ws/src/anymal_rsl/anymal_rsl/anymal_rsl_utils/anymal_rsl_recording/anymal_rsl_recording/data /home/jonfrey/mission_data/lpc"; + host = "jetson"; + } + cmd "7.2.collect_npc" { + exec = " rsync -rzPav anymal-dodo-npc:/home/jonfrey/catkin_ws/src/anymal_rsl/anymal_rsl/anymal_rsl_utils/anymal_rsl_recording/anymal_rsl_recording/data /home/jonfrey/mission_data/npc"; + host = "jetson"; + } + cmd "7.3.delete lpc" { + exec = "rm -r -f /home/jonfrey/catkin_ws/src/anymal_rsl/anymal_rsl/anymal_rsl_utils/anymal_rsl_recording/anymal_rsl_recording/data/*.bag"; + host = "lpc"; + } + cmd "7.4.delete npc" { + exec = "rm -r -f /home/jonfrey/catkin_ws/src/anymal_rsl/anymal_rsl/anymal_rsl_utils/anymal_rsl_recording/anymal_rsl_recording/data/*.bag"; + host = "npc"; + } +} + + +group "8.npc" { + cmd "8.1.alphasense_ptp" { + exec = "sudo service phc2sys restart"; + host = "npc"; + } + cmd "8.2.anymal_msg_converter" { + exec = "rosrun wild_visual_navigation_anymal anymal_msg_converter_node.py"; + host = "npc"; + } + cmd "8.3.local_planner_visual" { + exec = "roslaunch wild_visual_navigation_ros field_local_planner.launch traversability:=visual base_inverted:=false"; + host = "npc"; + } + cmd "8.4.local_planner_visual_inverted" { + exec = "roslaunch wild_visual_navigation_ros field_local_planner.launch traversability:=visual base_inverted:=true"; + host = "npc"; + } + cmd "8.5.local_planner_geometric" { + exec = "roslaunch wild_visual_navigation_ros field_local_planner.launch traversability:=geometric base_inverted:=false"; + host = "npc"; + } + cmd "8.6.local_planner_geometric_inverted" { + exec = "roslaunch wild_visual_navigation_ros field_local_planner.launch traversability:=geometric base_inverted:=true"; + host = "npc"; + } + cmd "8.7.local_planner_debug_on_robot" { + exec = "roslaunch wild_visual_navigation_ros field_local_planner.launch traversability:=geometric base_inverted:=true output_twist_topic:=/dummy_twist"; + host = "npc"; + } + cmd "8.8.smart_carrot" { + exec = "rosrun wild_visual_navigation_ros smart_carrot.py"; + host = "npc"; + } +} + + +group "9.orin" { + cmd "9.1.wild_visual_navigation" { + exec = "roslaunch wild_visual_navigation_ros robot.launch"; + host = "jetson"; + } + + cmd "9.2.kill_wvn" { + exec = "rosnode kill /wild_visual_navigation_node"; + host = "localhost"; + } +} + +group "10.visualization" { + cmd "10.1.rviz" { + exec = "roslaunch wild_visual_navigation_ros view.launch"; + host = "localhost"; + } +} + +group "11.configuration" { + cmd "11.1.dynamic_reconfigure" { + exec = "rosrun rqt_reconfigure rqt_reconfigure"; + host = "localhost"; + } +} + +group "12.white_balance" { + cmd "12.1.white_balance_front_reset" { + exec = "rosservice call /alphasense_raw_image_pipeline_front/reset_white_balance"; + host = "localhost"; + } + cmd "12.2.white_balance_left_reset" { + exec = "rosservice call /alphasense_raw_image_pipeline_left/reset_white_balance"; + host = "localhost"; + } + cmd "12.3.white_balance_right_reset" { + exec = "rosservice call /alphasense_raw_image_pipeline_right/reset_white_balance"; + host = "localhost"; + } +} + +group "x.learning_utils" { + cmd "x.1.pause_training" { + exec = "rosservice call /wild_visual_navigation_node/pause_learning 'data: true'"; + host = "npc"; + } + cmd "x.2.resume_training" { + exec = "rosservice call /wild_visual_navigation_node/pause_learning 'data: false'"; + host = "npc"; + } + cmd "x.3.save_checkpoint" { + exec = "rosservice call /wild_visual_navigation_node/save_checkpoint ''"; + host = "anymal_coyote_orin"; + } + cmd "x.4.load_checkpoint" { + exec = "rosservice call /wild_visual_navigation_node/load_checkpoint 'path: 'absolute_path_in_robot_filesystem'"; + host = "npc"; + } +} \ No newline at end of file diff --git a/wild_visual_navigation_ros/launch/resize_images.launch b/wild_visual_navigation_ros/launch/resize_images.launch index b1b9801e..813c8de1 100644 --- a/wild_visual_navigation_ros/launch/resize_images.launch +++ b/wild_visual_navigation_ros/launch/resize_images.launch @@ -7,7 +7,7 @@ - + diff --git a/wild_visual_navigation_ros/launch/robot.launch b/wild_visual_navigation_ros/launch/robot.launch index 86ce0bc9..c8090112 100644 --- a/wild_visual_navigation_ros/launch/robot.launch +++ b/wild_visual_navigation_ros/launch/robot.launch @@ -1,7 +1,7 @@ - + @@ -11,6 +11,6 @@ - + diff --git a/wild_visual_navigation_ros/launch/wild_visual_navigation.launch b/wild_visual_navigation_ros/launch/wild_visual_navigation.launch index 1bce2bdd..c8457ed5 100644 --- a/wild_visual_navigation_ros/launch/wild_visual_navigation.launch +++ b/wild_visual_navigation_ros/launch/wild_visual_navigation.launch @@ -1,10 +1,10 @@ - + - + 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 44b806bf..c8196388 100644 --- a/wild_visual_navigation_ros/scripts/wvn_feature_extractor_node.py +++ b/wild_visual_navigation_ros/scripts/wvn_feature_extractor_node.py @@ -40,11 +40,13 @@ def __init__(self): input_size=self.network_input_image_height, slic_num_components=self.slic_num_components, ) - self.i = 0 - self.setup_ros() + self.i = 0 + self.model = get_model(self.exp_cfg["model"]).to(self.device) self.model.eval() + + if not self.anomaly_detection: self.confidence_generator = ConfidenceGenerator( @@ -52,24 +54,28 @@ def __init__(self): ) self.scale_traversability = True else: - self.traversability_loss = AnomalyLoss(**self.exp_cfg["loss_anomaly"], - log_enabled=self.exp_cfg["general"]["log_confidence"], - log_folder=self.exp_cfg["general"]["model_path"]) + self.traversability_loss = AnomalyLoss(**self.exp_cfg["loss_anomaly"], log_enabled=self.exp_cfg["general"]["log_confidence"], log_folder=self.exp_cfg["general"]["model_path"]) self.traversability_loss.to(self.device) self.scale_traversability = False if self.verbose: + self.log_data = {} + self.status_thread_stop_event = Event() self.status_thread = Thread(target=self.status_thread_loop, name="status") self.run_status_thread = True self.status_thread.start() + self.setup_ros() + rospy.on_shutdown(self.shutdown_callback) signal.signal(signal.SIGINT, self.shutdown_callback) signal.signal(signal.SIGTERM, self.shutdown_callback) def shutdown_callback(self, *args, **kwargs): self.run_status_thread = False + self.status_thread_stop_event.set() self.status_thread.join() + rospy.signal_shutdown(f"Wild Visual Navigation Feature Extraction killed {args}") sys.exit(0) @@ -77,6 +83,12 @@ def status_thread_loop(self): rate = rospy.Rate(self.status_thread_rate) # Learning loop while self.run_status_thread: + + self.status_thread_stop_event.wait(timeout=0.01) + if self.status_thread_stop_event.is_set(): + rospy.logwarn("Stopped learning thread") + break + t = rospy.get_time() x = PrettyTable() x.field_names = ["Key", "Value"] @@ -101,6 +113,8 @@ def status_thread_loop(self): except Exception as e: rate = rospy.Rate(self.status_thread_rate) print("Ignored jump pack in time!") + self.status_thread_stop_event.clear() + def read_params(self): """Reads all the parameters from the parameter server""" @@ -144,7 +158,6 @@ def setup_ros(self, setup_fully=True): if self.verbose: # DEBUG Logging - self.log_data = {} self.log_data[f"time_last_model"] = -1 self.log_data[f"nr_model_updates"] = -1 diff --git a/wild_visual_navigation_ros/scripts/wvn_learning_node.py b/wild_visual_navigation_ros/scripts/wvn_learning_node.py index f05368c9..a81e36c6 100644 --- a/wild_visual_navigation_ros/scripts/wvn_learning_node.py +++ b/wild_visual_navigation_ros/scripts/wvn_learning_node.py @@ -115,7 +115,7 @@ def __init__(self): signal.signal(signal.SIGTERM, self.shutdown_callback) # Launch processes - print("─" * 80) + print("-" * 80) print("Launching [learning] thread") if self.mode != WVNMode.EXTRACT_LABELS: self.learning_thread_stop_event = Event() From 2877f286c9caf33d9eec944878abad71602ee464 Mon Sep 17 00:00:00 2001 From: RobinSchmid7 Date: Fri, 1 Sep 2023 15:33:25 +0200 Subject: [PATCH 19/43] Remove unnecessary libraries, added visu for learning trajectory --- .../traversability_estimator.py | 11 -- .../config/rviz/anymal.rviz | 148 +++++++----------- .../scripts/wvn_learning_node.py | 75 +++++++-- 3 files changed, 121 insertions(+), 113 deletions(-) diff --git a/wild_visual_navigation/traversability_estimator/traversability_estimator.py b/wild_visual_navigation/traversability_estimator/traversability_estimator.py index 0417e3ad..e94d55e9 100644 --- a/wild_visual_navigation/traversability_estimator/traversability_estimator.py +++ b/wild_visual_navigation/traversability_estimator/traversability_estimator.py @@ -29,12 +29,7 @@ import torch.nn.functional as F import torchvision.transforms as transforms from torchmetrics import ROC -import numpy as np -import cv2 -import rospy import random -from sensor_msgs.msg import Image -from cv_bridge import CvBridge to_tensor = transforms.ToTensor() @@ -109,12 +104,6 @@ def __init__( # Visualization self._visualizer = LearningVisualizer() - if self._vis_training_samples: - self._last_image_mask_pub = rospy.Publisher( - f"/wild_visual_navigation_node/last_node_image_mask", Image, queue_size=1 - ) - self._bridge = CvBridge() - # Lightning module seed_everything(42) diff --git a/wild_visual_navigation_ros/config/rviz/anymal.rviz b/wild_visual_navigation_ros/config/rviz/anymal.rviz index 2bfde7c3..e029d92c 100644 --- a/wild_visual_navigation_ros/config/rviz/anymal.rviz +++ b/wild_visual_navigation_ros/config/rviz/anymal.rviz @@ -10,7 +10,7 @@ Panels: - /Elevation Map WIFI1/Local Planner (SDF)1 - /Elevation Map RAW1 Splitter Ratio: 0.4659898579120636 - Tree Height: 492 + Tree Height: 825 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -26,10 +26,9 @@ Panels: Name: Views Splitter Ratio: 0.5 - Class: rviz/Time - Experimental: false Name: Time SyncMode: 0 - SyncSource: F Input Image + SyncSource: velodyne Preferences: PromptSaveOnExit: true Toolbars: @@ -121,21 +120,25 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true - LF_HAA: + LF_HAA_drive: Alpha: 1 Show Axes: false Show Trail: false Value: true - LF_HFE: + LF_HFE_drive: Alpha: 1 Show Axes: false Show Trail: false Value: true + LF_HFE_output: + Alpha: 1 + Show Axes: false + Show Trail: false LF_HIP: Alpha: 1 Show Axes: false Show Trail: false - LF_KFE: + LF_KFE_drive: Alpha: 1 Show Axes: false Show Trail: false @@ -168,21 +171,25 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true - LH_HAA: + LH_HAA_drive: Alpha: 1 Show Axes: false Show Trail: false Value: true - LH_HFE: + LH_HFE_drive: Alpha: 1 Show Axes: false Show Trail: false Value: true + LH_HFE_output: + Alpha: 1 + Show Axes: false + Show Trail: false LH_HIP: Alpha: 1 Show Axes: false Show Trail: false - LH_KFE: + LH_KFE_drive: Alpha: 1 Show Axes: false Show Trail: false @@ -216,21 +223,25 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true - RF_HAA: + RF_HAA_drive: Alpha: 1 Show Axes: false Show Trail: false Value: true - RF_HFE: + RF_HFE_drive: Alpha: 1 Show Axes: false Show Trail: false Value: true + RF_HFE_output: + Alpha: 1 + Show Axes: false + Show Trail: false RF_HIP: Alpha: 1 Show Axes: false Show Trail: false - RF_KFE: + RF_KFE_drive: Alpha: 1 Show Axes: false Show Trail: false @@ -263,21 +274,25 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true - RH_HAA: + RH_HAA_drive: Alpha: 1 Show Axes: false Show Trail: false Value: true - RH_HFE: + RH_HFE_drive: Alpha: 1 Show Axes: false Show Trail: false Value: true + RH_HFE_output: + Alpha: 1 + Show Axes: false + Show Trail: false RH_HIP: Alpha: 1 Show Axes: false Show Trail: false - RH_KFE: + RH_KFE_drive: Alpha: 1 Show Axes: false Show Trail: false @@ -305,11 +320,6 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true - alphasense_mesh: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true base: Alpha: 1 Show Axes: false @@ -333,74 +343,43 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true - cam0_sensor_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - cam1_sensor_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - cam2_sensor_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - cam3_sensor_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - cam3_sensor_frame_helper: - Alpha: 1 - Show Axes: false - Show Trail: false - cam4_sensor_frame: + depth_camera_front_lower_camera: Alpha: 1 Show Axes: false Show Trail: false - cam4_sensor_frame_helper: + depth_camera_front_lower_camera_parent: Alpha: 1 Show Axes: false Show Trail: false - cam5_sensor_frame: + depth_camera_front_upper_camera: Alpha: 1 Show Axes: false Show Trail: false - cam5_sensor_frame_helper: + depth_camera_front_upper_camera_parent: Alpha: 1 Show Axes: false Show Trail: false - cam6_sensor_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - cam6_sensor_frame_helper: - Alpha: 1 - Show Axes: false - Show Trail: false - depth_camera_front_camera: + depth_camera_left_camera: Alpha: 1 Show Axes: false Show Trail: false - Value: true - depth_camera_front_camera_parent: + depth_camera_left_camera_parent: Alpha: 1 Show Axes: false Show Trail: false - depth_camera_left_camera: + depth_camera_rear_lower_camera: Alpha: 1 Show Axes: false Show Trail: false - Value: true - depth_camera_left_camera_parent: + depth_camera_rear_lower_camera_parent: Alpha: 1 Show Axes: false Show Trail: false - depth_camera_rear_camera: + depth_camera_rear_upper_camera: Alpha: 1 Show Axes: false Show Trail: false - Value: true - depth_camera_rear_camera_parent: + depth_camera_rear_upper_camera_parent: Alpha: 1 Show Axes: false Show Trail: false @@ -408,68 +387,55 @@ Visualization Manager: Alpha: 1 Show Axes: false Show Trail: false - Value: true depth_camera_right_camera_parent: Alpha: 1 Show Axes: false Show Trail: false - docking_hatch_cover: + docking_socket: Alpha: 1 Show Axes: false Show Trail: false + Value: true face_front: Alpha: 1 Show Axes: false Show Trail: false - Value: true face_rear: Alpha: 1 Show Axes: false Show Trail: false - Value: true - front_handle: + face_shell_front: Alpha: 1 Show Axes: false Show Trail: false Value: true - handle: + face_shell_rear: Alpha: 1 Show Axes: false Show Trail: false Value: true - hatch: + hatch_shell: Alpha: 1 Show Axes: false Show Trail: false Value: true - imu_link: - Alpha: 1 - Show Axes: false - Show Trail: false - imu_sensor_frame: + hbc_receiver: Alpha: 1 Show Axes: false Show Trail: false - interface_hatch: + imu_link: Alpha: 1 Show Axes: false Show Trail: false - Value: true lidar: Alpha: 1 Show Axes: false Show Trail: false Value: true - perception_head_cage: + lidar_parent: Alpha: 1 Show Axes: false Show Trail: false - Value: true - remote: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true top_shell: Alpha: 1 Show Axes: false @@ -479,7 +445,6 @@ Visualization Manager: Alpha: 1 Show Axes: false Show Trail: false - Value: true wide_angle_camera_front_camera_parent: Alpha: 1 Show Axes: false @@ -488,7 +453,6 @@ Visualization Manager: Alpha: 1 Show Axes: false Show Trail: false - Value: true wide_angle_camera_rear_camera_parent: Alpha: 1 Show Axes: false @@ -510,7 +474,7 @@ Visualization Manager: Marker Topic: /wild_visual_navigation_node/graph_footprints Name: Footprint Namespaces: - footprints: true + {} Queue Size: 100 Value: true - Alpha: 1 @@ -864,8 +828,8 @@ Visualization Manager: - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: 11.379964828491211 - Min Value: -1.0356265306472778 + Max Value: 15.248048782348633 + Min Value: -2.6211636066436768 Value: true Axis: Z Channel Name: intensity @@ -1531,7 +1495,7 @@ Window Geometry: collapsed: false F Traversability Raw: collapsed: false - Height: 1043 + Height: 1376 Hide Left Dock: false Hide Right Dock: true L Confidence Raw: @@ -1542,7 +1506,7 @@ 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: @@ -1565,6 +1529,6 @@ Window Geometry: collapsed: false Views: collapsed: true - Width: 1920 - X: 299 - Y: 122 + Width: 2488 + X: 72 + Y: 27 diff --git a/wild_visual_navigation_ros/scripts/wvn_learning_node.py b/wild_visual_navigation_ros/scripts/wvn_learning_node.py index f05368c9..90bece6e 100644 --- a/wild_visual_navigation_ros/scripts/wvn_learning_node.py +++ b/wild_visual_navigation_ros/scripts/wvn_learning_node.py @@ -5,6 +5,7 @@ from wild_visual_navigation.traversability_estimator import MissionNode, ProprioceptionNode import wild_visual_navigation_ros.ros_converter as rc from wild_visual_navigation_msgs.msg import RobotState, SystemState, ImageFeatures +from wild_visual_navigation.visu import LearningVisualizer from wild_visual_navigation_msgs.srv import ( LoadCheckpoint, SaveCheckpoint, @@ -370,14 +371,31 @@ def setup_ros(self, setup_fully=True): self.camera_topics[cam]["name"] = cam # Set subscribers - imagefeat_sub = message_filters.Subscriber(f"/wild_visual_navigation_node/{cam}/feat", ImageFeatures) - info_sub = message_filters.Subscriber(f"/wild_visual_navigation_node/{cam}/camera_info", CameraInfo) - sync = message_filters.ApproximateTimeSynchronizer([imagefeat_sub, info_sub], queue_size=4, slop=0.5) - sync.registerCallback(self.imagefeat_callback, self.camera_topics[cam]) + if self.mode == WVNMode.DEBUG: + # In debug mode additionally send the image to the callback function + self._visualizer = LearningVisualizer() + + imagefeat_sub = message_filters.Subscriber(f"/wild_visual_navigation_node/{cam}/feat", + ImageFeatures) + info_sub = message_filters.Subscriber(f"/wild_visual_navigation_node/{cam}/camera_info", CameraInfo) + image_sub = message_filters.Subscriber(f"/wild_visual_navigation_node/{cam}/image_input", + Image) + sync = message_filters.ApproximateTimeSynchronizer([imagefeat_sub, info_sub, image_sub], queue_size=4, + slop=0.5) + sync.registerCallback(self.imagefeat_callback, self.camera_topics[cam]) + + last_image_overlay_pub = rospy.Publisher( + f"/wild_visual_navigation_node/{cam}/debug/last_node_image_overlay", Image, queue_size=10 + ) + + self.camera_handler[cam]["debug"] = {} + self.camera_handler[cam]["debug"]["image_overlay"] = last_image_overlay_pub - self.camera_handler[cam]["image_sub"] = imagefeat_sub - self.camera_handler[cam]["info_sub"] = info_sub - self.camera_handler[cam]["sync"] = sync + else: + imagefeat_sub = message_filters.Subscriber(f"/wild_visual_navigation_node/{cam}/feat", ImageFeatures) + info_sub = message_filters.Subscriber(f"/wild_visual_navigation_node/{cam}/camera_info", CameraInfo) + sync = message_filters.ApproximateTimeSynchronizer([imagefeat_sub, info_sub], queue_size=4, slop=0.5) + sync.registerCallback(self.imagefeat_callback, self.camera_topics[cam]) # 3D outputs self.pub_debug_proprio_graph = rospy.Publisher( @@ -593,13 +611,20 @@ def robot_state_callback(self, state_msg, desired_twist_msg: TwistStamped): raise Exception("Error in robot state callback") @accumulate_time - def imagefeat_callback(self, imagefeat_msg: ImageFeatures, info_msg: CameraInfo, camera_options: dict): + def imagefeat_callback(self, *args): """Main callback to process incoming images Args: imagefeat_msg (wild_visual_navigation_msg/ImageFeatures): Incoming imagefeatures info_msg (sensor_msgs/CameraInfo): Camera info message associated to the image """ + if self.mode == WVNMode.DEBUG: + assert len(args) == 4 + imagefeat_msg, info_msg, image_msg, camera_options = tuple(args) + else: + assert len(args) == 3 + imagefeat_msg, info_msg, camera_options = tuple(args) + self.system_events["image_callback_received"] = {"time": rospy.get_time(), "value": "message received"} if self.verbose: print(f"\nImage callback: {camera_options['name']}... ", end="") @@ -649,12 +674,21 @@ def imagefeat_callback(self, imagefeat_msg: ImageFeatures, info_msg: CameraInfo, imagefeat_msg.feature_segments, desired_encoding="passthrough", device=self.device ).clone() h_small, w_small = feature_segments.shape[1:3] + + torch_image = torch.zeros((3, h_small, w_small), device=self.device, dtype=torch.float32) + + # convert image message to torch image + if self.mode == WVNMode.DEBUG: + torch_image = rc.ros_image_to_torch( + image_msg, desired_encoding="passthrough", device=self.device + ).clone() + # Create mission node for the graph mission_node = MissionNode( timestamp=ts, pose_base_in_world=pose_base_in_world, pose_cam_in_base=pose_cam_in_base, - image=torch.zeros((3, h_small, w_small), device=self.device, dtype=torch.float32), + image=torch_image, image_projector=image_projector, camera_name=camera_options["name"], use_for_training=camera_options["use_for_training"], @@ -673,7 +707,10 @@ def imagefeat_callback(self, imagefeat_msg: ImageFeatures, info_msg: CameraInfo, # Publish current predictions self.visualize_mission_graph() # Publish supervision data depending on the mode - self.visualize_debug() + self.visualize_image_overlay() + + if added_new_node: + self.traversability_estimator.update_visualization_node() # Print callback time if required if self.print_image_callback_time: @@ -811,6 +848,24 @@ def visualize_mission_graph(self): self.pub_mission_graph.publish(mission_graph_msg) + @accumulate_time + def visualize_image_overlay(self): + """Publishes all the debugging, slow visualizations""" + + # Get visualization node + vis_node = self.traversability_estimator.get_mission_node_for_visualization() + + # Publish reprojections of last node in graph + if vis_node is not None: + cam = vis_node.camera_name + torch_image = vis_node._image + torch_mask = vis_node._supervision_mask + torch_mask = torch.nan_to_num(torch_mask.nanmean(axis=0)) != 0 + torch_mask = torch_mask.float() + + image_out = self._visualizer.plot_detectron_classification(torch_image, torch_mask) + self.camera_handler[cam]["debug"]["image_overlay"].publish(rc.numpy_to_ros_image(image_out)) + if __name__ == "__main__": node_name = "wvn_learning_node" From 274768c184344113f977ef8c609f50f7cec3749b Mon Sep 17 00:00:00 2001 From: Robin Date: Mon, 4 Sep 2023 09:38:16 +0200 Subject: [PATCH 20/43] Robot changes --- .../config/wild_visual_navigation/default.yaml | 12 ++++++------ .../inputs/wide_angle_front_resize.yaml | 11 +++++++++++ ...ages.launch => resize_images_alphasense.launch} | 6 +++--- .../launch/resize_images_wide_angle_front.launch | 14 ++++++++++++++ wild_visual_navigation_ros/launch/robot.launch | 2 +- .../launch/wild_visual_navigation.launch | 12 +++++++----- .../scripts/wvn_feature_extractor_node.py | 10 +++++----- 7 files changed, 47 insertions(+), 20 deletions(-) create mode 100644 wild_visual_navigation_ros/config/wild_visual_navigation/inputs/wide_angle_front_resize.yaml rename wild_visual_navigation_ros/launch/{resize_images.launch => resize_images_alphasense.launch} (96%) create mode 100644 wild_visual_navigation_ros/launch/resize_images_wide_angle_front.launch diff --git a/wild_visual_navigation_ros/config/wild_visual_navigation/default.yaml b/wild_visual_navigation_ros/config/wild_visual_navigation/default.yaml index 754b76d8..6c785633 100644 --- a/wild_visual_navigation_ros/config/wild_visual_navigation/default.yaml +++ b/wild_visual_navigation_ros/config/wild_visual_navigation/default.yaml @@ -22,18 +22,18 @@ image_graph_dist_thr: 0.2 # meters proprio_graph_dist_thr: 0.1 # meters network_input_image_height: 224 # 448 network_input_image_width: 224 # 448 -segmentation_type: "slic" +segmentation_type: "random" feature_type: "dino" dino_patch_size: 8 # DINO only, 16 -slic_num_components: 500 +slic_num_components: 100 confidence_std_factor: 4.0 scale_traversability: False # This parameter needs to be false when using the anomaly detection model scale_traversability_max_fpr: 0.25 min_samples_for_training: 5 -prediction_per_pixel: false +prediction_per_pixel: True traversability_threshold: 0.55 -clip_to_binary: false -vis_training_samples: true +clip_to_binary: False +vis_training_samples: True # Supervision Generator untraversable_thr: 0.01 @@ -57,7 +57,7 @@ print_image_callback_time: false print_proprio_callback_time: false log_time: false log_confidence: false -verbose: true +verbose: false debug_supervision_node_index_from_last: 10 use_debug_for_desired: false diff --git a/wild_visual_navigation_ros/config/wild_visual_navigation/inputs/wide_angle_front_resize.yaml b/wild_visual_navigation_ros/config/wild_visual_navigation/inputs/wide_angle_front_resize.yaml new file mode 100644 index 00000000..d4d63fe0 --- /dev/null +++ b/wild_visual_navigation_ros/config/wild_visual_navigation/inputs/wide_angle_front_resize.yaml @@ -0,0 +1,11 @@ +camera_topics: + front: + image_topic: "/wide_angle_camera_front/image_color_resize" + info_topic: "/wide_angle_camera_front_resize/camera_info" + use_for_training: true + publish_confidence: true + publish_input_image: true + +# 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_ros/launch/resize_images.launch b/wild_visual_navigation_ros/launch/resize_images_alphasense.launch similarity index 96% rename from wild_visual_navigation_ros/launch/resize_images.launch rename to wild_visual_navigation_ros/launch/resize_images_alphasense.launch index 813c8de1..c51df980 100644 --- a/wild_visual_navigation_ros/launch/resize_images.launch +++ b/wild_visual_navigation_ros/launch/resize_images_alphasense.launch @@ -25,7 +25,7 @@ - + @@ -33,7 +33,7 @@ - + @@ -41,7 +41,7 @@ - + diff --git a/wild_visual_navigation_ros/launch/resize_images_wide_angle_front.launch b/wild_visual_navigation_ros/launch/resize_images_wide_angle_front.launch new file mode 100644 index 00000000..b7b855af --- /dev/null +++ b/wild_visual_navigation_ros/launch/resize_images_wide_angle_front.launch @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + \ No newline at end of file diff --git a/wild_visual_navigation_ros/launch/robot.launch b/wild_visual_navigation_ros/launch/robot.launch index c8090112..93ac8ad6 100644 --- a/wild_visual_navigation_ros/launch/robot.launch +++ b/wild_visual_navigation_ros/launch/robot.launch @@ -1,7 +1,7 @@ - + diff --git a/wild_visual_navigation_ros/launch/wild_visual_navigation.launch b/wild_visual_navigation_ros/launch/wild_visual_navigation.launch index c8457ed5..2ba3478f 100644 --- a/wild_visual_navigation_ros/launch/wild_visual_navigation.launch +++ b/wild_visual_navigation_ros/launch/wild_visual_navigation.launch @@ -1,10 +1,11 @@ - + - + + @@ -22,13 +23,14 @@ - + - + + 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 c8196388..68b8a363 100644 --- a/wild_visual_navigation_ros/scripts/wvn_feature_extractor_node.py +++ b/wild_visual_navigation_ros/scripts/wvn_feature_extractor_node.py @@ -108,11 +108,11 @@ def status_thread_loop(self): else: x.add_row([k, v]) print(x) - try: - rate.sleep() - except Exception as e: - rate = rospy.Rate(self.status_thread_rate) - print("Ignored jump pack in time!") + # try: + # rate.sleep() + # except Exception as e: + # rate = rospy.Rate(self.status_thread_rate) + # print("Ignored jump pack in time!") self.status_thread_stop_event.clear() From df1116f0290af98b95e39df37ab22cb461d48bf0 Mon Sep 17 00:00:00 2001 From: RobinSchmid7 Date: Mon, 4 Sep 2023 13:33:47 +0200 Subject: [PATCH 21/43] Added replay function for wide angle camera --- .../launch/replay_launch.launch | 19 +++++++++++-------- 1 file changed, 11 insertions(+), 8 deletions(-) diff --git a/wild_visual_navigation_ros/launch/replay_launch.launch b/wild_visual_navigation_ros/launch/replay_launch.launch index a39a9b87..ac7e522a 100644 --- a/wild_visual_navigation_ros/launch/replay_launch.launch +++ b/wild_visual_navigation_ros/launch/replay_launch.launch @@ -3,17 +3,18 @@ - + - - + + - + - - - + + + + @@ -26,7 +27,7 @@ - + @@ -73,5 +74,7 @@ + + \ No newline at end of file From d1a40448ddbc97aa9b3d3baa0f36ef1173dff85a Mon Sep 17 00:00:00 2001 From: RobinSchmid7 Date: Mon, 4 Sep 2023 13:57:28 +0200 Subject: [PATCH 22/43] Change visu color of overlay --- wild_visual_navigation/visu/visualizer.py | 9 ++++++--- wild_visual_navigation_ros/scripts/wvn_learning_node.py | 2 +- 2 files changed, 7 insertions(+), 4 deletions(-) diff --git a/wild_visual_navigation/visu/visualizer.py b/wild_visual_navigation/visu/visualizer.py index e6bb6499..4c136098 100644 --- a/wild_visual_navigation/visu/visualizer.py +++ b/wild_visual_navigation/visu/visualizer.py @@ -355,9 +355,12 @@ def plot_detectron_classification( overlay_mask=None, **kwargs, ): - cmap = cm.get_cmap("RdYlBu", 256) - cmap = np.concatenate([cmap(np.linspace(0, 0.3, 128)), cmap(np.linspace(0.7, 1.0, 128))]) - cmap = torch.from_numpy(cmap).to(seg)[:, :3] + if kwargs.get("cmap", None): + cmap = kwargs["cmap"] + else: + cmap = cm.get_cmap("RdYlBu", 256) + cmap = np.concatenate([cmap(np.linspace(0, 0.3, 128)), cmap(np.linspace(0.7, 1.0, 128))]) + cmap = torch.from_numpy(cmap).to(seg)[:, :3] img = self.plot_image(img, not_log=True) seg_img = self.plot_segmentation( diff --git a/wild_visual_navigation_ros/scripts/wvn_learning_node.py b/wild_visual_navigation_ros/scripts/wvn_learning_node.py index 8613f7fa..ad9dfa95 100644 --- a/wild_visual_navigation_ros/scripts/wvn_learning_node.py +++ b/wild_visual_navigation_ros/scripts/wvn_learning_node.py @@ -863,7 +863,7 @@ def visualize_image_overlay(self): torch_mask = torch.nan_to_num(torch_mask.nanmean(axis=0)) != 0 torch_mask = torch_mask.float() - image_out = self._visualizer.plot_detectron_classification(torch_image, torch_mask) + image_out = self._visualizer.plot_detectron_classification(torch_image, torch_mask, cmap="Greens") self.camera_handler[cam]["debug"]["image_overlay"].publish(rc.numpy_to_ros_image(image_out)) From cf7e0d76193b16036280578ab7c96bd8be3a16c1 Mon Sep 17 00:00:00 2001 From: RobinSchmid7 Date: Mon, 4 Sep 2023 14:02:20 +0200 Subject: [PATCH 23/43] Update rviz layout --- .../config/rviz/anymal.rviz | 32 +++++++++++-------- 1 file changed, 19 insertions(+), 13 deletions(-) diff --git a/wild_visual_navigation_ros/config/rviz/anymal.rviz b/wild_visual_navigation_ros/config/rviz/anymal.rviz index e029d92c..9c20e665 100644 --- a/wild_visual_navigation_ros/config/rviz/anymal.rviz +++ b/wild_visual_navigation_ros/config/rviz/anymal.rviz @@ -7,10 +7,16 @@ Panels: - /Global Options1 - /TF1/Tree1 - /Wild Visual Navigation1 + - /Wild Visual Navigation1/Cameras1 + - /Wild Visual Navigation1/Prediction1 + - /Wild Visual Navigation1/Prediction1/F Traversability Raw1 + - /Wild Visual Navigation1/Prediction1/F Learning Mask1 + - /Wild Visual Navigation1/Prediction1/F Confidence Overlay1 + - /Wild Visual Navigation1/LR Prediction1 - /Elevation Map WIFI1/Local Planner (SDF)1 - /Elevation Map RAW1 Splitter Ratio: 0.4659898579120636 - Tree Height: 825 + Tree Height: 698 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -474,7 +480,7 @@ Visualization Manager: Marker Topic: /wild_visual_navigation_node/graph_footprints Name: Footprint Namespaces: - {} + footprints: true Queue Size: 100 Value: true - Alpha: 1 @@ -828,8 +834,8 @@ Visualization Manager: - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: 15.248048782348633 - Min Value: -2.6211636066436768 + Max Value: 15.389507293701172 + Min Value: -1.5122853517532349 Value: true Axis: Z Channel Name: intensity @@ -883,11 +889,11 @@ Visualization Manager: Value: false - Class: rviz/Image Enabled: true - Image Topic: /wild_visual_navigation_node/front/confidence + Image Topic: /wild_visual_navigation_node/front/debug/last_node_image_overlay Max Value: 1 Median window: 5 Min Value: 0 - Name: F Confidence Raw + Name: F Learning Mask Normalize Range: true Queue Size: 2 Transport Hint: raw @@ -1084,7 +1090,7 @@ Visualization Manager: Transport Hint: raw Unreliable: false Value: false - Enabled: true + Enabled: false Name: LR Prediction Enabled: true Name: Wild Visual Navigation @@ -1448,7 +1454,7 @@ Visualization Manager: Views: Current: Class: rviz/ThirdPersonFollower - Distance: 15.707101821899414 + Distance: 18.592376708984375 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -1464,9 +1470,9 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.8647973537445068 + Pitch: 0.6897974610328674 Target Frame: base - Yaw: 5.881691932678223 + Yaw: 5.176687240600586 Saved: ~ Window Geometry: AB Wide Angle Front: @@ -1483,10 +1489,10 @@ Window Geometry: collapsed: false F Confidence Overlay Replay: collapsed: false - F Confidence Raw: - collapsed: false F Input Image: collapsed: false + F Learning Mask: + collapsed: false F Trav Over: collapsed: false F Traversability Overlay: @@ -1506,7 +1512,7 @@ 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: From a5abfc97726d44336c54226b0ae09856261bb22c Mon Sep 17 00:00:00 2001 From: Jonas Frey Date: Tue, 5 Sep 2023 10:19:53 +0200 Subject: [PATCH 24/43] New procman --- .../config/procman/robot_dodo.pmd | 25 ++++++++++++------- 1 file changed, 16 insertions(+), 9 deletions(-) diff --git a/wild_visual_navigation_ros/config/procman/robot_dodo.pmd b/wild_visual_navigation_ros/config/procman/robot_dodo.pmd index c2f87fc8..22efa0a4 100644 --- a/wild_visual_navigation_ros/config/procman/robot_dodo.pmd +++ b/wild_visual_navigation_ros/config/procman/robot_dodo.pmd @@ -1,4 +1,4 @@ -group "1.startup" { +group "01.startup" { cmd "1.1.chrony" { exec = "rosrun anymal_rsl_chrony reset_chrony.sh jonfrey dodo d020 false"; host = "localhost"; @@ -25,7 +25,7 @@ group "1.startup" { } } -group "2.start" { +group "02.start" { cmd "2.1.lpc" { exec = "rosrun anymal_dodo_rsl lpc.py"; host = "lpc"; @@ -44,7 +44,7 @@ group "2.start" { } } -group "3.recording" { +group "03.recording" { cmd "3.1.rosbag_record" { exec = "rosservice call /rosbag_record_robot_coordinator/record_bag 'yaml_file: /home/jonfrey/git/diffusion_navigation/diffusion_navigation_ros/config/recording/dodo.yaml' "; host = "localhost"; @@ -71,7 +71,7 @@ group "3.recording" { } } -group "4.monitoring" { +group "04.monitoring" { cmd "4.1.lpc_disk" { exec = "rostopic echo /disk_monitor_lpc/status/disks[1]"; host = "localhost"; @@ -86,7 +86,7 @@ group "4.monitoring" { } } -group "5.network" { +group "05.network" { cmd "5.1.switch_to_access_point" { exec = "rosrun anymal_router_utils router_utils.bash -i 192.168.0.173 -p d020 -m 1"; host = "localhost"; @@ -98,7 +98,7 @@ group "5.network" { } -group "6.disk_free" { +group "06.disk_free" { cmd "6.1.lpc" { exec = "df -h /dev/sda4"; host = "lpc"; @@ -118,7 +118,7 @@ group "6.disk_free" { } -group "7.tools for bags" { +group "07.tools for bags" { cmd "7.1.collect_lpc" { exec = " rsync -rzPav anymal-dodo-lpc:/home/jonfrey/catkin_ws/src/anymal_rsl/anymal_rsl/anymal_rsl_utils/anymal_rsl_recording/anymal_rsl_recording/data /home/jonfrey/mission_data/lpc"; host = "jetson"; @@ -138,7 +138,7 @@ group "7.tools for bags" { } -group "8.npc" { +group "08.npc" { cmd "8.1.alphasense_ptp" { exec = "sudo service phc2sys restart"; host = "npc"; @@ -174,7 +174,7 @@ group "8.npc" { } -group "9.orin" { +group "09.orin" { cmd "9.1.wild_visual_navigation" { exec = "roslaunch wild_visual_navigation_ros robot.launch"; host = "jetson"; @@ -215,6 +215,13 @@ group "12.white_balance" { } } +group "13.lpc" { + cmd "13.1.anymal_msg_converter" { + exec = "rosrun wild_visual_navigation_anymal anymal_msg_converter_node.py"; + host = "lpc"; + } +} + group "x.learning_utils" { cmd "x.1.pause_training" { exec = "rosservice call /wild_visual_navigation_node/pause_learning 'data: true'"; From 5fab91387007fbcc14db717ee61cb4c57dca4c08 Mon Sep 17 00:00:00 2001 From: Jonas Frey Date: Tue, 5 Sep 2023 14:01:32 +0200 Subject: [PATCH 25/43] Change procman script --- .../config/procman/robot_dodo.pmd | 25 ++++++++++++------- 1 file changed, 16 insertions(+), 9 deletions(-) diff --git a/wild_visual_navigation_ros/config/procman/robot_dodo.pmd b/wild_visual_navigation_ros/config/procman/robot_dodo.pmd index c2f87fc8..22efa0a4 100644 --- a/wild_visual_navigation_ros/config/procman/robot_dodo.pmd +++ b/wild_visual_navigation_ros/config/procman/robot_dodo.pmd @@ -1,4 +1,4 @@ -group "1.startup" { +group "01.startup" { cmd "1.1.chrony" { exec = "rosrun anymal_rsl_chrony reset_chrony.sh jonfrey dodo d020 false"; host = "localhost"; @@ -25,7 +25,7 @@ group "1.startup" { } } -group "2.start" { +group "02.start" { cmd "2.1.lpc" { exec = "rosrun anymal_dodo_rsl lpc.py"; host = "lpc"; @@ -44,7 +44,7 @@ group "2.start" { } } -group "3.recording" { +group "03.recording" { cmd "3.1.rosbag_record" { exec = "rosservice call /rosbag_record_robot_coordinator/record_bag 'yaml_file: /home/jonfrey/git/diffusion_navigation/diffusion_navigation_ros/config/recording/dodo.yaml' "; host = "localhost"; @@ -71,7 +71,7 @@ group "3.recording" { } } -group "4.monitoring" { +group "04.monitoring" { cmd "4.1.lpc_disk" { exec = "rostopic echo /disk_monitor_lpc/status/disks[1]"; host = "localhost"; @@ -86,7 +86,7 @@ group "4.monitoring" { } } -group "5.network" { +group "05.network" { cmd "5.1.switch_to_access_point" { exec = "rosrun anymal_router_utils router_utils.bash -i 192.168.0.173 -p d020 -m 1"; host = "localhost"; @@ -98,7 +98,7 @@ group "5.network" { } -group "6.disk_free" { +group "06.disk_free" { cmd "6.1.lpc" { exec = "df -h /dev/sda4"; host = "lpc"; @@ -118,7 +118,7 @@ group "6.disk_free" { } -group "7.tools for bags" { +group "07.tools for bags" { cmd "7.1.collect_lpc" { exec = " rsync -rzPav anymal-dodo-lpc:/home/jonfrey/catkin_ws/src/anymal_rsl/anymal_rsl/anymal_rsl_utils/anymal_rsl_recording/anymal_rsl_recording/data /home/jonfrey/mission_data/lpc"; host = "jetson"; @@ -138,7 +138,7 @@ group "7.tools for bags" { } -group "8.npc" { +group "08.npc" { cmd "8.1.alphasense_ptp" { exec = "sudo service phc2sys restart"; host = "npc"; @@ -174,7 +174,7 @@ group "8.npc" { } -group "9.orin" { +group "09.orin" { cmd "9.1.wild_visual_navigation" { exec = "roslaunch wild_visual_navigation_ros robot.launch"; host = "jetson"; @@ -215,6 +215,13 @@ group "12.white_balance" { } } +group "13.lpc" { + cmd "13.1.anymal_msg_converter" { + exec = "rosrun wild_visual_navigation_anymal anymal_msg_converter_node.py"; + host = "lpc"; + } +} + group "x.learning_utils" { cmd "x.1.pause_training" { exec = "rosservice call /wild_visual_navigation_node/pause_learning 'data: true'"; From 04a58b7ac18f7422707c8ad527bc2d2fed5232a0 Mon Sep 17 00:00:00 2001 From: Jonas Frey Date: Tue, 5 Sep 2023 14:02:18 +0200 Subject: [PATCH 26/43] Add recording yaml --- .../config/recording/dodo.yaml | 141 ++++++++++++++++++ 1 file changed, 141 insertions(+) create mode 100644 wild_visual_navigation_ros/config/recording/dodo.yaml diff --git a/wild_visual_navigation_ros/config/recording/dodo.yaml b/wild_visual_navigation_ros/config/recording/dodo.yaml new file mode 100644 index 00000000..8c65f5f4 --- /dev/null +++ b/wild_visual_navigation_ros/config/recording/dodo.yaml @@ -0,0 +1,141 @@ +lpc: + - /anymal_low_level_controller/actuator_readings + - /anymal_low_level_controller/actuator_readings_extended_throttled + - /chrony_monitor_lpc/status + - /cpu_loupe_lpc/cpu_loupe + - /cpu_monitor_lpc/status + # + ########################################################################### + # Front Upper + - /depth_camera_front_upper/depth/camera_info + - /depth_camera_front_upper/depth/image_rect_raw + # - /depth_camera_front_upper/infra1/image_rect_raw + - /depth_camera_front_upper/point_cloud_self_filtered + # Front Lower + - /depth_camera_front_lower/depth/camera_info + - /depth_camera_front_lower/depth/image_rect_raw + # - /depth_camera_front_lower/infra1/image_rect_raw + - /depth_camera_front_lower/point_cloud_self_filtered + # Rear Lower + - /depth_camera_rear_lower/depth/camera_info + - /depth_camera_rear_lower/depth/image_rect_raw + # - /depth_camera_rear_lower/infra1/image_rect_raw + - /depth_camera_rear_lower/point_cloud_self_filtered + # Rear Upper + - /depth_camera_rear_upper/depth/camera_info + - /depth_camera_rear_upper/depth/image_rect_raw + # - /depth_camera_rear_upper/infra1/image_rect_raw + - /depth_camera_rear_upper/point_cloud_self_filtered + + ############################################################################# + - /sensors/imu + ########################################################################## + - /anyjoy/rcu + - /motion_reference/command_pose + - /motion_reference/command_twist + - /debug_info + - /twist_mux/rcu + - /twist_mux/twist + - /state_estimator/actuator_readings + - /state_estimator/anymal_state + - /state_estimator/contact_force_lf_foot + - /state_estimator/contact_force_lh_foot + - /state_estimator/contact_force_rf_foot + - /state_estimator/contact_force_rh_foot + - /state_estimator/disable_change_notification + - /state_estimator/enable_change_notification + - /state_estimator/force_calibrator_commands + - /state_estimator/imu + - /state_estimator/imu_angular_velocity_bias + - /state_estimator/imu_linear_acceleration_bias + - /state_estimator/init_change_notification + - /state_estimator/joint_states + - /state_estimator/odometry + - /state_estimator/pose_in_odom + - /state_estimator/reset_notification + - /state_estimator/twist + - /state_estimator/twist_throttle + - /state_estimator/zero_velocity_update_notification + ############################################################################# + - /tf + - /tf_static + ############################################################################# + - /disk_monitor_lpc/status + - /distance_tracker/total_distance + - /distance_tracker/display + - /foot_scan + - /memory_monitor_lpc/status + - /path_logger/logged_path + - /path_logger/reverse_path + - /ping_monitor_anymal/markers + - /pdb/battery_state + - /pdb/intrinsic_state + - /pdb/power_state + - /resource_database/reports + - /sensors/battery_voltage + - /soft_emcy_stop +npc: + ########################################################### + - /lidar/packets + - /point_cloud_filter/lidar/point_cloud_filtered + ########################################################### + - /wide_angle_camera_front/image_color_rect/compressed + - /wide_angle_camera_front/camera_info + - /wide_angle_camera_rear/image_color_rect/compressed + - /wide_angle_camera_rear/camera_info + ########################################################### + - /compslam_lio/full_path + - /compslam_lio/odometry + - /compslam_lio/map + - /compslam_lio/map_optimized + ########################################################### + # Right + - /depth_camera_right/depth/camera_info + - /depth_camera_right/depth/image_rect_raw + #- /depth_camera_right/infra1/image_rect_raw + - /depth_camera_right/point_cloud_self_filtered + # Left + - /depth_camera_left/depth/camera_info + - /depth_camera_left/depth/image_rect_raw + # - /depth_camera_left/infra1/image_rect_raw + - /depth_camera_left/point_cloud_self_filtered + ############################################################ + - /anymal/absolute_reference + - /behavior_engine/status + - /chrony_monitor_npc/status + - /clicked_point + - /colorless_uniform_mapper/local_point_cloud + - /transform_aft_mapped_to_init_CORRECTED + - /disk_monitor_npc/status + - /integrated_to_init_CORRECTED + - /local_guidance_path_follower/controller_reference_marker + - /local_guidance_path_manager/received_global_path + - /memory_monitor_npc/status + - /msf_compslam_lio_body_imu/msf_core/odometry + - /network_monitor_npc/status + - /pinger_npc_to_jetson/ping + - /pinger_npc_to_lpc/ping + - /cpu_monitor_npc/status + - /cpu_loupe_npc/cpu_loupe + - /resource_database/reports +jetson: + ############################################################################# + - /elevation_mapping/elevation_map_recordable + - /elevation_mapping/statistics + - /elevation_mapping/elevation_map_raw + ############################################################################# + - /chrony_monitor_jetson/status + - /cpu_loupe_jetson/cpu_loupe + - /disk_monitor_jetson/status + - /gpu_monitor_jetson + - /memory_monitor_jetson/status + - /network_monitor_jetson/status + - /safety_checker/traversabiliy_safety_status + - /safety_checker/footprint_polygon + - /safety_checker/untraversable_polygon + - /pinger_jetson_to_lpc/ping + - /pinger_jetson_to_npc/ping + - /pinger_jetson_to_apc/ping + - /wild_visual_navigation_node/front/image_input + - /wild_visual_navigation_node/front/debug/last_node_image_overlay + - /wild_visual_navigation_visu_traversability/traversability_overlayed \ No newline at end of file From ce6f3b22b4237cce93afe69a8059115d7f896590 Mon Sep 17 00:00:00 2001 From: RobinSchmid7 Date: Wed, 6 Sep 2023 15:10:03 +0200 Subject: [PATCH 27/43] Remove unnecessary param --- .../traversability_estimator/traversability_estimator.py | 1 - .../config/wild_visual_navigation/default.yaml | 3 +-- wild_visual_navigation_ros/launch/replay_launch.launch | 2 +- wild_visual_navigation_ros/scripts/wvn_learning_node.py | 2 -- 4 files changed, 2 insertions(+), 6 deletions(-) diff --git a/wild_visual_navigation/traversability_estimator/traversability_estimator.py b/wild_visual_navigation/traversability_estimator/traversability_estimator.py index e94d55e9..aff57db8 100644 --- a/wild_visual_navigation/traversability_estimator/traversability_estimator.py +++ b/wild_visual_navigation/traversability_estimator/traversability_estimator.py @@ -63,7 +63,6 @@ def __init__( self._params = params self._scale_traversability_threshold = 0 self._anomaly_detection = anomaly_detection - self._vis_training_samples = vis_training_samples if self._scale_traversability: # Use 500 bins for constant memory usuage diff --git a/wild_visual_navigation_ros/config/wild_visual_navigation/default.yaml b/wild_visual_navigation_ros/config/wild_visual_navigation/default.yaml index 3c6f1dfc..058af289 100644 --- a/wild_visual_navigation_ros/config/wild_visual_navigation/default.yaml +++ b/wild_visual_navigation_ros/config/wild_visual_navigation/default.yaml @@ -34,7 +34,6 @@ min_samples_for_training: 5 prediction_per_pixel: True traversability_threshold: 0.55 clip_to_binary: False -vis_training_samples: True # Supervision Generator untraversable_thr: 0.01 @@ -51,7 +50,7 @@ status_thread_rate: 0.5 # hertz # Runtime options device: "cuda" -mode: "online" # check out comments in the class WVNMode +mode: "debug" # check out comments in the class WVNMode colormap: "RdYlBu" print_image_callback_time: false diff --git a/wild_visual_navigation_ros/launch/replay_launch.launch b/wild_visual_navigation_ros/launch/replay_launch.launch index ac7e522a..e922eec3 100644 --- a/wild_visual_navigation_ros/launch/replay_launch.launch +++ b/wild_visual_navigation_ros/launch/replay_launch.launch @@ -3,7 +3,7 @@ - + diff --git a/wild_visual_navigation_ros/scripts/wvn_learning_node.py b/wild_visual_navigation_ros/scripts/wvn_learning_node.py index ad9dfa95..291a97f3 100644 --- a/wild_visual_navigation_ros/scripts/wvn_learning_node.py +++ b/wild_visual_navigation_ros/scripts/wvn_learning_node.py @@ -78,7 +78,6 @@ def __init__(self): patch_size=self.dino_patch_size, scale_traversability=self.scale_traversability, anomaly_detection=self.anomaly_detection, - vis_training_samples=self.vis_training_samples, ) # Initialize traversability generator to process velocity commands @@ -299,7 +298,6 @@ def read_params(self): # Select mode: # debug, online, extract_labels self.mode = WVNMode.from_string(rospy.get_param("~mode", "debug")) self.extraction_store_folder = rospy.get_param("~extraction_store_folder") - self.vis_training_samples = rospy.get_param("~vis_training_samples") # Parse operation modes if self.mode == WVNMode.ONLINE: From e015a7dc08a9cc16cc4c2b128244ef3db2aa40ca Mon Sep 17 00:00:00 2001 From: RobinSchmid7 Date: Wed, 6 Sep 2023 16:04:02 +0200 Subject: [PATCH 28/43] Add missing dep, fix default overlay param, add procman replay, add resize image --- environment.yaml | 3 +++ .../config/procman/replay.pmd | 22 +++++++++++++++++++ .../launch/replay_launch.launch | 9 +++++--- .../scripts/overlay_images.py | 7 ++++-- 4 files changed, 36 insertions(+), 5 deletions(-) create mode 100644 wild_visual_navigation_ros/config/procman/replay.pmd diff --git a/environment.yaml b/environment.yaml index 65cb20f0..fbccc8d1 100644 --- a/environment.yaml +++ b/environment.yaml @@ -47,3 +47,6 @@ dependencies: - git+https://github.com/kornia/kornia#egg=kornia - git+https://github.com/mmattamala/liegroups#egg=liegroups - --editable git+https://github.com/leggedrobotics/stego.git#egg=stego==0.0.1 + - pytictac + - prettytable + - termcolor diff --git a/wild_visual_navigation_ros/config/procman/replay.pmd b/wild_visual_navigation_ros/config/procman/replay.pmd new file mode 100644 index 00000000..c231065b --- /dev/null +++ b/wild_visual_navigation_ros/config/procman/replay.pmd @@ -0,0 +1,22 @@ +group "01.startup" { + cmd "1.1.roscore" { + exec = "roscore"; + host = "localhost"; + } + cmd "1.2.rviz replay" { + exec = "roslaunch wild_visual_navigation_ros replay_launch.launch"; + host = "localhost"; + } + cmd "1.3.feat extractor" { + exec = "/home/rschmid/anaconda3/envs/wvn/bin/python3 /home/rschmid/git/wild_visual_navigation/wild_visual_navigation_ros/scripts/wvn_feature_extractor_node.py"; + host = "localhost"; + } + cmd "1.4.learning node" { + exec = "/home/rschmid/anaconda3/envs/wvn/bin/python3 /home/rschmid/git/wild_visual_navigation/wild_visual_navigation_ros/scripts/wvn_learning_node.py"; + host = "localhost"; + } + cmd "1.5.image overlay" { + exec = "/home/rschmid/anaconda3/envs/wvn/bin/python3 /home/rschmid/git/wild_visual_navigation/wild_visual_navigation_ros/scripts/overlay_images.py _image_sub_topic:=/wild_visual_navigation_node/front/image_input _value_sub_topic:=/wild_visual_navigation_node/front/traversability _image_pub_topic:=traversability_overlayed --nr 0"; + host = "localhost"; + } +} diff --git a/wild_visual_navigation_ros/launch/replay_launch.launch b/wild_visual_navigation_ros/launch/replay_launch.launch index e922eec3..d29a062a 100644 --- a/wild_visual_navigation_ros/launch/replay_launch.launch +++ b/wild_visual_navigation_ros/launch/replay_launch.launch @@ -14,7 +14,9 @@ - + + + @@ -74,7 +76,8 @@ - + + + - \ No newline at end of file diff --git a/wild_visual_navigation_ros/scripts/overlay_images.py b/wild_visual_navigation_ros/scripts/overlay_images.py index 943cfc36..fae09177 100644 --- a/wild_visual_navigation_ros/scripts/overlay_images.py +++ b/wild_visual_navigation_ros/scripts/overlay_images.py @@ -29,8 +29,11 @@ def callback(self, image_msg, trav_msgs): if __name__ == "__main__": print("Stated ImageOverlayNode") try: - nr = "_" + rospy.myargv(argv=sys.argv)[-1].split(" ")[-1] - rospy.init_node(f"wild_visual_navigation_visu{nr}") + nr = "0" + if len(sys.argv) > 1: + nr = rospy.myargv(argv=sys.argv)[-1].split(" ")[-1] + nr_str = "_" + nr + rospy.init_node(f"wild_visual_navigation_visu{nr_str}") except: rospy.init_node("wild_visual_navigation_visu") From 4f72574834c4c76b932a000db0e103419bfe3551 Mon Sep 17 00:00:00 2001 From: RobinSchmid7 Date: Wed, 6 Sep 2023 17:06:49 +0200 Subject: [PATCH 29/43] Fix overlay, fix elevation map semantic layer --- .../anymal_parameters.yaml | 2 +- .../anymal_plugin_config.yaml | 14 ++++---- .../config/procman/replay.pmd | 36 ++++++++----------- .../scripts/overlay_images.py | 12 ++++--- 4 files changed, 30 insertions(+), 34 deletions(-) diff --git a/wild_visual_navigation_ros/config/elevation_mapping_cupy/anymal_parameters.yaml b/wild_visual_navigation_ros/config/elevation_mapping_cupy/anymal_parameters.yaml index b275acf0..7c021547 100644 --- a/wild_visual_navigation_ros/config/elevation_mapping_cupy/anymal_parameters.yaml +++ b/wild_visual_navigation_ros/config/elevation_mapping_cupy/anymal_parameters.yaml @@ -75,7 +75,7 @@ publishers: fps: 5.0 semantic_map_raw: - layers: ['elevation', 'traversability', 'visual_traversability', 'elevation_with_semantic'] + layers: ['elevation', 'traversability', 'visual_traversability'] basic_layers: ['elevation', 'traversability'] fps: 5.0 # elevation_map_recordable: diff --git a/wild_visual_navigation_ros/config/elevation_mapping_cupy/anymal_plugin_config.yaml b/wild_visual_navigation_ros/config/elevation_mapping_cupy/anymal_plugin_config.yaml index 20ef3f98..cb16b79d 100644 --- a/wild_visual_navigation_ros/config/elevation_mapping_cupy/anymal_plugin_config.yaml +++ b/wild_visual_navigation_ros/config/elevation_mapping_cupy/anymal_plugin_config.yaml @@ -29,10 +29,10 @@ inpainting: method: "telea" # telea or ns # Apply inpainting using opencv -elevation_semantic: - enable: True - fill_nan: False - is_height_layer: True - layer_name: "elevation_with_semantic" - extra_params: - method: "telea" \ No newline at end of file +#elevation_semantic: +# enable: True +# fill_nan: False +# is_height_layer: True +# layer_name: "elevation_with_semantic" +# extra_params: +# method: "telea" \ No newline at end of file diff --git a/wild_visual_navigation_ros/config/procman/replay.pmd b/wild_visual_navigation_ros/config/procman/replay.pmd index c231065b..ce2e97cb 100644 --- a/wild_visual_navigation_ros/config/procman/replay.pmd +++ b/wild_visual_navigation_ros/config/procman/replay.pmd @@ -1,22 +1,16 @@ -group "01.startup" { - cmd "1.1.roscore" { - exec = "roscore"; - host = "localhost"; - } - cmd "1.2.rviz replay" { - exec = "roslaunch wild_visual_navigation_ros replay_launch.launch"; - host = "localhost"; - } - cmd "1.3.feat extractor" { - exec = "/home/rschmid/anaconda3/envs/wvn/bin/python3 /home/rschmid/git/wild_visual_navigation/wild_visual_navigation_ros/scripts/wvn_feature_extractor_node.py"; - host = "localhost"; - } - cmd "1.4.learning node" { - exec = "/home/rschmid/anaconda3/envs/wvn/bin/python3 /home/rschmid/git/wild_visual_navigation/wild_visual_navigation_ros/scripts/wvn_learning_node.py"; - host = "localhost"; - } - cmd "1.5.image overlay" { - exec = "/home/rschmid/anaconda3/envs/wvn/bin/python3 /home/rschmid/git/wild_visual_navigation/wild_visual_navigation_ros/scripts/overlay_images.py _image_sub_topic:=/wild_visual_navigation_node/front/image_input _value_sub_topic:=/wild_visual_navigation_node/front/traversability _image_pub_topic:=traversability_overlayed --nr 0"; - host = "localhost"; - } +cmd "1. rviz replay" { + exec = "roslaunch wild_visual_navigation_ros replay_launch.launch"; + host = "localhost"; +} +cmd "2. feat extractor" { + exec = "/home/rschmid/anaconda3/envs/wvn/bin/python3 /home/rschmid/git/wild_visual_navigation/wild_visual_navigation_ros/scripts/wvn_feature_extractor_node.py"; + host = "localhost"; +} +cmd "3. learning node" { + exec = "/home/rschmid/anaconda3/envs/wvn/bin/python3 /home/rschmid/git/wild_visual_navigation/wild_visual_navigation_ros/scripts/wvn_learning_node.py"; + host = "localhost"; +} +cmd "4. image overlay" { + exec = "/home/rschmid/anaconda3/envs/wvn/bin/python3 /home/rschmid/git/wild_visual_navigation/wild_visual_navigation_ros/scripts/overlay_images.py _image_sub_topic:=/wild_visual_navigation_node/front/image_input _value_sub_topic:=/wild_visual_navigation_node/front/traversability _image_pub_topic:=traversability_overlayed"; + host = "localhost"; } diff --git a/wild_visual_navigation_ros/scripts/overlay_images.py b/wild_visual_navigation_ros/scripts/overlay_images.py index fae09177..a12b794d 100644 --- a/wild_visual_navigation_ros/scripts/overlay_images.py +++ b/wild_visual_navigation_ros/scripts/overlay_images.py @@ -29,11 +29,13 @@ def callback(self, image_msg, trav_msgs): if __name__ == "__main__": print("Stated ImageOverlayNode") try: - nr = "0" - if len(sys.argv) > 1: - nr = rospy.myargv(argv=sys.argv)[-1].split(" ")[-1] - nr_str = "_" + nr - rospy.init_node(f"wild_visual_navigation_visu{nr_str}") + args = rospy.myargv(argv=sys.argv) + if "--nr" in args: + nr_index = args.index("--nr") + nr = args[nr_index + 1] + else: + nr = "0" # Handle case when no arg is set + rospy.init_node(f"wild_visual_navigation_visu_{nr}") except: rospy.init_node("wild_visual_navigation_visu") From ecb419116be56a28e98da0e38e94e2a4b9557354 Mon Sep 17 00:00:00 2001 From: RobinSchmid7 Date: Thu, 7 Sep 2023 09:36:43 +0200 Subject: [PATCH 30/43] Add new rviz for dodo --- wild_visual_navigation/visu/visualizer.py | 3 +- .../config/rviz/anymal_dodo.rviz | 1498 +++++++++++++++++ .../launch/replay_launch.launch | 2 +- wild_visual_navigation_ros/launch/view.launch | 2 +- 4 files changed, 1502 insertions(+), 3 deletions(-) create mode 100644 wild_visual_navigation_ros/config/rviz/anymal_dodo.rviz diff --git a/wild_visual_navigation/visu/visualizer.py b/wild_visual_navigation/visu/visualizer.py index 4c136098..e6422f7a 100644 --- a/wild_visual_navigation/visu/visualizer.py +++ b/wild_visual_navigation/visu/visualizer.py @@ -358,8 +358,9 @@ def plot_detectron_classification( if kwargs.get("cmap", None): cmap = kwargs["cmap"] else: + c = 0.1 cmap = cm.get_cmap("RdYlBu", 256) - cmap = np.concatenate([cmap(np.linspace(0, 0.3, 128)), cmap(np.linspace(0.7, 1.0, 128))]) + cmap = np.concatenate([cmap(np.linspace(0, c, 128)), cmap(np.linspace(1 - c, 1.0, 128))]) cmap = torch.from_numpy(cmap).to(seg)[:, :3] img = self.plot_image(img, not_log=True) diff --git a/wild_visual_navigation_ros/config/rviz/anymal_dodo.rviz b/wild_visual_navigation_ros/config/rviz/anymal_dodo.rviz new file mode 100644 index 00000000..b23f3d04 --- /dev/null +++ b/wild_visual_navigation_ros/config/rviz/anymal_dodo.rviz @@ -0,0 +1,1498 @@ +Panels: + - Class: rviz/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /TF1/Tree1 + - /Wild Visual Navigation1 + - /Wild Visual Navigation1/Cameras1 + - /Wild Visual Navigation1/Prediction1 + - /Wild Visual Navigation1/Prediction1/Traversability Raw1 + - /Wild Visual Navigation1/Prediction1/Learning Mask1 + - /Wild Visual Navigation1/Prediction1/F Traversability Overlay1 + - /Wild Visual Navigation1/Prediction1/F Confidence Overlay1 + - /Elevation Map WIFI1/Local Planner (SDF)1 + - /Elevation Map RAW1 + Splitter Ratio: 0.5789473652839661 + Tree Height: 685 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Name: Time + SyncMode: 0 + SyncSource: velodyne +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 20 + Reference Frame: + Value: true + - Class: rviz/TF + Enabled: false + Frame Timeout: 1000 + Frames: + All Enabled: false + Marker Alpha: 1 + Marker Scale: 0.5 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + {} + Update Interval: 0 + Value: false + - Class: rviz/Group + Displays: + - Angle Tolerance: 0.10000000149011612 + Class: rviz/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: false + Keep: 200 + Name: Odometry + Position Tolerance: 1 + Queue Size: 10 + Shape: + Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Color: 255; 25; 0 + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Value: Arrow + Topic: /loam/odometry + Unreliable: false + Value: false + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + LF_FOOT: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + LF_HAA_drive: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + LF_HFE_drive: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + LF_HFE_output: + Alpha: 1 + Show Axes: false + Show Trail: false + LF_HIP: + Alpha: 1 + Show Axes: false + Show Trail: false + LF_KFE_drive: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + LF_SHANK: + Alpha: 1 + Show Axes: false + Show Trail: false + LF_THIGH: + Alpha: 1 + Show Axes: false + Show Trail: false + LF_hip_fixed: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + LF_shank_fixed: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + LF_thigh_fixed: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + LH_FOOT: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + LH_HAA_drive: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + LH_HFE_drive: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + LH_HFE_output: + Alpha: 1 + Show Axes: false + Show Trail: false + LH_HIP: + Alpha: 1 + Show Axes: false + Show Trail: false + LH_KFE_drive: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + LH_SHANK: + Alpha: 1 + Show Axes: false + Show Trail: false + LH_THIGH: + Alpha: 1 + Show Axes: false + Show Trail: false + LH_hip_fixed: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + LH_shank_fixed: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + LH_thigh_fixed: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Link Tree Style: Links in Alphabetic Order + RF_FOOT: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RF_HAA_drive: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RF_HFE_drive: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RF_HFE_output: + Alpha: 1 + Show Axes: false + Show Trail: false + RF_HIP: + Alpha: 1 + Show Axes: false + Show Trail: false + RF_KFE_drive: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RF_SHANK: + Alpha: 1 + Show Axes: false + Show Trail: false + RF_THIGH: + Alpha: 1 + Show Axes: false + Show Trail: false + RF_hip_fixed: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RF_shank_fixed: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RF_thigh_fixed: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RH_FOOT: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RH_HAA_drive: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RH_HFE_drive: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RH_HFE_output: + Alpha: 1 + Show Axes: false + Show Trail: false + RH_HIP: + Alpha: 1 + Show Axes: false + Show Trail: false + RH_KFE_drive: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RH_SHANK: + Alpha: 1 + Show Axes: false + Show Trail: false + RH_THIGH: + Alpha: 1 + Show Axes: false + Show Trail: false + RH_hip_fixed: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RH_shank_fixed: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RH_thigh_fixed: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_inertia: + Alpha: 1 + Show Axes: false + Show Trail: false + battery: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + body_top: + Alpha: 1 + Show Axes: false + Show Trail: false + bottom_shell: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + depth_camera_front_lower_camera: + Alpha: 1 + Show Axes: false + Show Trail: false + depth_camera_front_lower_camera_parent: + Alpha: 1 + Show Axes: false + Show Trail: false + depth_camera_front_upper_camera: + Alpha: 1 + Show Axes: false + Show Trail: false + depth_camera_front_upper_camera_parent: + Alpha: 1 + Show Axes: false + Show Trail: false + depth_camera_left_camera: + Alpha: 1 + Show Axes: false + Show Trail: false + depth_camera_left_camera_parent: + Alpha: 1 + Show Axes: false + Show Trail: false + depth_camera_rear_lower_camera: + Alpha: 1 + Show Axes: false + Show Trail: false + depth_camera_rear_lower_camera_parent: + Alpha: 1 + Show Axes: false + Show Trail: false + depth_camera_rear_upper_camera: + Alpha: 1 + Show Axes: false + Show Trail: false + depth_camera_rear_upper_camera_parent: + Alpha: 1 + Show Axes: false + Show Trail: false + depth_camera_right_camera: + Alpha: 1 + Show Axes: false + Show Trail: false + depth_camera_right_camera_parent: + Alpha: 1 + Show Axes: false + Show Trail: false + docking_socket: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + face_front: + Alpha: 1 + Show Axes: false + Show Trail: false + face_rear: + Alpha: 1 + Show Axes: false + Show Trail: false + face_shell_front: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + face_shell_rear: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + hatch_shell: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + hbc_receiver: + Alpha: 1 + Show Axes: false + Show Trail: false + imu_link: + Alpha: 1 + Show Axes: false + Show Trail: false + lidar: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lidar_parent: + Alpha: 1 + Show Axes: false + Show Trail: false + top_shell: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wide_angle_camera_front_camera: + Alpha: 1 + Show Axes: false + Show Trail: false + wide_angle_camera_front_camera_parent: + Alpha: 1 + Show Axes: false + Show Trail: false + wide_angle_camera_rear_camera: + Alpha: 1 + Show Axes: false + Show Trail: false + wide_angle_camera_rear_camera_parent: + Alpha: 1 + Show Axes: false + Show Trail: false + Name: RobotModel + Robot Description: anymal_description + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + Enabled: true + Name: Robot Info + - Class: rviz/Group + Displays: + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /wild_visual_navigation_node/graph_footprints + Name: Footprint + Namespaces: + footprints: true + Queue Size: 100 + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 40; 87; 244 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.029999999329447746 + Name: Proprio Graph + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Queue Size: 10 + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /wild_visual_navigation_node/proprioceptive_graph + Unreliable: false + Value: true + Enabled: true + Name: Self Supervision + - Class: rviz/Group + Displays: + - Class: rviz/Image + Enabled: true + Image Topic: /wild_visual_navigation_node/front/debug/last_image_traversability + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Traversability + 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_image_confidence + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Uncertainty + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 136; 138; 133 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.10000000149011612 + Line Style: Billboards + Line Width: 0.05000000074505806 + Name: Mission Graph + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: Axes + Queue Size: 10 + Radius: 0.009999999776482582 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /wild_visual_navigation_node/mission_graph + Unreliable: false + Value: true + - Class: rviz/Image + Enabled: true + Image Topic: /wild_visual_navigation_node/front/debug/last_node_image_mask + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Reprojected Path + 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_labeled + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Supervision Signal + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: true + - Class: rviz/Image + Enabled: true + Image Topic: "" + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Placeholder + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: true + Enabled: false + Name: Self Supervision (Debug) + - Class: rviz/Group + Displays: + - Class: rviz/Image + Enabled: false + Image Topic: /wide_angle_camera_front/image_color + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: AB Wide Angle Front + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + - Class: rviz/Image + Enabled: false + Image Topic: "" + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Placeholder + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + Enabled: true + Name: Cameras + - Class: rviz/Group + Displays: + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 21.55472183227539 + Min Value: -7.581119537353516 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 239; 41; 41 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: rear-bpearl + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Squares + Topic: /robot_self_filter/bpearl_rear/point_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 22.831743240356445 + Min Value: -4.732121467590332 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 239; 41; 41 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: front-bpearl + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Squares + Topic: /robot_self_filter/bpearl_front/point_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 21.55472183227539 + Min Value: -7.581119537353516 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 239; 41; 41 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: realsense-front + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Squares + Topic: /depth_camera_front/point_cloud_self_filtered + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 21.55472183227539 + Min Value: -7.581119537353516 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 239; 41; 41 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: realsense-rear + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Squares + Topic: /depth_camera_rear/point_cloud_self_filtered + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 21.55472183227539 + Min Value: -7.581119537353516 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 239; 41; 41 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: realsense-left + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Squares + Topic: /depth_camera_left/point_cloud_self_filtered + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 21.55472183227539 + Min Value: -7.581119537353516 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 239; 41; 41 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: realsense-right + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Squares + Topic: /depth_camera_right/point_cloud_self_filtered + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 15.77271556854248 + Min Value: -1.853008508682251 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 239; 41; 41 + Color Transformer: AxisColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: velodyne + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.05000000074505806 + Style: Squares + Topic: /point_cloud_filter/lidar/point_cloud_filtered + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Depth Sensors + - Class: rviz/Group + Displays: + - Class: rviz/Image + Enabled: true + Image Topic: /wild_visual_navigation_node/front/image_input + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Input Image + 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/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 + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: F Traversability Overlay Replay + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + - Class: rviz/Image + Enabled: false + Image Topic: /wild_visual_navigation_visu_confidence/confidence_overlayed + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: F Confidence 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_1/confidence_overlayed + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: F Confidence Overlay Replay + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + - Class: rviz/Image + Enabled: true + Image Topic: /wild_visual_navigation_visu_0/traversability_overlayed + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Traversability Overlay + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: true + Enabled: true + Name: Prediction + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /field_local_planner/real_carrot + Name: Goal (carrot) + Namespaces: + {} + Queue Size: 100 + Value: true + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Class: rviz/PoseWithCovariance + Color: 255; 25; 0 + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: true + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Name: Goal (pose) + Queue Size: 10 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Shape: Arrow + Topic: /field_local_planner/current_goal + Unreliable: false + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: false + Class: grid_map_rviz_plugin/GridMap + Color: 200; 200; 200 + Color Layer: geodesic + Color Transformer: IntensityLayer + ColorMap: plasma + Enabled: false + Grid Cell Decimation: 1 + Grid Line Thickness: 0.10000000149011612 + Height Layer: elevation_inpainted + Height Transformer: "" + History Length: 1 + Invert ColorMap: true + Max Color: 255; 255; 255 + Max Intensity: 5 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Geodesic Field (GDF) + Show Grid Lines: true + Topic: /elevation_mapping/elevation_map_wifi + Unreliable: false + Use ColorMap: true + Value: false + Enabled: true + Name: Local planner + - Class: rviz/Group + Displays: + - Class: rviz/Image + Enabled: false + Image Topic: /wild_visual_navigation_node/left/image_input + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: L Input Image + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + - Class: rviz/Image + Enabled: false + Image Topic: /wild_visual_navigation_node/left/traversability + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: L Traversability Raw + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + - Class: rviz/Image + Enabled: false + Image Topic: /wild_visual_navigation_node/left/confidence + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: L Confidence Raw + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + - Class: rviz/Image + Enabled: false + Image Topic: /wild_visual_navigation_node/right/image_input + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: R Input Image + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + - Class: rviz/Image + Enabled: false + Image Topic: /wild_visual_navigation_node/right/traversability + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: R Traversability Raw + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + - Class: rviz/Image + Enabled: false + Image Topic: /wild_visual_navigation_node/right/confidence + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: R Confidence Raw + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: false + Enabled: false + Name: LR Prediction + Enabled: true + Name: Wild Visual Navigation + - Class: rviz/Group + Displays: + - Alpha: 1 + Autocompute Intensity Bounds: true + Class: grid_map_rviz_plugin/GridMap + Color: 200; 200; 200 + Color Layer: elevation + Color Transformer: IntensityLayer + ColorMap: terrain + Enabled: false + Grid Cell Decimation: 1 + Grid Line Thickness: 0.10000000149011612 + Height Layer: elevation + Height Transformer: "" + History Length: 1 + Invert ColorMap: true + Max Color: 255; 255; 255 + Max Intensity: 1 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Elevation + Show Grid Lines: true + Topic: /elevation_mapping/elevation_map_wifi + Unreliable: false + Use ColorMap: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Class: grid_map_rviz_plugin/GridMap + Color: 200; 200; 200 + Color Layer: elevation_inpainted + Color Transformer: IntensityLayer + ColorMap: terrain + Enabled: false + Grid Cell Decimation: 1 + Grid Line Thickness: 0.10000000149011612 + Height Layer: elevation_inpainted + Height Transformer: "" + History Length: 1 + Invert ColorMap: true + Max Color: 255; 255; 255 + Max Intensity: 1 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Elevation (Inpainted) + Show Grid Lines: true + Topic: /elevation_mapping/elevation_map_wifi + Unreliable: false + Use ColorMap: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: false + Class: grid_map_rviz_plugin/GridMap + Color: 200; 200; 200 + Color Layer: geodesic + Color Transformer: IntensityLayer + ColorMap: plasma + Enabled: false + Grid Cell Decimation: 1 + Grid Line Thickness: 0.10000000149011612 + Height Layer: elevation_inpainted + Height Transformer: "" + History Length: 1 + Invert ColorMap: true + Max Color: 255; 255; 255 + Max Intensity: 5 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Local Planner (GDF) + Show Grid Lines: true + Topic: /elevation_mapping/elevation_map_wifi + Unreliable: false + Use ColorMap: true + Value: false + - Alpha: 0.5 + Autocompute Intensity Bounds: false + Class: grid_map_rviz_plugin/GridMap + Color: 200; 200; 200 + Color Layer: sdf + Color Transformer: GridMapLayer + ColorMap: turbo + Enabled: true + Grid Cell Decimation: 1 + Grid Line Thickness: 0.10000000149011612 + Height Layer: elevation + Height Transformer: GridMapLayer + History Length: 1 + Invert ColorMap: true + Max Color: 255; 255; 255 + Max Intensity: 2 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Local Planner (SDF) + Show Grid Lines: false + Topic: /elevation_mapping/elevation_map_wifi + Unreliable: false + Use ColorMap: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Class: grid_map_rviz_plugin/GridMap + Color: 200; 200; 200 + Color Layer: rgb_image + Color Transformer: ColorLayer + ColorMap: terrain + Enabled: false + Grid Cell Decimation: 1 + Grid Line Thickness: 0.10000000149011612 + Height Layer: elevation + Height Transformer: "" + History Length: 1 + Invert ColorMap: true + Max Color: 255; 255; 255 + Max Intensity: 1 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: RGB + Show Grid Lines: true + Topic: /elevation_mapping/semantic_map_raw + Unreliable: false + Use ColorMap: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Class: grid_map_rviz_plugin/GridMap + Color: 200; 200; 200 + Color Layer: visual_traversability_inpainted + Color Transformer: GridMapLayer + ColorMap: coolwarm + Enabled: false + Grid Cell Decimation: 1 + Grid Line Thickness: 0.10000000149011612 + Height Layer: elevation_inpainted + Height Transformer: GridMapLayer + History Length: 1 + Invert ColorMap: true + Max Color: 255; 255; 255 + Max Intensity: 10 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Visual Traversability Inpainted + Show Grid Lines: true + Topic: /elevation_mapping/elevation_map_wifi + Unreliable: false + Use ColorMap: true + Value: false + Enabled: false + Name: Elevation Map WIFI + - Class: rviz/Group + Displays: + - Alpha: 1 + Autocompute Intensity Bounds: true + Class: grid_map_rviz_plugin/GridMap + Color: 200; 200; 200 + Color Layer: elevation + Color Transformer: GridMapLayer + ColorMap: terrain + Enabled: false + Grid Cell Decimation: 1 + Grid Line Thickness: 0.10000000149011612 + Height Layer: elevation + Height Transformer: GridMapLayer + History Length: 1 + Invert ColorMap: false + Max Color: 255; 255; 255 + Max Intensity: 10 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Elevation + Show Grid Lines: true + Topic: /elevation_mapping/semantic_map_raw + Unreliable: false + Use ColorMap: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: false + Class: grid_map_rviz_plugin/GridMap + Color: 200; 200; 200 + Color Layer: geodesic + Color Transformer: IntensityLayer + ColorMap: plasma + Enabled: false + Grid Cell Decimation: 1 + Grid Line Thickness: 0.10000000149011612 + Height Layer: elevation_inpainted + Height Transformer: "" + History Length: 1 + Invert ColorMap: true + Max Color: 255; 255; 255 + Max Intensity: 5 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Local Planner (GDF) + Show Grid Lines: true + Topic: /elevation_mapping/elevation_map_wifi + Unreliable: false + Use ColorMap: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Class: grid_map_rviz_plugin/GridMap + Color: 200; 200; 200 + Color Layer: elevation_inpainted + Color Transformer: IntensityLayer + ColorMap: terrain + Enabled: false + Grid Cell Decimation: 1 + Grid Line Thickness: 0.10000000149011612 + Height Layer: elevation_inpainted + Height Transformer: "" + History Length: 1 + Invert ColorMap: true + Max Color: 255; 255; 255 + Max Intensity: 1 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Elevation (Inpainted) + Show Grid Lines: true + Topic: /elevation_mapping/elevation_map_wifi + Unreliable: false + Use ColorMap: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: false + Class: grid_map_rviz_plugin/GridMap + Color: 200; 200; 200 + Color Layer: sdf + Color Transformer: IntensityLayer + ColorMap: turbo + Enabled: false + Grid Cell Decimation: 1 + Grid Line Thickness: 0.10000000149011612 + Height Layer: elevation + Height Transformer: Layer + History Length: 1 + Invert ColorMap: true + Max Color: 255; 255; 255 + Max Intensity: 2 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Local Planner (SDF) + Show Grid Lines: true + Topic: /elevation_mapping/elevation_map_wifi + Unreliable: false + Use ColorMap: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Class: grid_map_rviz_plugin/GridMap + Color: 200; 200; 200 + Color Layer: rgb_image + Color Transformer: ColorLayer + ColorMap: terrain + Enabled: false + Grid Cell Decimation: 1 + Grid Line Thickness: 0.10000000149011612 + Height Layer: elevation + Height Transformer: "" + History Length: 1 + Invert ColorMap: true + Max Color: 255; 255; 255 + Max Intensity: 1 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: RGB + Show Grid Lines: true + Topic: /elevation_mapping/semantic_map_raw + Unreliable: false + Use ColorMap: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Class: grid_map_rviz_plugin/GridMap + Color: 200; 200; 200 + Color Layer: visual_traversability + Color Transformer: "" + ColorMap: coolwarm + Enabled: true + Grid Cell Decimation: 1 + Grid Line Thickness: 0.10000000149011612 + Height Layer: elevation_with_semantic + Height Transformer: GridMapLayer + History Length: 1 + Invert ColorMap: true + Max Color: 255; 255; 255 + Max Intensity: 10 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Visual Traversability + Show Grid Lines: true + Topic: /elevation_mapping/semantic_map_raw + Unreliable: false + Use ColorMap: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Class: grid_map_rviz_plugin/GridMap + Color: 200; 200; 200 + Color Layer: traversability + Color Transformer: "" + ColorMap: coolwarm + Enabled: false + Grid Cell Decimation: 1 + Grid Line Thickness: 0.10000000149011612 + Height Layer: elevation + Height Transformer: GridMapLayer + History Length: 1 + Invert ColorMap: true + Max Color: 255; 255; 255 + Max Intensity: 10 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Geom Traversability + Show Grid Lines: true + Topic: /elevation_mapping/elevation_map_raw + Unreliable: false + Use ColorMap: true + Value: false + Enabled: true + Name: Elevation Map RAW + Enabled: true + Global Options: + Background Color: 255; 255; 255 + Default Light: true + Fixed Frame: base + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/ThirdPersonFollower + Distance: 15.038888931274414 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: 1.4857360124588013 + Y: 1.2019472122192383 + Z: -5.593325614929199 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.794797420501709 + Target Frame: base + Yaw: 5.0866851806640625 + Saved: ~ +Window Geometry: + AB Wide Angle Front: + collapsed: false + Displays: + collapsed: true + F Confidence Overlay: + collapsed: false + F Confidence Overlay Replay: + collapsed: false + F Traversability Overlay: + collapsed: false + F Traversability Overlay Replay: + collapsed: false + Height: 1376 + Hide Left Dock: true + Hide Right Dock: true + Input Image: + collapsed: false + L Confidence Raw: + collapsed: false + L Input Image: + collapsed: false + L Traversability Raw: + collapsed: false + Learning Mask: + collapsed: false + Placeholder: + collapsed: false + QMainWindow State: 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 + R Confidence Raw: + collapsed: false + R Input Image: + collapsed: false + R Traversability Raw: + collapsed: false + Reprojected Path: + collapsed: false + Selection: + collapsed: false + Supervision Signal: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Traversability: + collapsed: false + Traversability Overlay: + collapsed: false + Traversability Raw: + collapsed: false + Uncertainty: + collapsed: false + Views: + collapsed: true + Width: 2488 + X: 72 + Y: 27 diff --git a/wild_visual_navigation_ros/launch/replay_launch.launch b/wild_visual_navigation_ros/launch/replay_launch.launch index d29a062a..10f92f0b 100644 --- a/wild_visual_navigation_ros/launch/replay_launch.launch +++ b/wild_visual_navigation_ros/launch/replay_launch.launch @@ -2,7 +2,7 @@ - + diff --git a/wild_visual_navigation_ros/launch/view.launch b/wild_visual_navigation_ros/launch/view.launch index 6fd0912e..837bb4d9 100644 --- a/wild_visual_navigation_ros/launch/view.launch +++ b/wild_visual_navigation_ros/launch/view.launch @@ -1,5 +1,5 @@ - + From 498d18ffe0feb7f1d7eafd338ca8896693b3c53a Mon Sep 17 00:00:00 2001 From: RobinSchmid7 Date: Thu, 7 Sep 2023 12:59:55 +0200 Subject: [PATCH 31/43] Add semantic elevation map for anymal D config --- wild_visual_navigation/visu/visualizer.py | 7 +++--- .../anymal_plugin_config.yaml | 14 +++++------ .../anymal_sensor_parameter.yaml | 25 ++++++++++++++----- 3 files changed, 30 insertions(+), 16 deletions(-) diff --git a/wild_visual_navigation/visu/visualizer.py b/wild_visual_navigation/visu/visualizer.py index e6422f7a..47bac9c8 100644 --- a/wild_visual_navigation/visu/visualizer.py +++ b/wild_visual_navigation/visu/visualizer.py @@ -358,9 +358,10 @@ def plot_detectron_classification( if kwargs.get("cmap", None): cmap = kwargs["cmap"] else: - c = 0.1 - cmap = cm.get_cmap("RdYlBu", 256) - cmap = np.concatenate([cmap(np.linspace(0, c, 128)), cmap(np.linspace(1 - c, 1.0, 128))]) + s = 0.2 # If bigger, get more fine-grained green, if smaller get more fine-grained red + cmap = cm.get_cmap("RdYlGn", 256) + # cmap = cm.get_cmap("RdBu", 256) + cmap = np.concatenate([cmap(np.linspace(0, s, 128)), cmap(np.linspace(1 - s, 1.0, 128))]) # Stretch the colormap cmap = torch.from_numpy(cmap).to(seg)[:, :3] img = self.plot_image(img, not_log=True) diff --git a/wild_visual_navigation_ros/config/elevation_mapping_cupy/anymal_plugin_config.yaml b/wild_visual_navigation_ros/config/elevation_mapping_cupy/anymal_plugin_config.yaml index cb16b79d..20ef3f98 100644 --- a/wild_visual_navigation_ros/config/elevation_mapping_cupy/anymal_plugin_config.yaml +++ b/wild_visual_navigation_ros/config/elevation_mapping_cupy/anymal_plugin_config.yaml @@ -29,10 +29,10 @@ inpainting: method: "telea" # telea or ns # Apply inpainting using opencv -#elevation_semantic: -# enable: True -# fill_nan: False -# is_height_layer: True -# layer_name: "elevation_with_semantic" -# extra_params: -# method: "telea" \ No newline at end of file +elevation_semantic: + enable: True + fill_nan: False + is_height_layer: True + layer_name: "elevation_with_semantic" + extra_params: + method: "telea" \ No newline at end of file diff --git a/wild_visual_navigation_ros/config/elevation_mapping_cupy/anymal_sensor_parameter.yaml b/wild_visual_navigation_ros/config/elevation_mapping_cupy/anymal_sensor_parameter.yaml index c16692b1..20dff807 100644 --- a/wild_visual_navigation_ros/config/elevation_mapping_cupy/anymal_sensor_parameter.yaml +++ b/wild_visual_navigation_ros/config/elevation_mapping_cupy/anymal_sensor_parameter.yaml @@ -80,17 +80,30 @@ subscribers: # channels: ["visual_confidence"] # data_type: image + # For Anymal C +# front_depth: +# channels: [] +# fusion: [] +# topic_name: /depth_camera_front/point_cloud_self_filtered +# data_type: pointcloud +# +# rear_depth: +# channels: [] +# fusion: [] +# topic_name: /depth_camera_rear/point_cloud_self_filtered +# data_type: pointcloud + # For Anymal D front_depth: - channels: [] - fusion: [] - topic_name: /depth_camera_front/point_cloud_self_filtered + channels: [ ] + fusion: [ ] + topic_name: /depth_camera_front_upper/point_cloud_self_filtered data_type: pointcloud rear_depth: - channels: [] - fusion: [] - topic_name: /depth_camera_rear/point_cloud_self_filtered + channels: [ ] + fusion: [ ] + topic_name: /depth_camera_rear_upper/point_cloud_self_filtered data_type: pointcloud left_depth: From 4430fd85df85ca533bcc112c5df63175c3ed980b Mon Sep 17 00:00:00 2001 From: RobinSchmid7 Date: Thu, 7 Sep 2023 13:00:56 +0200 Subject: [PATCH 32/43] Add semantic elevation map for anymal D config --- .../config/elevation_mapping_cupy/anymal_parameters.yaml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/wild_visual_navigation_ros/config/elevation_mapping_cupy/anymal_parameters.yaml b/wild_visual_navigation_ros/config/elevation_mapping_cupy/anymal_parameters.yaml index 7c021547..7c49b6f5 100644 --- a/wild_visual_navigation_ros/config/elevation_mapping_cupy/anymal_parameters.yaml +++ b/wild_visual_navigation_ros/config/elevation_mapping_cupy/anymal_parameters.yaml @@ -75,7 +75,8 @@ publishers: fps: 5.0 semantic_map_raw: - layers: ['elevation', 'traversability', 'visual_traversability'] + layers: ['elevation', 'traversability', 'visual_traversability', "elevation_semantic"] +# layers: [ 'elevation', 'traversability', 'visual_traversability'] basic_layers: ['elevation', 'traversability'] fps: 5.0 # elevation_map_recordable: From 4f2d0d6751c633c5502f90de93f2ccec32e20d54 Mon Sep 17 00:00:00 2001 From: RobinSchmid7 Date: Thu, 7 Sep 2023 16:09:39 +0200 Subject: [PATCH 33/43] Changed rviz --- .../anymal_parameters.yaml | 2 +- .../config/rviz/anymal_dodo.rviz | 70 +++++++++---------- .../launch/replay_launch.launch | 2 +- 3 files changed, 35 insertions(+), 39 deletions(-) diff --git a/wild_visual_navigation_ros/config/elevation_mapping_cupy/anymal_parameters.yaml b/wild_visual_navigation_ros/config/elevation_mapping_cupy/anymal_parameters.yaml index 7c49b6f5..e43278b9 100644 --- a/wild_visual_navigation_ros/config/elevation_mapping_cupy/anymal_parameters.yaml +++ b/wild_visual_navigation_ros/config/elevation_mapping_cupy/anymal_parameters.yaml @@ -75,7 +75,7 @@ publishers: fps: 5.0 semantic_map_raw: - layers: ['elevation', 'traversability', 'visual_traversability', "elevation_semantic"] + layers: ['elevation', 'traversability', 'visual_traversability', 'elevation_semantic'] # layers: [ 'elevation', 'traversability', 'visual_traversability'] basic_layers: ['elevation', 'traversability'] fps: 5.0 diff --git a/wild_visual_navigation_ros/config/rviz/anymal_dodo.rviz b/wild_visual_navigation_ros/config/rviz/anymal_dodo.rviz index b23f3d04..137174a1 100644 --- a/wild_visual_navigation_ros/config/rviz/anymal_dodo.rviz +++ b/wild_visual_navigation_ros/config/rviz/anymal_dodo.rviz @@ -7,14 +7,10 @@ Panels: - /Global Options1 - /TF1/Tree1 - /Wild Visual Navigation1 - - /Wild Visual Navigation1/Cameras1 - - /Wild Visual Navigation1/Prediction1 - - /Wild Visual Navigation1/Prediction1/Traversability Raw1 - - /Wild Visual Navigation1/Prediction1/Learning Mask1 - - /Wild Visual Navigation1/Prediction1/F Traversability Overlay1 - - /Wild Visual Navigation1/Prediction1/F Confidence Overlay1 + - /Wild Visual Navigation1/Depth Sensors1/realsense-front1 - /Elevation Map WIFI1/Local Planner (SDF)1 - /Elevation Map RAW1 + - /Elevation Map RAW1/Visual Traversability1 Splitter Ratio: 0.5789473652839661 Tree Height: 685 - Class: rviz/Selection @@ -34,7 +30,7 @@ Panels: - Class: rviz/Time Name: Time SyncMode: 0 - SyncSource: velodyne + SyncSource: Traversability Raw Preferences: PromptSaveOnExit: true Toolbars: @@ -476,13 +472,13 @@ Visualization Manager: - Class: rviz/Group Displays: - Class: rviz/Marker - Enabled: true + Enabled: false Marker Topic: /wild_visual_navigation_node/graph_footprints Name: Footprint Namespaces: - footprints: true + {} Queue Size: 100 - Value: true + Value: false - Alpha: 1 Buffer Length: 1 Class: rviz/Path @@ -706,7 +702,7 @@ Visualization Manager: Size (Pixels): 3 Size (m): 0.009999999776482582 Style: Squares - Topic: /depth_camera_front/point_cloud_self_filtered + Topic: /depth_camera_front_upper/point_cloud_self_filtered Unreliable: false Use Fixed Frame: true Use rainbow: true @@ -798,8 +794,8 @@ Visualization Manager: - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: 15.77271556854248 - Min Value: -1.853008508682251 + Max Value: 15.884197235107422 + Min Value: -2.124349355697632 Value: true Axis: Z Channel Name: intensity @@ -807,7 +803,7 @@ Visualization Manager: Color: 239; 41; 41 Color Transformer: AxisColor Decay Time: 0 - Enabled: true + Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 Min Color: 0; 0; 0 @@ -822,7 +818,7 @@ Visualization Manager: Unreliable: false Use Fixed Frame: true Use rainbow: true - Value: true + Value: false Enabled: true Name: Depth Sensors - Class: rviz/Group @@ -995,7 +991,7 @@ Visualization Manager: - Class: rviz/Group Displays: - Class: rviz/Image - Enabled: false + Enabled: true Image Topic: /wild_visual_navigation_node/left/image_input Max Value: 1 Median window: 5 @@ -1005,9 +1001,9 @@ Visualization Manager: Queue Size: 2 Transport Hint: raw Unreliable: false - Value: false + Value: true - Class: rviz/Image - Enabled: false + Enabled: true Image Topic: /wild_visual_navigation_node/left/traversability Max Value: 1 Median window: 5 @@ -1017,9 +1013,9 @@ Visualization Manager: Queue Size: 2 Transport Hint: raw Unreliable: false - Value: false + Value: true - Class: rviz/Image - Enabled: false + Enabled: true Image Topic: /wild_visual_navigation_node/left/confidence Max Value: 1 Median window: 5 @@ -1029,9 +1025,9 @@ Visualization Manager: Queue Size: 2 Transport Hint: raw Unreliable: false - Value: false + Value: true - Class: rviz/Image - Enabled: false + Enabled: true Image Topic: /wild_visual_navigation_node/right/image_input Max Value: 1 Median window: 5 @@ -1041,9 +1037,9 @@ Visualization Manager: Queue Size: 2 Transport Hint: raw Unreliable: false - Value: false + Value: true - Class: rviz/Image - Enabled: false + Enabled: true Image Topic: /wild_visual_navigation_node/right/traversability Max Value: 1 Median window: 5 @@ -1053,9 +1049,9 @@ Visualization Manager: Queue Size: 2 Transport Hint: raw Unreliable: false - Value: false + Value: true - Class: rviz/Image - Enabled: false + Enabled: true Image Topic: /wild_visual_navigation_node/right/confidence Max Value: 1 Median window: 5 @@ -1065,7 +1061,7 @@ Visualization Manager: Queue Size: 2 Transport Hint: raw Unreliable: false - Value: false + Value: true Enabled: false Name: LR Prediction Enabled: true @@ -1350,10 +1346,10 @@ Visualization Manager: Enabled: true Grid Cell Decimation: 1 Grid Line Thickness: 0.10000000149011612 - Height Layer: elevation_with_semantic + Height Layer: elevation_semantic Height Transformer: GridMapLayer History Length: 1 - Invert ColorMap: true + Invert ColorMap: false Max Color: 255; 255; 255 Max Intensity: 10 Min Color: 0; 0; 0 @@ -1418,7 +1414,7 @@ Visualization Manager: Views: Current: Class: rviz/ThirdPersonFollower - Distance: 15.038888931274414 + Distance: 12.582637786865234 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -1426,23 +1422,23 @@ Visualization Manager: Value: false Field of View: 0.7853981852531433 Focal Point: - X: 1.4857360124588013 - Y: 1.2019472122192383 + X: 1.0811700820922852 + Y: -0.22880220413208008 Z: -5.593325614929199 Focal Shape Fixed Size: false Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.794797420501709 + Pitch: 0.8147973418235779 Target Frame: base - Yaw: 5.0866851806640625 + Yaw: 3.7585225105285645 Saved: ~ Window Geometry: AB Wide Angle Front: collapsed: false Displays: - collapsed: true + collapsed: false F Confidence Overlay: collapsed: false F Confidence Overlay Replay: @@ -1452,7 +1448,7 @@ Window Geometry: F Traversability Overlay Replay: collapsed: false Height: 1376 - Hide Left Dock: true + Hide Left Dock: false Hide Right Dock: true Input Image: collapsed: false @@ -1466,7 +1462,7 @@ 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: diff --git a/wild_visual_navigation_ros/launch/replay_launch.launch b/wild_visual_navigation_ros/launch/replay_launch.launch index 10f92f0b..12c12f93 100644 --- a/wild_visual_navigation_ros/launch/replay_launch.launch +++ b/wild_visual_navigation_ros/launch/replay_launch.launch @@ -3,7 +3,7 @@ - + From 4fe42ccc523c5da17edae9e47b97320307af8d4b Mon Sep 17 00:00:00 2001 From: Robin Date: Thu, 7 Sep 2023 16:30:35 +0200 Subject: [PATCH 34/43] Changes from jetson --- .../config/procman/robot_dodo.pmd | 25 ++++++++++++------- .../wild_visual_navigation/default.yaml | 2 +- .../launch/wild_visual_navigation.launch | 4 +-- 3 files changed, 19 insertions(+), 12 deletions(-) diff --git a/wild_visual_navigation_ros/config/procman/robot_dodo.pmd b/wild_visual_navigation_ros/config/procman/robot_dodo.pmd index c2f87fc8..22efa0a4 100644 --- a/wild_visual_navigation_ros/config/procman/robot_dodo.pmd +++ b/wild_visual_navigation_ros/config/procman/robot_dodo.pmd @@ -1,4 +1,4 @@ -group "1.startup" { +group "01.startup" { cmd "1.1.chrony" { exec = "rosrun anymal_rsl_chrony reset_chrony.sh jonfrey dodo d020 false"; host = "localhost"; @@ -25,7 +25,7 @@ group "1.startup" { } } -group "2.start" { +group "02.start" { cmd "2.1.lpc" { exec = "rosrun anymal_dodo_rsl lpc.py"; host = "lpc"; @@ -44,7 +44,7 @@ group "2.start" { } } -group "3.recording" { +group "03.recording" { cmd "3.1.rosbag_record" { exec = "rosservice call /rosbag_record_robot_coordinator/record_bag 'yaml_file: /home/jonfrey/git/diffusion_navigation/diffusion_navigation_ros/config/recording/dodo.yaml' "; host = "localhost"; @@ -71,7 +71,7 @@ group "3.recording" { } } -group "4.monitoring" { +group "04.monitoring" { cmd "4.1.lpc_disk" { exec = "rostopic echo /disk_monitor_lpc/status/disks[1]"; host = "localhost"; @@ -86,7 +86,7 @@ group "4.monitoring" { } } -group "5.network" { +group "05.network" { cmd "5.1.switch_to_access_point" { exec = "rosrun anymal_router_utils router_utils.bash -i 192.168.0.173 -p d020 -m 1"; host = "localhost"; @@ -98,7 +98,7 @@ group "5.network" { } -group "6.disk_free" { +group "06.disk_free" { cmd "6.1.lpc" { exec = "df -h /dev/sda4"; host = "lpc"; @@ -118,7 +118,7 @@ group "6.disk_free" { } -group "7.tools for bags" { +group "07.tools for bags" { cmd "7.1.collect_lpc" { exec = " rsync -rzPav anymal-dodo-lpc:/home/jonfrey/catkin_ws/src/anymal_rsl/anymal_rsl/anymal_rsl_utils/anymal_rsl_recording/anymal_rsl_recording/data /home/jonfrey/mission_data/lpc"; host = "jetson"; @@ -138,7 +138,7 @@ group "7.tools for bags" { } -group "8.npc" { +group "08.npc" { cmd "8.1.alphasense_ptp" { exec = "sudo service phc2sys restart"; host = "npc"; @@ -174,7 +174,7 @@ group "8.npc" { } -group "9.orin" { +group "09.orin" { cmd "9.1.wild_visual_navigation" { exec = "roslaunch wild_visual_navigation_ros robot.launch"; host = "jetson"; @@ -215,6 +215,13 @@ group "12.white_balance" { } } +group "13.lpc" { + cmd "13.1.anymal_msg_converter" { + exec = "rosrun wild_visual_navigation_anymal anymal_msg_converter_node.py"; + host = "lpc"; + } +} + group "x.learning_utils" { cmd "x.1.pause_training" { exec = "rosservice call /wild_visual_navigation_node/pause_learning 'data: true'"; diff --git a/wild_visual_navigation_ros/config/wild_visual_navigation/default.yaml b/wild_visual_navigation_ros/config/wild_visual_navigation/default.yaml index 3c6f1dfc..a3a43422 100644 --- a/wild_visual_navigation_ros/config/wild_visual_navigation/default.yaml +++ b/wild_visual_navigation_ros/config/wild_visual_navigation/default.yaml @@ -51,7 +51,7 @@ status_thread_rate: 0.5 # hertz # Runtime options device: "cuda" -mode: "online" # check out comments in the class WVNMode +mode: "debug" # check out comments in the class WVNMode colormap: "RdYlBu" print_image_callback_time: false diff --git a/wild_visual_navigation_ros/launch/wild_visual_navigation.launch b/wild_visual_navigation_ros/launch/wild_visual_navigation.launch index 2ba3478f..eb54910f 100644 --- a/wild_visual_navigation_ros/launch/wild_visual_navigation.launch +++ b/wild_visual_navigation_ros/launch/wild_visual_navigation.launch @@ -23,11 +23,11 @@ - + From 929beb2bfdfc440e5e70f9305825a7582e8a7856 Mon Sep 17 00:00:00 2001 From: RobinSchmid7 Date: Fri, 8 Sep 2023 13:52:23 +0200 Subject: [PATCH 35/43] Cleanup procman and yaml files --- .../config/procman/replay.pmd | 12 +- .../config/procman/robot_dodo.pmd | 174 +++++++++--------- .../wild_visual_navigation/default.yaml | 6 +- 3 files changed, 96 insertions(+), 96 deletions(-) diff --git a/wild_visual_navigation_ros/config/procman/replay.pmd b/wild_visual_navigation_ros/config/procman/replay.pmd index ce2e97cb..e5d7270e 100644 --- a/wild_visual_navigation_ros/config/procman/replay.pmd +++ b/wild_visual_navigation_ros/config/procman/replay.pmd @@ -1,16 +1,20 @@ -cmd "1. rviz replay" { +cmd "01 - rviz replay" { exec = "roslaunch wild_visual_navigation_ros replay_launch.launch"; host = "localhost"; } -cmd "2. feat extractor" { +cmd "02 - feat extractor" { exec = "/home/rschmid/anaconda3/envs/wvn/bin/python3 /home/rschmid/git/wild_visual_navigation/wild_visual_navigation_ros/scripts/wvn_feature_extractor_node.py"; host = "localhost"; } -cmd "3. learning node" { +cmd "03 - learning node" { exec = "/home/rschmid/anaconda3/envs/wvn/bin/python3 /home/rschmid/git/wild_visual_navigation/wild_visual_navigation_ros/scripts/wvn_learning_node.py"; host = "localhost"; } -cmd "4. image overlay" { +cmd "04 - image overlay" { exec = "/home/rschmid/anaconda3/envs/wvn/bin/python3 /home/rschmid/git/wild_visual_navigation/wild_visual_navigation_ros/scripts/overlay_images.py _image_sub_topic:=/wild_visual_navigation_node/front/image_input _value_sub_topic:=/wild_visual_navigation_node/front/traversability _image_pub_topic:=traversability_overlayed"; host = "localhost"; } +cmd "05 - rqt learning graph" { + exec = "rosrun rqt_multiplot rqt_multiplot"; + host = "localhost"; +} \ No newline at end of file diff --git a/wild_visual_navigation_ros/config/procman/robot_dodo.pmd b/wild_visual_navigation_ros/config/procman/robot_dodo.pmd index 22efa0a4..da508fd7 100644 --- a/wild_visual_navigation_ros/config/procman/robot_dodo.pmd +++ b/wild_visual_navigation_ros/config/procman/robot_dodo.pmd @@ -1,241 +1,237 @@ -group "01.startup" { - cmd "1.1.chrony" { +group "01 - startup" { + cmd "01 - power jetson" { + exec = "/home/jonfrey/powerline.sh"; + host = "lpc"; + } + cmd "02 - ping jetson" { + exec = "ping anymal-dodo-jetson"; + host = "localhost"; + } + cmd "03 - chrony" { exec = "rosrun anymal_rsl_chrony reset_chrony.sh jonfrey dodo d020 false"; host = "localhost"; } - cmd "1.2.depth" { + cmd "04 - depth" { exec = "rosservice call /depth_cameras/enable 'data: true' "; host = "localhost"; } - cmd "1.3.lidar" { + cmd "05 - lidar" { exec = "rosservice call /lidar/enable 'data: true' "; host = "localhost"; } - cmd "1.4.wide angle" { + cmd "06 - wide angle" { exec = "rosservice call /wide_angle_cameras/enable 'data: true' "; host = "localhost"; } - cmd "1.5.power jetson" { - exec = "/home/jonfrey/powerline.sh"; - host = "lpc"; - } - cmd "1.6.ping jetson" { - exec = "ping anymal-dodo-jetson"; - host = "localhost"; - } } -group "02.start" { - cmd "2.1.lpc" { +group "02 - start" { + cmd "01 - lpc" { exec = "rosrun anymal_dodo_rsl lpc.py"; host = "lpc"; } - cmd "2.2.npc" { + cmd "02 - npc" { exec = "rosrun anymal_d_rsl npc.py"; host = "npc"; } - cmd "2.3.jetson" { + cmd "03 - jetson" { exec = "rosrun anymal_d_rsl jetson.py"; host = "jetson"; } - cmd "2.4.opc" { + cmd "04 - opc" { exec = "rosrun anymal_d_rsl opc.py"; host = "localhost"; } } -group "03.recording" { - cmd "3.1.rosbag_record" { +group "09 - wvn" { + cmd "01 - anymal_msg_converter" { + exec = "rosrun wild_visual_navigation_anymal anymal_msg_converter_node.py"; + host = "lpc"; + } + cmd "02 - rviz" { + exec = "roslaunch wild_visual_navigation_ros view.launch"; + host = "localhost"; + } + cmd "03 - wild_visual_navigation" { + exec = "roslaunch wild_visual_navigation_ros robot.launch"; + host = "jetson"; + } + cmd "04 - rqt learning graph" { + exec = "rosrun rqt_multiplot rqt_multiplot"; + host = "localhost"; + } + cmd "04 - kill_wvn" { + exec = "rosnode kill /wild_visual_navigation_node"; + host = "localhost"; + } +} + +group "03 - recording" { + cmd "01 - rosbag_record" { exec = "rosservice call /rosbag_record_robot_coordinator/record_bag 'yaml_file: /home/jonfrey/git/diffusion_navigation/diffusion_navigation_ros/config/recording/dodo.yaml' "; host = "localhost"; } - cmd "3.2.rosbag_record_state" { + cmd "02 - rosbag_record_state" { exec = "rosnode list | grep record"; host = "localhost"; } - cmd "3.3.rosbag_stop" { + cmd "03 - rosbag_stop" { exec = "rosservice call /rosbag_record_robot_coordinator/stop_bag 'verbose: false' "; host = "localhost"; } - cmd "3.4.fetch_bags" { + cmd "04 - fetch_bags" { exec = "/home/jonfrey/git/anymal_rsl/anymal_rsl/anymal_rsl_utils/anymal_rsl_recording/anymal_rsl_recording/bin/copy_mission_data_from_robot.sh jonfrey dodo /home/jonfrey/mission_data"; host = "localhost"; } - cmd "3.5.delete_bags" { + cmd "05 - delete_bags" { exec = "/home/jonfrey/git/anymal_rsl/anymal_rsl/anymal_rsl_utils/anymal_rsl_recording/anymal_rsl_recording/bin/remove_mission_data_from_robot.sh jonfrey dodo"; host = "localhost"; } - cmd "3.6.reset_recording" { + cmd "06 - reset_recording" { exec = "rosnode kill rosbag_record_coordinator rosbag_record_robot_jetson rosbag_record_robot_npc rosbag_record_robot_lpc"; host = "localhost"; } } -group "04.monitoring" { - cmd "4.1.lpc_disk" { +group "04 - monitoring" { + cmd "01 - lpc_disk" { exec = "rostopic echo /disk_monitor_lpc/status/disks[1]"; host = "localhost"; } - cmd "4.2.npc_disk" { + cmd "02 - npc_disk" { exec = "rostopic echo /disk_monitor_npc/status/disks[1]"; host = "localhost"; } - cmd "4.3.jetson_disk" { + cmd "03 - jetson_disk" { exec = "rostopic echo /disk_monitor_jetson/status/disks[1]"; host = "localhost"; } } -group "05.network" { - cmd "5.1.switch_to_access_point" { +group "05 - network" { + cmd "01 - switch_to_access_point" { exec = "rosrun anymal_router_utils router_utils.bash -i 192.168.0.173 -p d020 -m 1"; host = "localhost"; } - cmd "5.2.switch_to_client_mode" { + cmd "02 - switch_to_client_mode" { exec = "rosrun anymal_router_utils router_utils.bash -i 192.168.0.173 -p d020 -m 3"; host = "localhost"; } } -group "06.disk_free" { - cmd "6.1.lpc" { +group "06 - disk_free" { + cmd "01 - lpc" { exec = "df -h /dev/sda4"; host = "lpc"; } - cmd "6.2.npc" { + cmd "02 - npc" { exec = " df -h /dev/sda4"; host = "npc"; } - cmd "6.3.jetson" { + cmd "03 - jetson" { exec = "df -h /dev/nvme0n1p1"; host = "jetson"; } - cmd "6.3.opc" { + cmd "04 - opc" { exec = "df -h /dev/nvme0n1p5"; host = "localhost"; } } -group "07.tools for bags" { - cmd "7.1.collect_lpc" { +group "07 - tools for bags" { + cmd "01 - collect_lpc" { exec = " rsync -rzPav anymal-dodo-lpc:/home/jonfrey/catkin_ws/src/anymal_rsl/anymal_rsl/anymal_rsl_utils/anymal_rsl_recording/anymal_rsl_recording/data /home/jonfrey/mission_data/lpc"; host = "jetson"; } - cmd "7.2.collect_npc" { + cmd "02 - collect_npc" { exec = " rsync -rzPav anymal-dodo-npc:/home/jonfrey/catkin_ws/src/anymal_rsl/anymal_rsl/anymal_rsl_utils/anymal_rsl_recording/anymal_rsl_recording/data /home/jonfrey/mission_data/npc"; host = "jetson"; } - cmd "7.3.delete lpc" { + cmd "03 - delete lpc" { exec = "rm -r -f /home/jonfrey/catkin_ws/src/anymal_rsl/anymal_rsl/anymal_rsl_utils/anymal_rsl_recording/anymal_rsl_recording/data/*.bag"; host = "lpc"; } - cmd "7.4.delete npc" { + cmd "04 - delete npc" { exec = "rm -r -f /home/jonfrey/catkin_ws/src/anymal_rsl/anymal_rsl/anymal_rsl_utils/anymal_rsl_recording/anymal_rsl_recording/data/*.bag"; host = "npc"; } } -group "08.npc" { - cmd "8.1.alphasense_ptp" { +group "08 - npc" { + cmd "01 - alphasense_ptp" { exec = "sudo service phc2sys restart"; host = "npc"; } - cmd "8.2.anymal_msg_converter" { + cmd "02 - anymal_msg_converter" { exec = "rosrun wild_visual_navigation_anymal anymal_msg_converter_node.py"; host = "npc"; } - cmd "8.3.local_planner_visual" { + cmd "03 - local_planner_visual" { exec = "roslaunch wild_visual_navigation_ros field_local_planner.launch traversability:=visual base_inverted:=false"; host = "npc"; } - cmd "8.4.local_planner_visual_inverted" { + cmd "04 - local_planner_visual_inverted" { exec = "roslaunch wild_visual_navigation_ros field_local_planner.launch traversability:=visual base_inverted:=true"; host = "npc"; } - cmd "8.5.local_planner_geometric" { + cmd "05 - local_planner_geometric" { exec = "roslaunch wild_visual_navigation_ros field_local_planner.launch traversability:=geometric base_inverted:=false"; host = "npc"; } - cmd "8.6.local_planner_geometric_inverted" { + cmd "06 - local_planner_geometric_inverted" { exec = "roslaunch wild_visual_navigation_ros field_local_planner.launch traversability:=geometric base_inverted:=true"; host = "npc"; } - cmd "8.7.local_planner_debug_on_robot" { + cmd "07 - local_planner_debug_on_robot" { exec = "roslaunch wild_visual_navigation_ros field_local_planner.launch traversability:=geometric base_inverted:=true output_twist_topic:=/dummy_twist"; host = "npc"; } - cmd "8.8.smart_carrot" { + cmd "08 - smart_carrot" { exec = "rosrun wild_visual_navigation_ros smart_carrot.py"; host = "npc"; } } - -group "09.orin" { - cmd "9.1.wild_visual_navigation" { - exec = "roslaunch wild_visual_navigation_ros robot.launch"; - host = "jetson"; - } - - cmd "9.2.kill_wvn" { - exec = "rosnode kill /wild_visual_navigation_node"; - host = "localhost"; - } -} - -group "10.visualization" { - cmd "10.1.rviz" { - exec = "roslaunch wild_visual_navigation_ros view.launch"; - host = "localhost"; - } -} - -group "11.configuration" { - cmd "11.1.dynamic_reconfigure" { +group "10 - configuration" { + cmd "01 - dynamic_reconfigure" { exec = "rosrun rqt_reconfigure rqt_reconfigure"; host = "localhost"; } } -group "12.white_balance" { - cmd "12.1.white_balance_front_reset" { +group "11 - white_balance" { + cmd "01 - white_balance_front_reset" { exec = "rosservice call /alphasense_raw_image_pipeline_front/reset_white_balance"; host = "localhost"; } - cmd "12.2.white_balance_left_reset" { + cmd "02 - white_balance_left_reset" { exec = "rosservice call /alphasense_raw_image_pipeline_left/reset_white_balance"; host = "localhost"; } - cmd "12.3.white_balance_right_reset" { + cmd "03 - white_balance_right_reset" { exec = "rosservice call /alphasense_raw_image_pipeline_right/reset_white_balance"; host = "localhost"; } } -group "13.lpc" { - cmd "13.1.anymal_msg_converter" { - exec = "rosrun wild_visual_navigation_anymal anymal_msg_converter_node.py"; - host = "lpc"; - } -} - -group "x.learning_utils" { - cmd "x.1.pause_training" { +group "x - learning_utils" { + cmd "01 - pause_training" { exec = "rosservice call /wild_visual_navigation_node/pause_learning 'data: true'"; host = "npc"; } - cmd "x.2.resume_training" { + cmd "02 - resume_training" { exec = "rosservice call /wild_visual_navigation_node/pause_learning 'data: false'"; host = "npc"; } - cmd "x.3.save_checkpoint" { + cmd "03 - save_checkpoint" { exec = "rosservice call /wild_visual_navigation_node/save_checkpoint ''"; host = "anymal_coyote_orin"; } - cmd "x.4.load_checkpoint" { + cmd "04 - load_checkpoint" { exec = "rosservice call /wild_visual_navigation_node/load_checkpoint 'path: 'absolute_path_in_robot_filesystem'"; host = "npc"; } diff --git a/wild_visual_navigation_ros/config/wild_visual_navigation/default.yaml b/wild_visual_navigation_ros/config/wild_visual_navigation/default.yaml index 058af289..7d52ae40 100644 --- a/wild_visual_navigation_ros/config/wild_visual_navigation/default.yaml +++ b/wild_visual_navigation_ros/config/wild_visual_navigation/default.yaml @@ -17,16 +17,16 @@ robot_height: 0.3 robot_max_velocity: 0.8 # Traversability estimation params -traversability_radius: 5.0 # meters +traversability_radius: 10.0 # meters image_graph_dist_thr: 0.2 # meters proprio_graph_dist_thr: 0.1 # meters network_input_image_height: 224 # 448 network_input_image_width: 224 # 448 segmentation_type: "random" feature_type: "dino" -dino_patch_size: 8 # DINO only, 16 +dino_patch_size: 8 # 8 or 16; 8 is finer slic_num_components: 100 -dino_dim: 384 # 90, 384 +dino_dim: 384 # 90 or 384; 384 is better confidence_std_factor: 4.0 scale_traversability: False # This parameter needs to be false when using the anomaly detection model scale_traversability_max_fpr: 0.25 From 8602f1d89e01fe5898a3367f1f6b0acf9198a410 Mon Sep 17 00:00:00 2001 From: RobinSchmid7 Date: Fri, 8 Sep 2023 13:54:53 +0200 Subject: [PATCH 36/43] Rechange color --- wild_visual_navigation/visu/visualizer.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/wild_visual_navigation/visu/visualizer.py b/wild_visual_navigation/visu/visualizer.py index 47bac9c8..b177e411 100644 --- a/wild_visual_navigation/visu/visualizer.py +++ b/wild_visual_navigation/visu/visualizer.py @@ -359,8 +359,8 @@ def plot_detectron_classification( cmap = kwargs["cmap"] else: s = 0.2 # If bigger, get more fine-grained green, if smaller get more fine-grained red - cmap = cm.get_cmap("RdYlGn", 256) - # cmap = cm.get_cmap("RdBu", 256) + # cmap = cm.get_cmap("RdYlGn", 256) + cmap = cm.get_cmap("RdYlBu", 256) cmap = np.concatenate([cmap(np.linspace(0, s, 128)), cmap(np.linspace(1 - s, 1.0, 128))]) # Stretch the colormap cmap = torch.from_numpy(cmap).to(seg)[:, :3] From e32a91f28858a46334b35bfa0b464ec70d09d388 Mon Sep 17 00:00:00 2001 From: RobinSchmid7 Date: Fri, 8 Sep 2023 16:07:13 +0200 Subject: [PATCH 37/43] Fix running mean --- .../utils/confidence_generator.py | 21 ++++++++++++++----- 1 file changed, 16 insertions(+), 5 deletions(-) diff --git a/wild_visual_navigation/utils/confidence_generator.py b/wild_visual_navigation/utils/confidence_generator.py index 7113fe6c..224a86fd 100644 --- a/wild_visual_navigation/utils/confidence_generator.py +++ b/wild_visual_navigation/utils/confidence_generator.py @@ -48,9 +48,14 @@ def __init__( running_sum = torch.zeros(1, dtype=torch.float64) running_sum_of_squares = torch.zeros(1, dtype=torch.float64) - self.running_n = torch.nn.Parameter(running_n, requires_grad=False) - self.running_sum = torch.nn.Parameter(running_sum, requires_grad=False) - self.running_sum_of_squares = torch.nn.Parameter(running_sum_of_squares, requires_grad=False) + # self.running_n = torch.nn.Parameter(running_n, requires_grad=False) + # self.running_sum = torch.nn.Parameter(running_sum, requires_grad=False) + # self.running_sum_of_squares = torch.nn.Parameter(running_sum_of_squares, requires_grad=False) + + self.running_n = running_n.to("cuda") + self.running_sum = running_sum.to("cuda") + self.running_sum_of_squares = running_sum_of_squares.to("cuda") + self._update = self.update_running_mean self._reset = self.reset_running_mean elif method == "latest_measurment": @@ -81,9 +86,15 @@ def update_running_mean(self, x: torch.tensor, x_positive: torch.tensor): self.var[0] = self.running_sum_of_squares / self.running_n - self.mean**2 self.std[0] = torch.sqrt(self.var) + if x.device != self.mean.device: + return torch.zeros_like(x) + # Then the confidence is computed as the distance to the center of the Gaussian given factor*sigma - confidence = torch.exp(-(((x - self.mean) / (self.std * self.std_factor)) ** 2) * 0.5) - confidence[x < self.mean] = 1.0 + # confidence = torch.exp(-(((x - self.mean) / (self.std * self.std_factor)) ** 2) * 0.5) + # confidence[x < self.mean] = 1.0 + + x = torch.clip(x, self.mean - 2 * self.std, self.mean + 2 * self.std) + confidence = (x - torch.min(x)) / (torch.max(x) - torch.min(x)) return confidence.type(torch.float32) From ad1a69b7975d8a648875dee4e7716907aa127de7 Mon Sep 17 00:00:00 2001 From: RobinSchmid7 Date: Fri, 8 Sep 2023 17:02:34 +0200 Subject: [PATCH 38/43] Added moving average --- .../cfg/experiment_params.py | 2 +- .../utils/confidence_generator.py | 35 ++++++++++- wild_visual_navigation/visu/visualizer.py | 63 +++++++++++++++++++ 3 files changed, 96 insertions(+), 4 deletions(-) diff --git a/wild_visual_navigation/cfg/experiment_params.py b/wild_visual_navigation/cfg/experiment_params.py index 1f3768b1..90d90e63 100644 --- a/wild_visual_navigation/cfg/experiment_params.py +++ b/wild_visual_navigation/cfg/experiment_params.py @@ -50,7 +50,7 @@ class LossParams: @dataclass class LossAnomalyParams: - method: str = "latest_measurment" # "latest_measurment", "running_mean" + method: str = "moving_average" # "latest_measurment", "running_mean" confidence_std_factor: float = 0.5 loss_anomaly: LossAnomalyParams = LossAnomalyParams() diff --git a/wild_visual_navigation/utils/confidence_generator.py b/wild_visual_navigation/utils/confidence_generator.py index 224a86fd..927317e9 100644 --- a/wild_visual_navigation/utils/confidence_generator.py +++ b/wild_visual_navigation/utils/confidence_generator.py @@ -1,6 +1,7 @@ from wild_visual_navigation.utils import KalmanFilter import torch import os +from collections import deque class ConfidenceGenerator(torch.nn.Module): @@ -26,9 +27,13 @@ def __init__( mean = torch.zeros(1, dtype=torch.float32) var = torch.ones((1, 1), dtype=torch.float32) std = torch.ones(1, dtype=torch.float32) - self.mean = torch.nn.Parameter(mean, requires_grad=False) - self.var = torch.nn.Parameter(var, requires_grad=False) - self.std = torch.nn.Parameter(std, requires_grad=False) + # self.mean = torch.nn.Parameter(mean, requires_grad=False) + # self.var = torch.nn.Parameter(var, requires_grad=False) + # self.std = torch.nn.Parameter(std, requires_grad=False) + + self.mean = 0 + self.var = 1 + self.std = 1 if method == "kalman_filter": kf_process_cov = 0.2 @@ -61,6 +66,11 @@ def __init__( elif method == "latest_measurment": self._update = self.update_latest_measurment self._reset = self.reset_latest_measurment + elif method == "moving_average": + window_size = 5 + self.data_window = deque(maxlen=window_size) + self._update = self.update_moving_average + self._reset = self.reset_moving_average else: raise ValueError("Unknown method") @@ -75,6 +85,11 @@ def reset_latest_measurment(self, x: torch.Tensor, x_positive: torch.Tensor): self.var[0] = 1 self.std[0] = 1 + def reset_moving_average(self, x: torch.Tensor, x_positive: torch.Tensor): + self.mean[0] = 0 + self.var[0] = 1 + self.std[0] = 1 + def update_running_mean(self, x: torch.tensor, x_positive: torch.tensor): # We assume the positive samples' loss follows a Gaussian distribution # We estimate the parameters empirically @@ -98,6 +113,20 @@ def update_running_mean(self, x: torch.tensor, x_positive: torch.tensor): return confidence.type(torch.float32) + def update_moving_average(self, x: torch.tensor, x_positive: torch.tensor): + self.data_window.append(x_positive) + + data_window_tensor = list(self.data_window) + data_window_tensor = torch.cat(data_window_tensor, dim=0) + + self.mean = data_window_tensor.mean() + self.std = data_window_tensor.std() + + x = torch.clip(x, self.mean - 2 * self.std, self.mean + 2 * self.std) + confidence = (x - torch.min(x)) / (torch.max(x) - torch.min(x)) + + return confidence.type(torch.float32) + def update_kalman_filter(self, x: torch.tensor, x_positive: torch.tensor): # Kalman Filter implementation if x_positive.shape[0] != 0: diff --git a/wild_visual_navigation/visu/visualizer.py b/wild_visual_navigation/visu/visualizer.py index b177e411..c5b4a2c7 100644 --- a/wild_visual_navigation/visu/visualizer.py +++ b/wild_visual_navigation/visu/visualizer.py @@ -21,6 +21,8 @@ from wild_visual_navigation.visu import paper_colors_rgb_f, paper_colors_rgba_f from pytictac import Timer, accumulate_time +import rospy + __all__ = ["LearningVisualizer"] @@ -369,6 +371,16 @@ def plot_detectron_classification( (seg * 255).type(torch.long).clip(0, 255), max_seg=256, colormap=cmap, store=False, not_log=True ) + # plt.hist(seg_img.ravel(), bins=500) + # # Get current ros time + # now = rospy.Time.now() + # # Create a unique filename + # filename = f"{now.secs}_{now.nsecs}.png" + # # Save the figure + # plt.savefig(f"/home/rschmid/overlays/{filename}") + # # Close the figure + # plt.close() + H, W = img.shape[:2] back = np.zeros((H, W, 4)) back[:, :, :3] = img @@ -613,6 +625,57 @@ def plot_sparse_optical_flow( # pass # return np.array(pil_img).astype(np.uint8) + def shiftedColorMap(cmap, start=0, midpoint=0.5, stop=1.0, name='shiftedcmap'): + ''' + Function to offset the "center" of a colormap. Useful for + data with a negative min and positive max and you want the + middle of the colormap's dynamic range to be at zero. + + Input + ----- + cmap : The matplotlib colormap to be altered + start : Offset from lowest point in the colormap's range. + Defaults to 0.0 (no lower offset). Should be between + 0.0 and `midpoint`. + midpoint : The new center of the colormap. Defaults to + 0.5 (no shift). Should be between 0.0 and 1.0. In + general, this should be 1 - vmax / (vmax + abs(vmin)) + For example if your data range from -15.0 to +5.0 and + you want the center of the colormap at 0.0, `midpoint` + should be set to 1 - 5/(5 + 15)) or 0.75 + stop : Offset from highest point in the colormap's range. + Defaults to 1.0 (no upper offset). Should be between + `midpoint` and 1.0. + ''' + cdict = { + 'red': [], + 'green': [], + 'blue': [], + 'alpha': [] + } + + # regular index to compute the colors + reg_index = np.linspace(start, stop, 257) + + # shifted index to match the data + shift_index = np.hstack([ + np.linspace(0.0, midpoint, 128, endpoint=False), + np.linspace(midpoint, 1.0, 129, endpoint=True) + ]) + + for ri, si in zip(reg_index, shift_index): + r, g, b, a = cmap(ri) + + cdict['red'].append((si, r, r)) + cdict['green'].append((si, g, g)) + cdict['blue'].append((si, b, b)) + cdict['alpha'].append((si, a, a)) + + newcmap = matplotlib.colors.LinearSegmentedColormap(name, cdict) + plt.register_cmap(cmap=newcmap) + + return newcmap + if __name__ == "__main__": # Data was generated in the visu function of the lightning_module with the following code From fa0c7cf82554499e969faab29bf01efad7c5cfb3 Mon Sep 17 00:00:00 2001 From: RobinSchmid7 Date: Fri, 8 Sep 2023 17:03:00 +0200 Subject: [PATCH 39/43] Added moving average --- wild_visual_navigation/cfg/experiment_params.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/wild_visual_navigation/cfg/experiment_params.py b/wild_visual_navigation/cfg/experiment_params.py index 90d90e63..ea641a5a 100644 --- a/wild_visual_navigation/cfg/experiment_params.py +++ b/wild_visual_navigation/cfg/experiment_params.py @@ -50,7 +50,7 @@ class LossParams: @dataclass class LossAnomalyParams: - method: str = "moving_average" # "latest_measurment", "running_mean" + method: str = "moving_average" # "latest_measurment", "running_mean", "moving_average" confidence_std_factor: float = 0.5 loss_anomaly: LossAnomalyParams = LossAnomalyParams() From 892a2da1d371cacecd354276f9bbd9b94a093965 Mon Sep 17 00:00:00 2001 From: RobinSchmid7 Date: Mon, 11 Sep 2023 09:42:24 +0200 Subject: [PATCH 40/43] Fix running mean, change some configs --- .../cfg/experiment_params.py | 2 +- .../utils/confidence_generator.py | 14 ++--- wild_visual_navigation/visu/visualizer.py | 58 +------------------ .../wild_visual_navigation/default.yaml | 2 +- 4 files changed, 9 insertions(+), 67 deletions(-) diff --git a/wild_visual_navigation/cfg/experiment_params.py b/wild_visual_navigation/cfg/experiment_params.py index ea641a5a..5f1c2f2a 100644 --- a/wild_visual_navigation/cfg/experiment_params.py +++ b/wild_visual_navigation/cfg/experiment_params.py @@ -50,7 +50,7 @@ class LossParams: @dataclass class LossAnomalyParams: - method: str = "moving_average" # "latest_measurment", "running_mean", "moving_average" + method: str = "running_mean" # "latest_measurment", "running_mean", "moving_average" confidence_std_factor: float = 0.5 loss_anomaly: LossAnomalyParams = LossAnomalyParams() diff --git a/wild_visual_navigation/utils/confidence_generator.py b/wild_visual_navigation/utils/confidence_generator.py index 927317e9..23df4dbe 100644 --- a/wild_visual_navigation/utils/confidence_generator.py +++ b/wild_visual_navigation/utils/confidence_generator.py @@ -27,13 +27,9 @@ def __init__( mean = torch.zeros(1, dtype=torch.float32) var = torch.ones((1, 1), dtype=torch.float32) std = torch.ones(1, dtype=torch.float32) - # self.mean = torch.nn.Parameter(mean, requires_grad=False) - # self.var = torch.nn.Parameter(var, requires_grad=False) - # self.std = torch.nn.Parameter(std, requires_grad=False) - - self.mean = 0 - self.var = 1 - self.std = 1 + self.mean = torch.nn.Parameter(mean, requires_grad=False) + self.var = torch.nn.Parameter(var, requires_grad=False) + self.std = torch.nn.Parameter(std, requires_grad=False) if method == "kalman_filter": kf_process_cov = 0.2 @@ -119,8 +115,8 @@ def update_moving_average(self, x: torch.tensor, x_positive: torch.tensor): data_window_tensor = list(self.data_window) data_window_tensor = torch.cat(data_window_tensor, dim=0) - self.mean = data_window_tensor.mean() - self.std = data_window_tensor.std() + self.mean[0] = data_window_tensor.mean() + self.std[0] = data_window_tensor.std() x = torch.clip(x, self.mean - 2 * self.std, self.mean + 2 * self.std) confidence = (x - torch.min(x)) / (torch.max(x) - torch.min(x)) diff --git a/wild_visual_navigation/visu/visualizer.py b/wild_visual_navigation/visu/visualizer.py index c5b4a2c7..f5a74211 100644 --- a/wild_visual_navigation/visu/visualizer.py +++ b/wild_visual_navigation/visu/visualizer.py @@ -21,8 +21,6 @@ from wild_visual_navigation.visu import paper_colors_rgb_f, paper_colors_rgba_f from pytictac import Timer, accumulate_time -import rospy - __all__ = ["LearningVisualizer"] @@ -360,9 +358,8 @@ def plot_detectron_classification( if kwargs.get("cmap", None): cmap = kwargs["cmap"] else: - s = 0.2 # If bigger, get more fine-grained green, if smaller get more fine-grained red - # cmap = cm.get_cmap("RdYlGn", 256) - cmap = cm.get_cmap("RdYlBu", 256) + s = 0.3 # If bigger, get more fine-grained green, if smaller get more fine-grained red + cmap = cm.get_cmap("RdYlBu", 256) # or RdYlGn cmap = np.concatenate([cmap(np.linspace(0, s, 128)), cmap(np.linspace(1 - s, 1.0, 128))]) # Stretch the colormap cmap = torch.from_numpy(cmap).to(seg)[:, :3] @@ -625,57 +622,6 @@ def plot_sparse_optical_flow( # pass # return np.array(pil_img).astype(np.uint8) - def shiftedColorMap(cmap, start=0, midpoint=0.5, stop=1.0, name='shiftedcmap'): - ''' - Function to offset the "center" of a colormap. Useful for - data with a negative min and positive max and you want the - middle of the colormap's dynamic range to be at zero. - - Input - ----- - cmap : The matplotlib colormap to be altered - start : Offset from lowest point in the colormap's range. - Defaults to 0.0 (no lower offset). Should be between - 0.0 and `midpoint`. - midpoint : The new center of the colormap. Defaults to - 0.5 (no shift). Should be between 0.0 and 1.0. In - general, this should be 1 - vmax / (vmax + abs(vmin)) - For example if your data range from -15.0 to +5.0 and - you want the center of the colormap at 0.0, `midpoint` - should be set to 1 - 5/(5 + 15)) or 0.75 - stop : Offset from highest point in the colormap's range. - Defaults to 1.0 (no upper offset). Should be between - `midpoint` and 1.0. - ''' - cdict = { - 'red': [], - 'green': [], - 'blue': [], - 'alpha': [] - } - - # regular index to compute the colors - reg_index = np.linspace(start, stop, 257) - - # shifted index to match the data - shift_index = np.hstack([ - np.linspace(0.0, midpoint, 128, endpoint=False), - np.linspace(midpoint, 1.0, 129, endpoint=True) - ]) - - for ri, si in zip(reg_index, shift_index): - r, g, b, a = cmap(ri) - - cdict['red'].append((si, r, r)) - cdict['green'].append((si, g, g)) - cdict['blue'].append((si, b, b)) - cdict['alpha'].append((si, a, a)) - - newcmap = matplotlib.colors.LinearSegmentedColormap(name, cdict) - plt.register_cmap(cmap=newcmap) - - return newcmap - if __name__ == "__main__": # Data was generated in the visu function of the lightning_module with the following code diff --git a/wild_visual_navigation_ros/config/wild_visual_navigation/default.yaml b/wild_visual_navigation_ros/config/wild_visual_navigation/default.yaml index 7d52ae40..71f65689 100644 --- a/wild_visual_navigation_ros/config/wild_visual_navigation/default.yaml +++ b/wild_visual_navigation_ros/config/wild_visual_navigation/default.yaml @@ -17,7 +17,7 @@ robot_height: 0.3 robot_max_velocity: 0.8 # Traversability estimation params -traversability_radius: 10.0 # meters +traversability_radius: 5.0 # meters image_graph_dist_thr: 0.2 # meters proprio_graph_dist_thr: 0.1 # meters network_input_image_height: 224 # 448 From e7e5e69fe0f36deb7d7c2b13891e2dc70d829649 Mon Sep 17 00:00:00 2001 From: RobinSchmid7 Date: Mon, 11 Sep 2023 10:05:31 +0200 Subject: [PATCH 41/43] Fix running mean, change some configs --- wild_visual_navigation/learning/utils/loss.py | 2 ++ .../utils/confidence_generator.py | 23 +++++++++++-------- .../scripts/wvn_feature_extractor_node.py | 2 +- 3 files changed, 17 insertions(+), 10 deletions(-) diff --git a/wild_visual_navigation/learning/utils/loss.py b/wild_visual_navigation/learning/utils/loss.py index 48f9302a..f4aa6837 100644 --- a/wild_visual_navigation/learning/utils/loss.py +++ b/wild_visual_navigation/learning/utils/loss.py @@ -15,6 +15,7 @@ def __init__(self, confidence_std_factor: float, method: str, log_enabled: bool, method=method, log_enabled=log_enabled, log_folder=log_folder, + anomaly_detection=True, ) def forward( @@ -75,6 +76,7 @@ def __init__( method=method, log_enabled=log_enabled, log_folder=log_folder, + anomaly_detection=False, ) def reset(self): diff --git a/wild_visual_navigation/utils/confidence_generator.py b/wild_visual_navigation/utils/confidence_generator.py index 23df4dbe..ac7f17da 100644 --- a/wild_visual_navigation/utils/confidence_generator.py +++ b/wild_visual_navigation/utils/confidence_generator.py @@ -11,6 +11,7 @@ def __init__( method: str = "running_mean", log_enabled: bool = False, log_folder: str = "/tmp", + anomaly_detection: bool = False, ): """Returns a confidence value for each number @@ -23,6 +24,7 @@ def __init__( self.log_enabled = log_enabled self.log_folder = log_folder + self.anomaly_detection = anomaly_detection mean = torch.zeros(1, dtype=torch.float32) var = torch.ones((1, 1), dtype=torch.float32) @@ -100,12 +102,13 @@ def update_running_mean(self, x: torch.tensor, x_positive: torch.tensor): if x.device != self.mean.device: return torch.zeros_like(x) - # Then the confidence is computed as the distance to the center of the Gaussian given factor*sigma - # confidence = torch.exp(-(((x - self.mean) / (self.std * self.std_factor)) ** 2) * 0.5) - # confidence[x < self.mean] = 1.0 - - x = torch.clip(x, self.mean - 2 * self.std, self.mean + 2 * self.std) - confidence = (x - torch.min(x)) / (torch.max(x) - torch.min(x)) + if self.anomaly_detection: + x = torch.clip(x, self.mean - 2 * self.std, self.mean + 2 * self.std) + confidence = (x - torch.min(x)) / (torch.max(x) - torch.min(x)) + else: + # Then the confidence is computed as the distance to the center of the Gaussian given factor*sigma + confidence = torch.exp(-(((x - self.mean) / (self.std * self.std_factor)) ** 2) * 0.5) + confidence[x < self.mean] = 1.0 return confidence.type(torch.float32) @@ -167,9 +170,11 @@ def inference_without_update(self, x: torch.tensor): if x.device != self.mean.device: return torch.zeros_like(x) - # confidence = torch.exp(-(((x - self.mean) / (self.std * self.std_factor)) ** 2) * 0.5) - x = torch.clip(x, self.mean - 2 * self.std, self.mean + 2 * self.std) - confidence = (x - torch.min(x)) / (torch.max(x) - torch.min(x)) + if self.anomaly_detection: + x = torch.clip(x, self.mean - 2 * self.std, self.mean + 2 * self.std) + confidence = (x - torch.min(x)) / (torch.max(x) - torch.min(x)) + else: + confidence = torch.exp(-(((x - self.mean) / (self.std * self.std_factor)) ** 2) * 0.5) return confidence.type(torch.float32) 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 4914e5f5..c9d08ec5 100644 --- a/wild_visual_navigation_ros/scripts/wvn_feature_extractor_node.py +++ b/wild_visual_navigation_ros/scripts/wvn_feature_extractor_node.py @@ -51,7 +51,7 @@ def __init__(self): if not self.anomaly_detection: self.confidence_generator = ConfidenceGenerator( - method=self.exp_cfg["loss"]["method"], std_factor=self.exp_cfg["loss"]["confidence_std_factor"] + method=self.exp_cfg["loss"]["method"], std_factor=self.exp_cfg["loss"]["confidence_std_factor"], anomaly_detection=self.anomaly_detection ) self.scale_traversability = True else: From f10882fad05ebf13b0c8102b74c7d4d6a7661113 Mon Sep 17 00:00:00 2001 From: Robin Schmid Date: Fri, 6 Oct 2023 16:18:19 +0200 Subject: [PATCH 42/43] Add HDR cam to recording yaml file --- .../config/procman/robot_dodo.pmd | 8 ++++++++ .../config/recording/dodo.yaml | 12 ++++++++---- 2 files changed, 16 insertions(+), 4 deletions(-) diff --git a/wild_visual_navigation_ros/config/procman/robot_dodo.pmd b/wild_visual_navigation_ros/config/procman/robot_dodo.pmd index 22efa0a4..8f08c1a8 100644 --- a/wild_visual_navigation_ros/config/procman/robot_dodo.pmd +++ b/wild_visual_navigation_ros/config/procman/robot_dodo.pmd @@ -42,6 +42,10 @@ group "02.start" { exec = "rosrun anymal_d_rsl opc.py"; host = "localhost"; } + cmd "9.1.elevation_mapping_cupy" { + exec = "roslaunch elevation_mapping_cupy elevation_mapping_cupy.launch"; + host = "jetson"; + } } group "03.recording" { @@ -179,6 +183,10 @@ group "09.orin" { exec = "roslaunch wild_visual_navigation_ros robot.launch"; host = "jetson"; } + cmd "9.1.wild_visual_navigation" { + exec = "roslaunch wild_visual_navigation_ros robot.launch"; + host = "jetson"; + } cmd "9.2.kill_wvn" { exec = "rosnode kill /wild_visual_navigation_node"; diff --git a/wild_visual_navigation_ros/config/recording/dodo.yaml b/wild_visual_navigation_ros/config/recording/dodo.yaml index 8c65f5f4..ef0850e4 100644 --- a/wild_visual_navigation_ros/config/recording/dodo.yaml +++ b/wild_visual_navigation_ros/config/recording/dodo.yaml @@ -74,6 +74,7 @@ lpc: - /resource_database/reports - /sensors/battery_voltage - /soft_emcy_stop + - /clock npc: ########################################################### - /lidar/packets @@ -123,7 +124,8 @@ jetson: - /elevation_mapping/elevation_map_recordable - /elevation_mapping/statistics - /elevation_mapping/elevation_map_raw - ############################################################################# + # - /elevation_mapping/semantic_map_raw + ############################################################################ - /chrony_monitor_jetson/status - /cpu_loupe_jetson/cpu_loupe - /disk_monitor_jetson/status @@ -136,6 +138,8 @@ jetson: - /pinger_jetson_to_lpc/ping - /pinger_jetson_to_npc/ping - /pinger_jetson_to_apc/ping - - /wild_visual_navigation_node/front/image_input - - /wild_visual_navigation_node/front/debug/last_node_image_overlay - - /wild_visual_navigation_visu_traversability/traversability_overlayed \ No newline at end of file + - /hdr_camera/image_raw/compressed + - /hdr_camera/camera_info + # - /wild_visual_navigation_node/front/image_input + # - /wild_visual_navigation_node/front/debug/last_node_image_overlay + # - /wild_visual_navigation_visu_traversability/traversability_overlayed From fb9d35f2990c74a05fe9e58cac36f41d27cbb601 Mon Sep 17 00:00:00 2001 From: RobinSchmid7 Date: Wed, 20 Dec 2023 12:49:04 +0100 Subject: [PATCH 43/43] Change rviz config --- .../config/rviz/anymal_dodo.rviz | 20 +++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/wild_visual_navigation_ros/config/rviz/anymal_dodo.rviz b/wild_visual_navigation_ros/config/rviz/anymal_dodo.rviz index 137174a1..ca4d2ee1 100644 --- a/wild_visual_navigation_ros/config/rviz/anymal_dodo.rviz +++ b/wild_visual_navigation_ros/config/rviz/anymal_dodo.rviz @@ -7,12 +7,12 @@ Panels: - /Global Options1 - /TF1/Tree1 - /Wild Visual Navigation1 + - /Wild Visual Navigation1/Self Supervision1 - /Wild Visual Navigation1/Depth Sensors1/realsense-front1 - /Elevation Map WIFI1/Local Planner (SDF)1 - /Elevation Map RAW1 - - /Elevation Map RAW1/Visual Traversability1 Splitter Ratio: 0.5789473652839661 - Tree Height: 685 + Tree Height: 528 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -472,13 +472,13 @@ Visualization Manager: - Class: rviz/Group Displays: - Class: rviz/Marker - Enabled: false + Enabled: true Marker Topic: /wild_visual_navigation_node/graph_footprints Name: Footprint Namespaces: - {} + footprints: true Queue Size: 100 - Value: false + Value: true - Alpha: 1 Buffer Length: 1 Class: rviz/Path @@ -1447,7 +1447,7 @@ Window Geometry: collapsed: false F Traversability Overlay Replay: collapsed: false - Height: 1376 + Height: 1024 Hide Left Dock: false Hide Right Dock: true Input Image: @@ -1462,7 +1462,7 @@ Window Geometry: collapsed: false Placeholder: collapsed: false - QMainWindow State: 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 + QMainWindow State: 000000ff00000000fd00000004000000000000026c0000024dfc0200000013fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000005fb000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000001960000024d000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d00650072006100000002ea000000c80000000000000000fb00000036005300750070006500720076006900730069006f006e0020002d00200049006d0061006700650020004c006100620065006c006500640000000259000000520000000000000000fb00000024005300750070006500720076006900730069006f006e0020002d0020004d00610073006b000000027e000000b80000000000000000fb0000000a0049006d00610067006500000002eb000001280000000000000000fb000000140043006f006e0066006900640065006e0063006500000003b3000000c60000000000000000fb00000028004600200043006f006e0066006900640065006e006300650020004f007600650072006c0061007900000002be000000160000001600fffffffb00000030004600200054007200610076006500720073006100620069006c0069007400790020004f007600650072006c0061007900000005290000001a0000001600fffffffb00000036004600200043006f006e0066006900640065006e006300650020004f007600650072006c006100790020005200650070006c0061007900000002fc0000001e0000001600fffffffb0000003e004600200054007200610076006500720073006100620069006c0069007400790020004f007600650072006c006100790020005200650070006c0061007900000004a40000009f0000001600fffffffb00000028004600200054007200610076006500720073006100620069006c00690074007900200052006100770000000482000000c10000000000000000fb00000028004600200054007200610076006500720073006100620069006c00690074007900200052006100770000000482000000c10000000000000000000000010000010f0000025ffc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000017c0000025f000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b2000000000000000000000002000009a600000153fc010000000cfb0000001c0049006d00610067006500200046006500610074007500720065007300000000bf000003690000000000000000fc00000000000001af0000000000fffffffa000000000200000004fb0000000a0049006d0061006700650000000000ffffffff0000000000000000fb00000026004100420020005700690064006500200041006e0067006c0065002000460072006f006e00740000000000ffffffff0000001600fffffffb0000002400460072006f006e0074002000280063006f006d007000720065007300730065006400290000000000ffffffff0000000000000000fb0000001e00410053002000460072006f006e00740020002800630061006d00340029000000003d0000010c0000000000000000fc000000000000026f0000008600fffffffa000000000200000003fb000000160049006e00700075007400200049006d006100670065010000003d000001ac0000001600fffffffb0000001a005200200049006e00700075007400200049006d0061006700650000000000ffffffff0000001600fffffffb0000001a004c00200049006e00700075007400200049006d0061006700650000000000ffffffff0000001600fffffffb0000000a0049006d00610067006501000003240000018a0000000000000000fb0000002c0054007200610076006500720073006100620069006c0069007400790020004f007600650072006c006100790100000275000002c4000000c800fffffffc0000053f00000224000000b000fffffffa000000020200000006fb000000160046002000540072006100760020004f0076006500720000000000ffffffff0000000000000000fb0000003e004600200054007200610076006500720073006100620069006c0069007400790020004f007600650072006c006100790020005200650070006c006100790100000000ffffffff0000000000000000fb000000240054007200610076006500720073006100620069006c0069007400790020005200610077010000003d0000012b0000001600fffffffb00000028005200200054007200610076006500720073006100620069006c00690074007900200052006100770000000000ffffffff0000001600fffffffb00000028004c00200054007200610076006500720073006100620069006c00690074007900200052006100770000000000ffffffff0000001600fffffffb00000030004600200054007200610076006500720073006100620069006c0069007400790020004f007600650072006c006100790000000000ffffffff0000000000000000fc000007690000023d0000009700fffffffa000000010200000005fb00000036004600200043006f006e0066006900640065006e006300650020004f007600650072006c006100790020005200650070006c006100790100000000ffffffff0000000000000000fb0000001a004c006500610072006e0069006e00670020004d00610073006b010000003d0000012b0000001600fffffffb00000020005200200043006f006e0066006900640065006e0063006500200052006100770000000000ffffffff0000001600fffffffb00000020004c00200043006f006e0066006900640065006e0063006500200052006100770000000000ffffffff0000001600fffffffb00000028004600200043006f006e0066006900640065006e006300650020004f007600650072006c006100790000000000ffffffff0000000000000000fb000000160050006c0061006300650068006f006c006400650072000000081e0000019a0000008500fffffffb000000160049006e00700075007400200049006d006100670065010000031e000001790000000000000000fb00000036005300750070006500720076006900730069006f006e0020002d00200049006d0061006700650020004c006100620065006c006500640100000659000001af0000000000000000fb00000024005300750070006500720076006900730069006f006e0020002d0020004d00610073006b010000080e000001aa0000000000000000fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009b800000061fc010000000dfb000000200052006500700072006f006a00650063007400650064002000500061007400680000000000000000ab000000ab00fffffffb00000024005300750070006500720076006900730069006f006e0020005300690067006e0061006c00000000b100000797000000b000fffffffb0000001c0054007200610076006500720073006100620069006c006900740079000000084e000000dd0000009100fffffffb000000160055006e006300650072007400610069006e007400790000000931000000870000008500fffffffb000000160050006c0061006300650068006f006c0064006500720000000000000009b80000008500fffffffb0000001c0054007200610076006500720073006100620069006c006900740079010000031f0000017d0000000000000000fb000000160055006e006300650072007400610069006e0074007901000004a2000002960000000000000000fb000000140043006f006e0066006900640065006e006300650000000000000003d40000000000000000fb0000001c0054007200610076006500720073006100620069006c0069007400790000000231000001f20000000000000000fb0000001c004100530020004c0065006600740020002800630061006d003300290000000000000007380000000000000000fb0000001e004100530020005200690067006800740020002800630061006d0035002900000003f0000003480000000000000000fb0000000800540069006d00650000000000000007800000041800fffffffb0000000800540069006d00650100000000000004500000000000000000000007340000024d00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 R Confidence Raw: collapsed: false R Input Image: @@ -1489,6 +1489,6 @@ Window Geometry: collapsed: false Views: collapsed: true - Width: 2488 - X: 72 - Y: 27 + Width: 2470 + X: 81 + Y: 56