From 78db656f6fc0a93ac33f0a31776227956ed899c5 Mon Sep 17 00:00:00 2001 From: RobinSchmid7 Date: Wed, 10 Jan 2024 18:00:14 +0100 Subject: [PATCH] changes from deployment --- .../config/wild_visual_navigation/default.yaml | 2 +- .../config/wild_visual_navigation/inputs/hdr.yaml | 4 ++-- .../scripts/wvn_feature_extractor_node.py | 4 ++-- wild_visual_navigation_ros/scripts/wvn_learning_node.py | 9 ++++----- 4 files changed, 9 insertions(+), 10 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 71f65689..fcd7a6ad 100644 --- a/wild_visual_navigation_ros/config/wild_visual_navigation/default.yaml +++ b/wild_visual_navigation_ros/config/wild_visual_navigation/default.yaml @@ -57,7 +57,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 use_debug_for_desired: false diff --git a/wild_visual_navigation_ros/config/wild_visual_navigation/inputs/hdr.yaml b/wild_visual_navigation_ros/config/wild_visual_navigation/inputs/hdr.yaml index 70e4ad46..a0c27733 100644 --- a/wild_visual_navigation_ros/config/wild_visual_navigation/inputs/hdr.yaml +++ b/wild_visual_navigation_ros/config/wild_visual_navigation/inputs/hdr.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 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 bccced1e..22f81a71 100644 --- a/wild_visual_navigation_ros/scripts/wvn_feature_extractor_node.py +++ b/wild_visual_navigation_ros/scripts/wvn_feature_extractor_node.py @@ -401,10 +401,10 @@ 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/hdr.yaml wvn_feature_extractor_node" + f"rosparam load {wvn_path}/config/wild_visual_navigation/inputs/hdr_compressed.yaml wvn_feature_extractor_node" ) print( - f"rosparam load {wvn_path}/config/wild_visual_navigation/inputs/hdr.yaml wvn_feature_extractor_node" + f"rosparam load {wvn_path}/config/wild_visual_navigation/inputs/hdr_compressed.yaml wvn_feature_extractor_node" ) wvn = WvnFeatureExtractor() diff --git a/wild_visual_navigation_ros/scripts/wvn_learning_node.py b/wild_visual_navigation_ros/scripts/wvn_learning_node.py index 54d7ea94..5481611a 100644 --- a/wild_visual_navigation_ros/scripts/wvn_learning_node.py +++ b/wild_visual_navigation_ros/scripts/wvn_learning_node.py @@ -370,7 +370,7 @@ def setup_ros(self, setup_fully=True): self.camera_topics[cam]["name"] = cam # Set subscribers - if self.mode == WVNMode.DEBUG: + if self.mode == WVNMode.DEBUG or self.mode == WVNMode.ONLINE: # In debug mode additionally send the image to the callback function self._visualizer = LearningVisualizer() @@ -628,7 +628,6 @@ def imagefeat_callback(self, *args): if self.verbose: print(f"\nImage callback: {camera_options['name']}... ", end="") try: - # Run the callback so as to match the desired rate ts = imagefeat_msg.header.stamp.to_sec() if abs(ts - self.last_image_ts) < 1.0 / self.image_callback_rate: @@ -677,7 +676,7 @@ def imagefeat_callback(self, *args): 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: + if self.mode == WVNMode.DEBUG or self.mode == WVNMode.ONLINE: torch_image = rc.ros_image_to_torch( image_msg, desired_encoding="passthrough", device=self.device ).clone() @@ -702,7 +701,7 @@ def imagefeat_callback(self, *args): # Add node to graph added_new_node = self.traversability_estimator.add_mission_node(mission_node, update_features=False) - if self.mode == WVNMode.DEBUG: + if self.mode == WVNMode.DEBUG or self.mode == WVNMode.ONLINE: # Publish current predictions self.visualize_mission_graph() # Publish supervision data depending on the mode @@ -876,7 +875,7 @@ def visualize_image_overlay(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/wide_angle_front_compressed.yaml wvn_learning_node" + f"rosparam load {wvn_path}/config/wild_visual_navigation/inputs/hdr_compressed.yaml wvn_learning_node" ) wvn = WvnLearning()