Robot changes

wild_visual_navigation_ros/config/wild_visual_navigation/default.yaml

@@ -22,18 +22,18 @@ image_graph_dist_thr: 0.2 # meters
 proprio_graph_dist_thr: 0.1 # meters
 network_input_image_height: 224 # 448
 network_input_image_width: 224 # 448
-segmentation_type: "slic"
+segmentation_type: "random"
 feature_type: "dino"
 dino_patch_size: 8 # DINO only, 16
-slic_num_components: 500
+slic_num_components: 100
 confidence_std_factor: 4.0
 scale_traversability: False  # This parameter needs to be false when using the anomaly detection model
 scale_traversability_max_fpr: 0.25
 min_samples_for_training: 5
-prediction_per_pixel: false
+prediction_per_pixel: True
 traversability_threshold: 0.55
-clip_to_binary: false
-vis_training_samples: true
+clip_to_binary: False
+vis_training_samples: True
 
 # Supervision Generator
 untraversable_thr: 0.01
@@ -57,7 +57,7 @@ print_image_callback_time: false
 print_proprio_callback_time: false
 log_time: false
 log_confidence: false
-verbose: true
+verbose: false
 debug_supervision_node_index_from_last: 10
 use_debug_for_desired: false
 

wild_visual_navigation_ros/config/wild_visual_navigation/inputs/wide_angle_front_resize.yaml

@@ -0,0 +1,11 @@
+camera_topics:
+  front:
+    image_topic: "/wide_angle_camera_front/image_color_resize"
+    info_topic: "/wide_angle_camera_front_resize/camera_info"
+    use_for_training: true
+    publish_confidence: true
+    publish_input_image: true
+
+# Provides 1080 (height) x 1920 (width) images
+network_input_image_height: 224 # 448
+network_input_image_width: 224 # 448

wild_visual_navigation_ros/launch/resize_images.launch → wild_visual_navigation_ros/launch/resize_images_alphasense.launch

@@ -25,23 +25,23 @@
     <remap from="~camera_info" to="/alphasense_driver_ros/cam3/color_rect_resize/camera_info" />
     <remap from="image" to="/alphasense_driver_ros/cam3/color_rect/image" />
     <remap from="~image" to="/alphasense_driver_ros/cam3/color_rect_resize/image" />
-    <param name="scale_height" value="0.2074077" />
+    <param name="scale_width" value="0.2074077" />
     <param name="scale_height" value="0.2074077" />
   </node>
     <node if="$(arg resize_alphasense_cam4)" pkg="nodelet" type="nodelet" name="image_proc_nodelet_cam4" args="load image_proc/resize standalone_nodelet">
     <remap from="camera_info" to="/alphasense_driver_ros/cam4/color_rect/camera_info" />
     <remap from="~camera_info" to="/alphasense_driver_ros/cam4/color_rect_resize/camera_info" />
     <remap from="image" to="/alphasense_driver_ros/cam4/color_rect/image" />
     <remap from="~image" to="/alphasense_driver_ros/cam4/color_rect_resize/image" />
-    <param name="scale_height" value="0.2074077" />
+    <param name="scale_width" value="0.2074077" />
     <param name="scale_height" value="0.2074077" />
   </node>
   <node if="$(arg resize_alphasense_cam5)" pkg="nodelet" type="nodelet" name="image_proc_nodelet_cam5" args="load image_proc/resize standalone_nodelet">
     <remap from="camera_info" to="/alphasense_driver_ros/cam5/color_rect/camera_info" />
     <remap from="~camera_info" to="/alphasense_driver_ros/cam5/color_rect_resize/camera_info" />
     <remap from="image" to="/alphasense_driver_ros/cam5/color_rect/image" />
     <remap from="~image" to="/alphasense_driver_ros/cam5/color_rect_resize/image" />
-    <param name="scale_height" value="0.2074077" />
+    <param name="scale_width" value="0.2074077" />
     <param name="scale_height" value="0.2074077" />
   </node>
 

wild_visual_navigation_ros/launch/resize_images_wide_angle_front.launch

