feature extractor node working

tests/test_configuration.py

@@ -0,0 +1,16 @@
+from wild_visual_navigation.cfg import RosLearningNodeParams
+from omegaconf import OmegaConf
+from omegaconf import read_write
+
+
+def test_configuration():
+    cfg = OmegaConf.structured(RosLearningNodeParams)
+    print(cfg)
+    with read_write(cfg):
+        cfg.image_callback_rate = 1.0
+
+    print(cfg.image_callback_rate)
+
+
+if __name__ == "__main__":
+    test_configuration()

wild_visual_navigation/cfg/__init__.py

@@ -1 +1,2 @@
 from .experiment_params import ExperimentParams
+from .ros_params import RosLearningNodeParams, RosFeatureExtractorNodeParams

wild_visual_navigation/cfg/experiment_params.py

@@ -1,10 +1,9 @@
 from dataclasses import dataclass, field
 from typing import Tuple, Dict, List, Optional
-from simple_parsing.helpers import Serializable
 
 
 @dataclass
-class ExperimentParams(Serializable):
+class ExperimentParams:
     @dataclass
     class GeneralParams:
         name: str = "debug/debug"

wild_visual_navigation/cfg/ros_params.py

@@ -0,0 +1,145 @@
+from dataclasses import dataclass
+from typing import Dict
+from typing import Any
+
+
+@dataclass
+class RosLearningNodeParams:
+    # TODO remove all unnecessary topics here
+    # Input topics
+    robot_state_topic: str
+    desired_twist_topic: str
+    # desired_twist_topic: "/log/state/desiredRobotTwist"
+
+    # Relevant frames
+    fixed_frame: str
+    base_frame: str
+    footprint_frame: str
+
+    # Robot size
+    robot_length: float
+    robot_width: float
+    robot_height: float
+
+    # Robot specs
+    robot_max_velocity: float
+
+    # Traversability estimation params
+    traversability_radius: float  # meters
+    image_graph_dist_thr: float  # meters
+    proprio_graph_dist_thr: float  # meters
+    network_input_image_height: int  # 448
+    network_input_image_width: int  # 448
+    segmentation_type: str
+    feature_type: str
+    dino_patch_size: int  # 8 or 16; 8 is finer
+    slic_num_components: int
+    dino_dim: int  # 90 or 384; 384 is better
+    confidence_std_factor: float
+    scale_traversability: bool  # This parameter needs to be false when using the anomaly detection model
+    scale_traversability_max_fpr: float
+    min_samples_for_training: int
+    prediction_per_pixel: bool
+    traversability_threshold: float
+    clip_to_binary: bool
+
+    # Supervision Generator
+    untraversable_thr: float
+
+    mission_name: str
+    mission_timestamp: bool
+
+    # Threads
+    image_callback_rate: float  # hertz
+    proprio_callback_rate: float  # hertz
+    learning_thread_rate: float  # hertz
+    logging_thread_rate: float  # hertz
+    status_thread_rate: float  # hertz
+
+    # Runtime options
+    device: str
+    mode: str  # check out comments in the class WVNMode
+    colormap: str
+
+    print_image_callback_time: bool
+    print_proprio_callback_time: bool
+    log_time: bool
+    log_confidence: bool
+    verbose: bool
+    debug_supervision_node_index_from_last: int
+    use_debug_for_desired: bool
+
+    extraction_store_folder: str
+    exp: str
+    use_binary_only: bool
+
+
+@dataclass
+class RosFeatureExtractorNodeParams:
+    # Input topics
+    robot_state_topic: str
+    desired_twist_topic: str
+    # desired_twist_topic: "/log/state/desiredRobotTwist"
+
+    # Relevant frames
+    fixed_frame: str
+    base_frame: str
+    footprint_frame: str
+
+    # Robot size
+    robot_length: float
+    robot_width: float
+    robot_height: float
+
+    # Robot specs
+    robot_max_velocity: float
+
+    # Traversability estimation params
+    traversability_radius: float  # meters
+    image_graph_dist_thr: float  # meters
+    proprio_graph_dist_thr: float  # meters
+    network_input_image_height: int  # 448
+    network_input_image_width: int  # 448
+    segmentation_type: str
+    feature_type: str
+    dino_patch_size: int  # 8 or 16; 8 is finer
+    slic_num_components: int
+    dino_dim: int  # 90 or 384; 384 is better
+    confidence_std_factor: float
+    scale_traversability: bool  # This parameter needs to be false when using the anomaly detection model
+    scale_traversability_max_fpr: float
+    min_samples_for_training: int
+    prediction_per_pixel: bool
+    traversability_threshold: float
+    clip_to_binary: bool
+
+    # Supervision Generator
+    untraversable_thr: float
+
+    mission_name: str
+    mission_timestamp: bool
+
+    # Threads
+    image_callback_rate: float  # hertz
+    proprio_callback_rate: float  # hertz
+    learning_thread_rate: float  # hertz
+    logging_thread_rate: float  # hertz
+    status_thread_rate: float  # hertz
+
+    # Runtime options
+    device: str
+    mode: str  # check out comments in the class WVNMode
+    colormap: str
+
+    print_image_callback_time: bool
+    print_proprio_callback_time: bool
+    log_time: bool
+    log_confidence: bool
+    verbose: bool
+    debug_supervision_node_index_from_last: int
+    use_debug_for_desired: bool
+
+    extraction_store_folder: str
+    exp: str
+    use_binary_only: bool
+    camera_topics: Dict[str, Any]

