failing learning node

leggedrobotics · Feb 8, 2024 · e80d803 · e80d803
1 parent f06e079
commit e80d803
Show file tree

Hide file tree

Showing 4 changed files with 16 additions and 17 deletions.
diff --git a/wild_visual_navigation/traversability_estimator/traversability_estimator.py b/wild_visual_navigation/traversability_estimator/traversability_estimator.py
@@ -354,28 +354,27 @@ 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(
                 last_mission_node, 0, self._supervision_graph.max_distance
             )
 
             if len(mission_nodes) < 1:
+
                 return False
 
             # Set color
             color = torch.ones((3,), device=self._device)
 
             # 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
@@ -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
@@ -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
@@ -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()