@@ -0,0 +1,14 @@
+<launch>
+  <!-- Launch the image_proc nodelet -->
+  <node pkg="nodelet" type="nodelet" name="standalone_nodelet" args="manager"/>
+  <node pkg="nodelet" type="nodelet" name="image_proc_nodelet_wide_angle_front" args="load image_proc/resize standalone_nodelet">
+    <remap from="camera_info" to="/wide_angle_camera_front/camera_info" />
+    <remap from="~camera_info" to="/wide_angle_camera_front_resize/camera_info" />
+    <remap from="image" to="/wide_angle_camera_front/image_color" />
+    <remap from="~image" to="/wide_angle_camera_front/image_color_resize" />
+    <param name="scale_width" value="0.2074077" />
+    <param name="scale_height" value="0.2074077" />
+  </node>
+  <node name="dynparam_width_wide_angle_front" pkg="dynamic_reconfigure" type="dynparam" args="set /image_proc_nodelet_wide_angle_front scale_width 0.2074077" />
+  <node name="dynparam_height_wide_angle_front" pkg="dynamic_reconfigure" type="dynparam" args="set /image_proc_nodelet_wide_angle_front scale_height 0.2074077" />
+</launch>

wild_visual_navigation_ros/launch/robot.launch

@@ -1,7 +1,7 @@
 <launch>
   <!-- Arguments -->
   <arg name="elevation_mapping" default="False"/>
-  <arg name="camera"            default="wide_angle_front"/> <!-- alphasense, wide_angle_front, realsense_front, realsense_rear -->
+  <arg name="camera"            default="wide_angle_front_resize"/> <!-- alphasense, wide_angle_front, realsense_front, realsense_rear -->
   <arg name="stack"             default="anybotics"/> <!-- rsl or anybotics -->
 
   <!-- Use Elevation Mapping-->

wild_visual_navigation_ros/launch/wild_visual_navigation.launch

@@ -1,10 +1,11 @@
 <launch>
   <!-- Arguments -->
   <arg name="camera"            default="wide_angle_front"/> <!-- alphasense, wide_angle_front, realsense_front, realsense_rear -->
-  <arg name="stack"             default="anybotics"/> <!-- rsl or anybotics -->
+  <arg name="stack"             default="anybotics"/> <!-- rsl or anybotics  $(eval arg('camera') == 'wide_angle_front_resize')  $(eval arg('camera') == 'alphasense_resize') -->  
   <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"     default="$(eval arg('camera') == 'alphasense_resize')"/>
+  <arg name="resize_images_alphasense"     default="False"/>
+  <arg name="resize_images_wide_angle_front"     default="True"/>
   <arg name="reload_default_params"    default="False"/>
 
   <!-- Load parameters -->
@@ -22,13 +23,14 @@
     <param if="$(eval arg('stack') == 'anybotics')" name="desired_twist_topic" value="/motion_reference/command_twist"/>
     <param name="reload_default_params" value="$(arg reload_default_params)" />
   </node>
-  <node name="wvn_feature_extractor_node" pkg="wild_visual_navigation_ros" type="wvn_feature_extractor_node.py" output="screen">
+  <!--<node name="wvn_feature_extractor_node" pkg="wild_visual_navigation_ros" type="wvn_feature_extractor_node.py" output="screen">
     <param if="$(eval arg('stack') == 'rsl')" name="desired_twist_topic" value="/log/state/desiredRobotTwist"/>
     <param if="$(eval arg('stack') == 'anybotics')" name="desired_twist_topic" value="/motion_reference/command_twist"/>
     <param name="reload_default_params" value="$(arg reload_default_params)" />
-  </node>
+  </node>-->
 
-  <include if="$(arg resize_images)" file="$(find wild_visual_navigation_ros)/launch/resize_images.launch"/> 
+  <include if="$(arg resize_images_alphasense)" file="$(find wild_visual_navigation_ros)/launch/resize_images_alphasense.launch"/> 
+  <include if="$(arg resize_images_wide_angle_front)" file="$(find wild_visual_navigation_ros)/launch/resize_images_wide_angle_front.launch"/> 
 
   <!-- Nice visualization overlay-->
   <include if="$(arg overlay_images)" file="$(find wild_visual_navigation_ros)/launch/overlay_images.launch"/> 

wild_visual_navigation_ros/scripts/wvn_feature_extractor_node.py

@@ -108,11 +108,11 @@ def status_thread_loop(self):
                 else:
                     x.add_row([k, v])
             print(x)
-            try:
-                rate.sleep()
-            except Exception as e:
-                rate = rospy.Rate(self.status_thread_rate)
-                print("Ignored jump pack in time!")
+            # try:
+            #    rate.sleep()
+            # except Exception as e:
+            #    rate = rospy.Rate(self.status_thread_rate)
+            #    print("Ignored jump pack in time!")
         self.status_thread_stop_event.clear()