wild_visual_navigation/utils/__init__.py

@@ -3,13 +3,13 @@
 from .loading import load_env, load_yaml, file_path
 from .create_experiment_folder import create_experiment_folder
 from .get_confidence import get_confidence
-from .loss import TraversabilityLoss, AnomalyLoss
-from .metric_logger import MetricLogger
 from .kalman_filter import KalmanFilter
+from .confidence_generator import ConfidenceGenerator
+from .metric_logger import MetricLogger
 from .meshes import make_box, make_rounded_box, make_ellipsoid, make_plane, make_polygon_from_points, make_dense_plane
 from .klt_tracker import KLTTracker, KLTTrackerOpenCV
-from .confidence_generator import ConfidenceGenerator
 from .operation_modes import WVNMode
 from .dataset_info import perugia_dataset, ROOT_DIR
 from .override_params import override_params
 from .gpu_monitor import GpuMonitor, SystemLevelGpuMonitor, SystemLevelContextGpuMonitor, accumulate_memory
+from .loss import TraversabilityLoss, AnomalyLoss

wild_visual_navigation/utils/loss.py

@@ -1,8 +1,9 @@
+from wild_visual_navigation.utils import ConfidenceGenerator
+
 import torch.nn.functional as F
 from torch_geometric.data import Data
 import torch
 from typing import Optional
-from wild_visual_navigation.utils import ConfidenceGenerator
 from torch import nn
 
 

wild_visual_navigation_ros/CMakeLists.txt

@@ -14,12 +14,9 @@ catkin_package(
 )
 
 catkin_python_setup()
-catkin_install_python(PROGRAMS  scripts/wild_visual_navigation_node.py
-                                scripts/wvn_feature_extractor_node.py
+catkin_install_python(PROGRAMS  scripts/wvn_feature_extractor_node.py
                                 scripts/wvn_learning_node.py
                                 scripts/overlay_images.py
-                                scripts/shift_gridmap.py
                                 scripts/smart_carrot.py
                                 scripts/rosbag_play.sh
-                                scripts/rotate_image.py
                       DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})

wild_visual_navigation_ros/config/wild_visual_navigation/default.yaml

@@ -1,3 +1,5 @@
+# TODO split into three configuration files - shared - learning - feature_extractor
+
 # Input topics
 robot_state_topic: "/wild_visual_navigation_node/robot_state"
 desired_twist_topic: "/motion_reference/command_twist"

wild_visual_navigation_ros/scripts/rosbag_play.sh

@@ -6,35 +6,33 @@ for option in "$@"; do
     args="$args /elevation_mapping/elevation_map_raw:=/recorded/elevation_mapping/elevation_map_raw \
     /elevation_mapping/semantic_map_raw:=/recorded/elevation_mapping/semantic_map_raw"
   elif [ "$option" == "--wvn" ]; then
