fix merge

leggedrobotics · Jan 10, 2024 · 0918b4f · 0918b4f
2 parents 78db656 + 0eb6f82
commit 0918b4f
Show file tree

Hide file tree

Showing 5 changed files with 17 additions and 4 deletions.
diff --git a/wild_visual_navigation_msgs/CMakeLists.txt b/wild_visual_navigation_msgs/CMakeLists.txt
@@ -15,6 +15,7 @@ add_message_files(
   RobotState.msg
   SystemState.msg
   ImageFeatures.msg
+  ChannelInfo.msg
 )
 
 add_service_files(

diff --git a/wild_visual_navigation_msgs/msg/ChannelInfo.msg b/wild_visual_navigation_msgs/msg/ChannelInfo.msg
@@ -0,0 +1,2 @@
+Header header
+string[] channels       # channel names for each layer
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
@@ -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
+network_input_image_height: 224 # 448
+network_input_image_width: 224 # 448
diff --git a/wild_visual_navigation_ros/launch/wild_visual_navigation.launch b/wild_visual_navigation_ros/launch/wild_visual_navigation.launch
@@ -5,7 +5,7 @@
   <arg name="params_file"       default="$(find wild_visual_navigation_ros)/config/wild_visual_navigation/default.yaml"/>
   <arg name="overlay_images"    default="True"/>
   <arg name="resize_images_alphasense"     default="False"/>
-  <arg name="resize_images_wide_angle_front"     default="False"/>
+  <arg name="resize_images_wide_angle_front"     default="True"/>
   <arg name="resize_images_hdr"     default="True"/>
   <arg name="reload_default_params"    default="False"/>
 

diff --git a/wild_visual_navigation_ros/scripts/wvn_feature_extractor_node.py b/wild_visual_navigation_ros/scripts/wvn_feature_extractor_node.py
@@ -9,6 +9,7 @@
 from wild_visual_navigation.learning.model import get_model
 from wild_visual_navigation.utils import ConfidenceGenerator
 from wild_visual_navigation.learning.utils import AnomalyLoss
+from wild_visual_navigation_msgs.msg import ChannelInfo
 
 import rospy
 from sensor_msgs.msg import Image, CameraInfo, CompressedImage
@@ -197,6 +198,7 @@ def setup_ros(self, setup_fully=True):
             msg.P = image_projector.scaled_camera_matrix[0, :3, :4].cpu().numpy().flatten().tolist()
 
             self.camera_topics[cam]["camera_info_msg_out"] = msg
+            self.camera_topics[cam]["channel_info_msg_out"] = ChannelInfo()
             self.camera_topics[cam]["image_projector"] = image_projector
 
             # Set subscribers
@@ -220,8 +222,12 @@ def setup_ros(self, setup_fully=True):
             # Set publishers
             trav_pub = rospy.Publisher(f"/wild_visual_navigation_node/{cam}/traversability", Image, queue_size=10)
             info_pub = rospy.Publisher(f"/wild_visual_navigation_node/{cam}/camera_info", CameraInfo, queue_size=10)
+            channel_info_pub = rospy.Publisher(
+                f"/wild_visual_navigation_node/{cam}/channel_info", ChannelInfo, queue_size=10
+            )
             self.camera_handler[cam]["trav_pub"] = trav_pub
             self.camera_handler[cam]["info_pub"] = info_pub
+            self.camera_handler[cam]["channel_info_pub"] = channel_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
@@ -316,6 +322,11 @@ def image_callback(self, image_msg: Image, cam: str):  #  info_msg: CameraInfo
         msg.header = image_msg.header
         self.camera_handler[cam]["info_pub"].publish(msg)
 
+        msg = self.camera_topics[cam]["channel_info_msg_out"]
+        msg.channels = ["anomaly"]
+        msg.header = image_msg.header
+        self.camera_handler[cam]["channel_info_pub"].publish(msg)
+
         # Publish image
         if self.camera_topics[cam]["publish_input_image"]:
             msg = rc.numpy_to_ros_image((torch_image.permute(1, 2, 0) * 255).cpu().numpy().astype(np.uint8), "rgb8")
@@ -405,7 +416,6 @@ def load_model(self):
         )
         print(
             f"rosparam load {wvn_path}/config/wild_visual_navigation/inputs/hdr_compressed.yaml wvn_feature_extractor_node"
-        )
 
     wvn = WvnFeatureExtractor()
     rospy.spin()