diff --git a/wild_visual_navigation_ros/config/wild_visual_navigation/inputs/alphasense.yaml b/wild_visual_navigation_ros/config/wild_visual_navigation/inputs/alphasense.yaml index 87528b7e..82ab4cb2 100644 --- a/wild_visual_navigation_ros/config/wild_visual_navigation/inputs/alphasense.yaml +++ b/wild_visual_navigation_ros/config/wild_visual_navigation/inputs/alphasense.yaml @@ -5,18 +5,21 @@ camera_topics: use_for_training: true publish_confidence: true publish_input_image: true + scheduler_weight: 1 left: image_topic: "/alphasense_driver_ros/cam3/debayered" info_topic: "/alphasense_driver_ros/cam3/camera_info" use_for_training: false publish_confidence: true publish_input_image: true + scheduler_weight: 1 right: image_topic: "/alphasense_driver_ros/cam5/debayered" info_topic: "/alphasense_driver_ros/cam5/camera_info" use_for_training: false publish_confidence: true publish_input_image: true + scheduler_weight: 1 # Provides 1080 (height) x 1920 (width) images network_input_image_height: 224 # 448 diff --git a/wild_visual_navigation_ros/config/wild_visual_navigation/inputs/alphasense_compressed.yaml b/wild_visual_navigation_ros/config/wild_visual_navigation/inputs/alphasense_compressed.yaml index f19e5d1a..3017f436 100644 --- a/wild_visual_navigation_ros/config/wild_visual_navigation/inputs/alphasense_compressed.yaml +++ b/wild_visual_navigation_ros/config/wild_visual_navigation/inputs/alphasense_compressed.yaml @@ -5,18 +5,21 @@ camera_topics: use_for_training: true publish_confidence: true publish_input_image: true + scheduler_weight: 1 left: image_topic: "/alphasense_driver_ros/cam3/color_rect/image/compressed" info_topic: "/alphasense_driver_ros/cam3/color_rect/camera_info" use_for_training: false publish_confidence: true publish_input_image: true + scheduler_weight: 1 right: image_topic: "/alphasense_driver_ros/cam5/color_rect/image/compressed" info_topic: "/alphasense_driver_ros/cam5/color_rect/camera_info" use_for_training: false publish_confidence: true publish_input_image: true + scheduler_weight: 1 # Provides 1080 (height) x 1920 (width) images network_input_image_height: 224 # 448 diff --git a/wild_visual_navigation_ros/config/wild_visual_navigation/inputs/alphasense_compressed_front.yaml b/wild_visual_navigation_ros/config/wild_visual_navigation/inputs/alphasense_compressed_front.yaml index 6f252694..6e284945 100644 --- a/wild_visual_navigation_ros/config/wild_visual_navigation/inputs/alphasense_compressed_front.yaml +++ b/wild_visual_navigation_ros/config/wild_visual_navigation/inputs/alphasense_compressed_front.yaml @@ -5,6 +5,7 @@ camera_topics: use_for_training: true publish_confidence: true publish_input_image: true + scheduler_weight: 1 # Provides 1080 (height) x 1920 (width) images network_input_image_height: 224 # 448 diff --git a/wild_visual_navigation_ros/config/wild_visual_navigation/inputs/alphasense_resize.yaml b/wild_visual_navigation_ros/config/wild_visual_navigation/inputs/alphasense_resize.yaml index 3d806b0c..bd7d2c61 100644 --- a/wild_visual_navigation_ros/config/wild_visual_navigation/inputs/alphasense_resize.yaml +++ b/wild_visual_navigation_ros/config/wild_visual_navigation/inputs/alphasense_resize.yaml @@ -5,18 +5,21 @@ camera_topics: use_for_training: true publish_confidence: true publish_input_image: true + scheduler_weight: 1 left: image_topic: "/alphasense_driver_ros/cam3/color_rect_resize/image" info_topic: "/alphasense_driver_ros/cam3/color_rect_resize/camera_info" use_for_training: false publish_confidence: true publish_input_image: true + scheduler_weight: 1 right: image_topic: "/alphasense_driver_ros/cam5/color_rect_resize/image" info_topic: "/alphasense_driver_ros/cam5/color_rect_resize/camera_info" use_for_training: false publish_confidence: true publish_input_image: true + scheduler_weight: 1 # Provides 1080 (height) x 1920 (width) images network_input_image_height: 224 # 448 diff --git a/wild_visual_navigation_ros/config/wild_visual_navigation/inputs/realsense_front.yaml b/wild_visual_navigation_ros/config/wild_visual_navigation/inputs/realsense_front.yaml index 3bc33ece..c98d812d 100644 --- a/wild_visual_navigation_ros/config/wild_visual_navigation/inputs/realsense_front.yaml +++ b/wild_visual_navigation_ros/config/wild_visual_navigation/inputs/realsense_front.yaml @@ -5,6 +5,7 @@ camera_topics: use_for_training: true publish_confidence: true publish_input_image: true + scheduler_weight: 1 # Provides 480 (height) x 848 (width) images network_input_image_height: 448 # 448 diff --git a/wild_visual_navigation_ros/config/wild_visual_navigation/inputs/realsense_rear.yaml b/wild_visual_navigation_ros/config/wild_visual_navigation/inputs/realsense_rear.yaml index 35ad15fc..1cb0ea2c 100644 --- a/wild_visual_navigation_ros/config/wild_visual_navigation/inputs/realsense_rear.yaml +++ b/wild_visual_navigation_ros/config/wild_visual_navigation/inputs/realsense_rear.yaml @@ -5,6 +5,7 @@ camera_topics: use_for_training: true publish_confidence: true publish_input_image: true + scheduler_weight: 1 # Provides 480 (height) x 848 (width) images network_input_image_height: 224 # 448 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 index 41106092..538caac7 100644 --- 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 @@ -5,6 +5,7 @@ camera_topics: use_for_training: true publish_confidence: true publish_input_image: true + scheduler_weight: 1 # Provides 1080 (height) x 1920 (width) images network_input_image_height: 448 # 448 diff --git a/wild_visual_navigation_ros/config/wild_visual_navigation/inputs/wide_angle_front_compressed.yaml b/wild_visual_navigation_ros/config/wild_visual_navigation/inputs/wide_angle_front_compressed.yaml index df07ccb8..0f6a60fc 100644 --- a/wild_visual_navigation_ros/config/wild_visual_navigation/inputs/wide_angle_front_compressed.yaml +++ b/wild_visual_navigation_ros/config/wild_visual_navigation/inputs/wide_angle_front_compressed.yaml @@ -5,6 +5,7 @@ camera_topics: use_for_training: true publish_confidence: true publish_input_image: true + scheduler_weight: 1 # Provides 1080 (height) x 1920 (width) images network_input_image_height: 224 # 448 diff --git a/wild_visual_navigation_ros/config/wild_visual_navigation/inputs/wide_angle_front_resize.yaml b/wild_visual_navigation_ros/config/wild_visual_navigation/inputs/wide_angle_front_resize.yaml index d4d63fe0..a848c505 100644 --- a/wild_visual_navigation_ros/config/wild_visual_navigation/inputs/wide_angle_front_resize.yaml +++ b/wild_visual_navigation_ros/config/wild_visual_navigation/inputs/wide_angle_front_resize.yaml @@ -5,6 +5,7 @@ camera_topics: use_for_training: true publish_confidence: true publish_input_image: true + scheduler_weight: 1 # Provides 1080 (height) x 1920 (width) images network_input_image_height: 224 # 448 diff --git a/wild_visual_navigation_ros/launch/wild_visual_navigation.launch b/wild_visual_navigation_ros/launch/wild_visual_navigation.launch index adec6014..5fdfa90a 100644 --- a/wild_visual_navigation_ros/launch/wild_visual_navigation.launch +++ b/wild_visual_navigation_ros/launch/wild_visual_navigation.launch @@ -1,6 +1,6 @@ - + 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 98439e71..61de24b1 100644 --- a/wild_visual_navigation_ros/scripts/wvn_feature_extractor_node.py +++ b/wild_visual_navigation_ros/scripts/wvn_feature_extractor_node.py @@ -113,7 +113,7 @@ def setup_ros(self, setup_fully=True): # Image callback self._camera_handler = {} - self._scheduler = Scheduler() + self._camera_scheduler = Scheduler() if self._ros_params.verbose: # DEBUG Logging @@ -132,7 +132,8 @@ def setup_ros(self, setup_fully=True): self._ros_params.camera_topics[cam]["name"] = cam # Add to scheduler - self._scheduler.add_process(cam, 1) + rospy.logwarn(self._ros_params.camera_topics) + self._camera_scheduler.add_process(cam, self._ros_params.camera_topics[cam]["scheduler_weight"]) # Camera info rospy.loginfo(f"[{self._node_name}] Waiting for camera info topic...") @@ -276,7 +277,7 @@ def image_callback(self, image_msg: Image, cam: str): # info_msg: CameraInfo return # Check the scheduler - if self._scheduler.get() != cam: + if self._camera_scheduler.get() != cam: return else: if self._ros_params.verbose: @@ -415,7 +416,7 @@ def image_callback(self, image_msg: Image, cam: str): # info_msg: CameraInfo raise Exception("Error in image callback") # Step scheduler - self._scheduler.step() + self._camera_scheduler.step() def load_model(self, stamp): """Method to load the new model weights to perform inference on the incoming images