Add variable map and image size

leggedrobotics · Oct 3, 2023 · 068f119 · 068f119
1 parent 270099b
commit 068f119
Show file tree

Hide file tree

Showing 3 changed files with 17 additions and 13 deletions.
diff --git a/wild_visual_navigation/image_projector/image_projector.py b/wild_visual_navigation/image_projector/image_projector.py
@@ -205,8 +205,11 @@ def project_and_render_on_map(
         # self.masks = self.masks * 0.0
         B = self.camera.batch_size
         C = 3  # RGB channel output
-        H = self.camera.height.item()
-        W = self.camera.width.item()
+        # H = self.camera.height.item()
+        # W = self.camera.width.item()
+        # Camera height and width are identical here
+        H = map_size
+        W = map_size
         self.masks = torch.zeros((B, C, H, W), dtype=torch.float32, device=self.camera.camera_matrix.device)
         image_overlay = image
 

diff --git a/wild_visual_navigation/traversability_estimator/traversability_estimator.py b/wild_visual_navigation/traversability_estimator/traversability_estimator.py
@@ -302,7 +302,8 @@ def add_mission_node(self, node: MissionNode, verbose: bool = False, update_feat
                 print(s)
 
             # Project past footprints on current image
-            supervision_mask = torch.ones(node.image.shape).to(self._device) * torch.nan
+            # supervision_mask = torch.ones(node.image.shape).to(self._device) * torch.nan
+            supervision_mask = torch.ones((3, self._map_size, self._map_size)).to(self._device) * torch.nan
 
             # Finally overwrite the current mask
             node.supervision_mask = supervision_mask
@@ -427,14 +428,14 @@ def add_proprio_node(self, pnode: ProprioceptionNode, projection_mode: str = "im
                     ))
 
                     # Write as grid map msg and save as rosbag
-                    mask_torch = mask.cpu().numpy()[np.newaxis, ...].astype(np.uint8)
-                    mask_msg = self.torch_array_to_grid_map(mask_torch, res=0.1, layers=["target"],
-                                                            timestamp=rospy.Time.from_sec(mnode.timestamp),
-                                                            reference_frame="odom", x=0, y=0)
-                    # Hack to also publish a clock and record a new bag
-                    # print(rospy.Time.from_sec(mnode.timestamp))
-                    self.pub_clock.publish(rospy.Time.from_sec(mnode.timestamp))
-                    self.pub_grid_map.publish(mask_msg)
+                    # mask_torch = mask.cpu().numpy()[np.newaxis, ...].astype(np.uint8)
+                    # mask_msg = self.torch_array_to_grid_map(mask_torch, res=0.1, layers=["target"],
+                    #                                         timestamp=rospy.Time.from_sec(mnode.timestamp),
+                    #                                         reference_frame="odom", x=0, y=0)
+                    # # Hack to also publish a clock and record a new bag
+                    # # print(rospy.Time.from_sec(mnode.timestamp))
+                    # self.pub_clock.publish(rospy.Time.from_sec(mnode.timestamp))
+                    # self.pub_grid_map.publish(mask_msg)
 
                     # Save mask as jpg
                     mask_jpg = mask.cpu().numpy().astype(np.uint8) * 255

diff --git a/wild_visual_navigation_ros/config/wild_visual_navigation/default.yaml b/wild_visual_navigation_ros/config/wild_visual_navigation/default.yaml
@@ -35,8 +35,8 @@ robot_max_velocity: 0.8
 traversability_radius: 10.0 # meters
 image_graph_dist_thr: 0.2 # meters
 proprio_graph_dist_thr: 0.1 # meters
-network_input_image_height: 128 # 448
-network_input_image_width: 128 # 448
+network_input_image_height: 540
+network_input_image_width: 720
 segmentation_type: "slic"
 feature_type: "dino"
 dino_patch_size: 8 # 8 or 16; 8 is finer