-    args="$args /wild_visual_navigation_node/front/camera_info:=/recorded_wvn/wild_visual_navigation_node/front/camera_info \
-    /wild_visual_navigation_node/front/confidence:=/recorded_wvn/wild_visual_navigation_node/front/confidence \
-    /wild_visual_navigation_node/front/image_input:=/recorded_wvn/wild_visual_navigation_node/front/image_input \
-    /wild_visual_navigation_node/front/traversability:=/recorded_wvn/wild_visual_navigation_node/front/traversability \
-    /wild_visual_navigation_node/graph_footprints:=/recorded_wvn/wild_visual_navigation_node/graph_footprints \
-    /wild_visual_navigation_node/instant_traversability:=/recorded_wvn/wild_visual_navigation_node/instant_traversability \
-    /wild_visual_navigation_node/proprioceptive_graph:=/recorded_wvn/wild_visual_navigation_node/proprioceptive_graph \
-    /wild_visual_navigation_node/robot_state:=/recorded_wvn/wild_visual_navigation_node/robot_state \
-    /wild_visual_navigation_node/system_state:=/recorded_wvn/wild_visual_navigation_node/system_state \
-    /wild_visual_navigation_visu_confidence/confidence_overlayed:=/recorded_wvn/wild_visual_navigation_visu_confidence/confidence_overlayed \
-    /wild_visual_navigation_visu_traversability/traversability_overlayed:=/recorded_wvn/wild_visual_navigation_visu_traversability/traversability_overlayed"
+    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 \
+    /wild_visual_navigation_node/front/image_input:=/recorded/wild_visual_navigation_node/front/image_input \
+    /wild_visual_navigation_node/front/traversability:=/recorded/wild_visual_navigation_node/front/traversability \
+    /wild_visual_navigation_node/graph_footprints:=/recorded/wild_visual_navigation_node/graph_footprints \
+    /wild_visual_navigation_node/instant_traversability:=/recorded/wild_visual_navigation_node/instant_traversability \
+    /wild_visual_navigation_node/proprioceptive_graph:=/recorded/wild_visual_navigation_node/proprioceptive_graph \
+    /wild_visual_navigation_node/robot_state:=/recorded/wild_visual_navigation_node/robot_state \
+    /wild_visual_navigation_node/system_state:=/recorded/wild_visual_navigation_node/system_state \
+    /wild_visual_navigation_visu_confidence/confidence_overlayed:=/recorded/wild_visual_navigation_visu_confidence/confidence_overlayed \
+    /wild_visual_navigation_visu_traversability/traversability_overlayed:=/recorded/wild_visual_navigation_visu_traversability/traversability_overlayed"
   elif [ "$option" == "--flp" ]; then
-    args="$args /field_local_planner/action_server/status:=/recorded_flp/field_local_planner/action_server/status \
-    /field_local_planner/current_base:=/recorded_flp/field_local_planner/current_base \
-    /field_local_planner/current_goal:=/recorded_flp/field_local_planner/current_goal \
-    /field_local_planner/parameter_descriptions:=/recorded_flp/field_local_planner/parameter_descriptions \
-    /field_local_planner/parameter_updates:=/recorded_flp/field_local_planner/parameter_updates \
-    /field_local_planner/path:=/recorded_flp/field_local_planner/path \
-    /field_local_planner/real_carrot:=/recorded_flp/field_local_planner/real_carrot \
-    /field_local_planner/rmp/control_points:=/recorded_flp/field_local_planner/rmp/control_points \
-    /field_local_planner/rmp/parameter_descriptions:=/recorded_flp/field_local_planner/rmp/parameter_descriptions \
-    /field_local_planner/rmp/parameter_updates:=/recorded_flp/field_local_planner/rmp/parameter_updates \
-    /field_local_planner/status:=/recorded_flp/field_local_planner/status \
-    /elevation_mapping/elevation_map_wifi:=/recorded_flp/elevation_mapping/elevation_map_wifi"
+    args="$args /field_local_planner/action_server/status:=/recorded/field_local_planner/action_server/status \
+    /field_local_planner/current_base:=/recorded/field_local_planner/current_base \
+    /field_local_planner/current_goal:=/recorded/field_local_planner/current_goal \
+    /field_local_planner/parameter_descriptions:=/recorded/field_local_planner/parameter_descriptions \
+    /field_local_planner/parameter_updates:=/recorded/field_local_planner/parameter_updates \
+    /field_local_planner/path:=/recorded/field_local_planner/path \
+    /field_local_planner/real_carrot:=/recorded/field_local_planner/real_carrot \
+    /field_local_planner/rmp/control_points:=/recorded/field_local_planner/rmp/control_points \
+    /field_local_planner/rmp/parameter_descriptions:=/recorded/field_local_planner/rmp/parameter_descriptions \
+    /field_local_planner/rmp/parameter_updates:=/recorded/field_local_planner/rmp/parameter_updates \
+    /field_local_planner/status:=/recorded/field_local_planner/status \
+    /elevation_mapping/elevation_map_wifi:=/recorded/elevation_mapping/elevation_map_wifi"
   elif [ "$option" == "--tf" ]; then
-    args="$args /tf:=/recorded_flp/tf" 
-
-    # /tf_static:=/recorded_flp/tf_static"
-
+    args="$args /tf:=/recorded/tf" 
+    # /tf_static:=/recorded/tf_static"
     echo "rosrun anymal_rsl_launch replay.py c /media/Data/Datasets/2023_Oxford_Testing/2023_01_27_Oxford_Park/mission_data/2023-01-27-11-00-22/2023-01-27-11-00-22_anymal-coyote-lpc_mission.yaml"
   else
     args="$args $option"