From e80d803b74ed6fc597640bf3a637018a9aca6f1a Mon Sep 17 00:00:00 2001 From: Jonas Frey Date: Thu, 8 Feb 2024 21:47:57 +0100 Subject: [PATCH] failing learning node --- .../traversability_estimator.py | 11 +++++------ .../anymal_sensor_parameter.yaml | 18 +++++++++--------- .../scripts/rosbag_play.sh | 3 +-- .../scripts/wvn_learning_node.py | 1 + 4 files changed, 16 insertions(+), 17 deletions(-) diff --git a/wild_visual_navigation/traversability_estimator/traversability_estimator.py b/wild_visual_navigation/traversability_estimator/traversability_estimator.py index 209128ca..3ab351dd 100644 --- a/wild_visual_navigation/traversability_estimator/traversability_estimator.py +++ b/wild_visual_navigation/traversability_estimator/traversability_estimator.py @@ -354,13 +354,12 @@ def add_supervision_node(self, pnode: SupervisionNode): list(self._mission_graph._graph.nodes._nodes.items())[self._debug_info_node_count :] ): node, values = ele - if last_mission_node.timestamp - values["timestamp"] > 20: + if last_mission_node.timestamp - values["timestamp"] > 30: node.clear_debug_data() self._debug_info_node_count += 1 - length = len(list(self._mission_graph._graph.nodes._nodes.items())) - print( - f"cleaned node {self._debug_info_node_count} nodes {self._debug_info_node_count}, length {length}" - ) + #length = len(list(self._mission_graph._graph.nodes._nodes.items())) + else: + break # Get all mission nodes within a range mission_nodes = self._mission_graph.get_nodes_within_radius_range( @@ -368,6 +367,7 @@ def add_supervision_node(self, pnode: SupervisionNode): ) if len(mission_nodes) < 1: + return False # Set color @@ -375,7 +375,6 @@ def add_supervision_node(self, pnode: SupervisionNode): # New implementation B = len(mission_nodes) - # Prepare batches K = torch.eye(4, device=self._device).repeat(B, 1, 1) supervision_masks = torch.zeros(last_mission_node.supervision_mask.shape, device=self._device).repeat( diff --git a/wild_visual_navigation_anymal/config/elevation_mapping_cupy/anymal_sensor_parameter.yaml b/wild_visual_navigation_anymal/config/elevation_mapping_cupy/anymal_sensor_parameter.yaml index a9f93e9a..bad469c6 100644 --- a/wild_visual_navigation_anymal/config/elevation_mapping_cupy/anymal_sensor_parameter.yaml +++ b/wild_visual_navigation_anymal/config/elevation_mapping_cupy/anymal_sensor_parameter.yaml @@ -29,15 +29,15 @@ subscribers: data_type: pointcloud # Semantics - # front_wide_angle: - # topic_name: /wide_angle_camera_front/image_color_rect - # camera_info_topic_name: /wide_angle_camera_front/camera_info - # data_type: image - - # rear_wide_angle: - # topic_name: /wide_angle_camera_rear/image_color_rect - # camera_info_topic_name: /wide_angle_camera_rear/camera_info - # data_type: image + front_wide_angle: + topic_name: /wide_angle_camera_front/image_color_rect + camera_info_topic_name: /wide_angle_camera_front/camera_info + data_type: image + + rear_wide_angle: + topic_name: /wide_angle_camera_rear/image_color_rect + camera_info_topic_name: /wide_angle_camera_rear/camera_info + data_type: image # Traversability # channels: ["visual_traversability"] diff --git a/wild_visual_navigation_ros/scripts/rosbag_play.sh b/wild_visual_navigation_ros/scripts/rosbag_play.sh index befd94a5..a6a39dad 100755 --- a/wild_visual_navigation_ros/scripts/rosbag_play.sh +++ b/wild_visual_navigation_ros/scripts/rosbag_play.sh @@ -4,7 +4,7 @@ args="" for option in "$@"; do if [ "$option" == "--sem" ]; then args="$args /elevation_mapping/elevation_map_raw:=/recorded/elevation_mapping/elevation_map_raw \ - /elevation_mapping/semantic_map_raw:=/recorded/elevation_mapping/semantic_map_raw" + /elevation_mapping/semantic_map:=/recorded/elevation_mapping/semantic_map" elif [ "$option" == "--wvn" ]; then args="$args /wild_visual_navigation_node/front/camera_info:=/recorded/wild_visual_navigation_node/front/camera_info \ /wild_visual_navigation_node/front/confidence:=/recorded/wild_visual_navigation_node/front/confidence \ @@ -18,7 +18,6 @@ for option in "$@"; do /wild_visual_navigation_node/rear/confidence:=/recorded/wild_visual_navigation_node/rear/confidence \ /wild_visual_navigation_node/rear/image_input:=/recorded/wild_visual_navigation_node/rear/image_input \ /wild_visual_navigation_node/rear/traversability:=/recorded/wild_visual_navigation_node/rear/traversability \ - /wild_visual_navigation_node/robot_state:=/recorded/wild_visual_navigation_node/robot_state \ /wild_visual_navigation_node/supervision_graph:=/recorded/wild_visual_navigation_node/supervision_graph \ /wild_visual_navigation_visu_front_trav/traversability:=/recorded/wild_visual_navigation_visu_front_trav/traversability \ /wild_visual_navigation_visu_rear_trav/traversability:=/recorded/wild_visual_navigation_visu_rear_trav/traversability" diff --git a/wild_visual_navigation_ros/scripts/wvn_learning_node.py b/wild_visual_navigation_ros/scripts/wvn_learning_node.py index f4489f05..85c08f8d 100644 --- a/wild_visual_navigation_ros/scripts/wvn_learning_node.py +++ b/wild_visual_navigation_ros/scripts/wvn_learning_node.py @@ -395,6 +395,7 @@ def learning_thread_loop(self): os.remove(fn) torch.save(new_model_state_dict, fn) self._last_checkpoint_ts = ts + print("Update model. Valid Nodes: ", self._traversability_estimator._mission_graph.get_num_valid_nodes(), " steps: ", self._traversability_estimator._step) rate.sleep()