cleaned up repo

leggedrobotics · Feb 7, 2024 · 8b7c6eb · 8b7c6eb
1 parent b9b7816
commit 8b7c6eb
Show file tree

Hide file tree

Showing 63 changed files with 191 additions and 9,988 deletions.
diff --git a/scripts/train_optuna.py b/scripts/train_optuna.py
@@ -2,7 +2,7 @@
 from argparse import ArgumentParser
 import copy
 import os
-import neptune.new.integrations.optuna as optuna_utils
+import neptune.integrations.optuna as optuna_utils
 import torch
 
 from wild_visual_navigation.cfg import ExperimentParams

diff --git a/wild_visual_navigation/utils/get_logger.py b/wild_visual_navigation/utils/get_logger.py
@@ -3,7 +3,7 @@
 from wild_visual_navigation.utils import flatten_dict
 import inspect
 import os
-import neptune.new as neptune
+import neptune
 
 __all__ = [
     "get_neptune_logger",

diff --git a/wild_visual_navigation/visu/image_functionality.py b/wild_visual_navigation/visu/image_functionality.py
@@ -54,7 +54,7 @@ def wrap(*args, **kwargs):
                 ds = cv2.resize(img, dsize=(int(W / 2), int(H / 2)), interpolation=cv2.INTER_CUBIC)
                 try:
                     # logger == neptuneai
-                    from neptune.new.types import File
+                    from neptune.types import File
 
                     args[0]._pl_model.logger.experiment[tag].log(File.as_image(np.float32(ds) / 255), step=epoch)
                 except Exception:

diff --git a/wild_visual_navigation_anymal/CMakeLists.txt b/wild_visual_navigation_anymal/CMakeLists.txt
@@ -29,13 +29,15 @@ include_directories(
     ${catkin_INCLUDE_DIRS}
 )
 
-# Declare node
-add_executable(anymal_msg_converter_cpp_node
+if(BUILD_ANYMAL)
+   # Declare node
+   add_executable(anymal_msg_converter_cpp_node
                src/anymal_msg_converter_cpp_node.cpp)
 
-target_link_libraries(anymal_msg_converter_cpp_node
+   target_link_libraries(anymal_msg_converter_cpp_node
                       ${catkin_LIBRARIES}
-)
+   )
+endif()
 
 #############
 ## Install ##

diff --git a/wild_visual_navigation_anymal/README.md b/wild_visual_navigation_anymal/README.md
@@ -5,6 +5,8 @@ NPC:
 ```
 sudo apt-get install ros-noetic-anymal-msgs-dev
 catkin build wild_visual_navigation_anymal
+
+catkin build wild_visual_navigation_anymal --cmake-args -DBUILD_ANYMAL=1
 ```
 
 Jetson:

diff --git a/wild_visual_navigation_anymal/config/elevation_mapping_cupy/anymal_parameters.yaml b/wild_visual_navigation_anymal/config/elevation_mapping_cupy/anymal_parameters.yaml
@@ -69,7 +69,7 @@ initialize_tf_grid_size: 0.4                                        # This is no
 use_initializer_at_start: true                                      # Use initializer when the node starts.
 
 #### Plugins ########
-plugin_config_file: '$(rospack find wild_visual_navigation_ros)/config/elevation_mapping_cupy/anymal_plugin_config.yaml'
+plugin_config_file: '$(rospack find wild_visual_navigation_anymal)/config/elevation_mapping_cupy/anymal_plugin_config.yaml'
 
 pointcloud_channel_fusions:
   rgb: 'color'
@@ -94,8 +94,13 @@ publishers:
     basic_layers: ['elevation', 'traversability']
     fps: 5.0
 
-  semantic_map_raw:
-    layers: ['elevation', 'traversability', 'visual_traversability', 'visual_confidence', 'elevation_with_semantics']
+  semantic_map:
+    layers: ['elevation', 'traversability', 'visual_traversability', 'visual_confidence', 'elevation_with_semantics', 'rgb']
+    basic_layers: ['elevation', 'traversability']
+    fps: 5.0
+
+  local_planning_map:
+    layers: ['elevation', 'traversability', 'visual_traversability']
     basic_layers: ['elevation', 'traversability']
     fps: 5.0
 

diff --git a/wild_visual_navigation_anymal/config/elevation_mapping_cupy/anymal_sensor_parameter.yaml b/wild_visual_navigation_anymal/config/elevation_mapping_cupy/anymal_sensor_parameter.yaml
@@ -30,16 +30,15 @@ subscribers:
 
   # Semantics
   front_wide_angle:
-    topic_name: /wide_angle_camera_front/image_color_rect/compressed
+    topic_name: /wide_angle_camera_front/image_color_rect
     camera_info_topic_name: /wide_angle_camera_front/camera_info
     data_type: image
 
   rear_wide_angle:
-    topic_name: /wide_angle_camera_rear/image_color_rect/compressed
+    topic_name: /wide_angle_camera_rear/image_color_rect
     camera_info_topic_name: /wide_angle_camera_rear/camera_info
     data_type: image
 
-
   # Traversability
   # channels: ["visual_traversability"]
   sem_wvn_front_traversability:
@@ -48,15 +47,9 @@ subscribers:
     channels: ['visual_traversability']
     data_type: image
 
-  sem_wvn_left_traversability:
-    topic_name: '/wild_visual_navigation_node/left/traversability'
-    camera_info_topic_name: '/wild_visual_navigation_node/left/camera_info'
-    channels: ['visual_traversability']
-    data_type: image
-
-  sem_wvn_right_traversability:
-    topic_name: '/wild_visual_navigation_node/right/traversability'
-    camera_info_topic_name: '/wild_visual_navigation_node/right/camera_info'
+  sem_wvn_rear_traversability:
+    topic_name: '/wild_visual_navigation_node/rear/traversability'
+    camera_info_topic_name: '/wild_visual_navigation_node/rear/camera_info'
     channels: ['visual_traversability']
     data_type: image
 

diff --git a/wild_visual_navigation_anymal/config/procman/replay.pmd b/wild_visual_navigation_anymal/config/procman/replay.pmd
@@ -7,80 +7,92 @@ group "0 - Setup" {
         exec = "rosrun wild_visual_navigation_anymal anymal_msg_converter_node.py";
         host = "localhost";
     }
-    cmd "0.2 - resize_images_wide_angle_front" {
-        exec = "roslaunch wild_visual_navigation_ros resize_images_wide_angle_front.launch";
+    cmd "0.2 - rviz anymal" {
+        exec = "roslaunch wild_visual_navigation_anymal view.launch";
         host = "localhost";
     }
-    cmd "0.3 - rviz" {
-        exec = "roslaunch wild_visual_navigation_ros view.launch";
+    cmd "0.3 - resize_images_wide_angle_front" {
+        exec = "roslaunch wild_visual_navigation_anymal resize_images_wide_angle_front.launch";
         host = "localhost";
     }
-    cmd "0.4 - overlay" {
-        exec = "roslaunch wild_visual_navigation_ros overlay_images.launch";
-        host = "localhost";
-    }
-    cmd "0.5 - elevation_mapping_cupy" {
-        exec = "roslaunch wild_visual_navigation_ros elevation_mapping_cupy.launch";
+    cmd "0.4 - resize_images_wide_angle_rear" {
+        exec = "roslaunch wild_visual_navigation_anymal resize_images_wide_angle_rear.launch";
         host = "localhost";
     }
-    cmd "0.6 - rqt learning graph" {
+    cmd "0.5 - rqt learning graph" {
         exec = "rosrun rqt_multiplot rqt_multiplot";
         host = "localhost";
     }
-    cmd "0.7 - rosbag play" {
-        exec = "rosbag_play --sem --tf --flp --wvn --sem  /Data/2024_02_06_Dodo_MPI_WVN/dodo_mission_2024_02_06/2024-02-06-14-39-01_Small_MPI_Indoor/*.bag ";
+    cmd "0.6 - rosbag play" {
+        exec = "rosbag_play --sem --tf --flp --wvn --sem  /Data/2024_02_06_Dodo_MPI_WVN/dodo_mission_2024_02_06/2024-02-06-14-39-01_Small_MPI_Indoor/*.bag -r 0.5 -l ";
         host = "localhost";
     }
 }
 
 group "1 - WVN" {
-    cmd "1.1 - wild_visual_navigation" {
-        exec = "roslaunch wild_visual_navigation_ros robot.launch";
+    cmd "1.1 - wild_visual_navigation - resize - elevation mapping - overlay" {
+        exec = "roslaunch wild_visual_navigation_anymal robot.launch camera:=wide_angle_dual_resize";
+        host = "localhost";
+    }
+
+    cmd "1.2 - overlay" {
+        exec = "roslaunch wild_visual_navigation_ros overlay_images.launch";
         host = "localhost";
     }
-    cmd "1.2 - wild_visual_navigation_learning" {
+    cmd "1.3 - elevation_mapping_cupy" {
+        exec = "roslaunch wild_visual_navigation_anymal elevation_mapping_cupy.launch";
+        host = "localhost";
+    }
+    cmd "1.4 - wild_visual_navigation_learning" {
         exec = "python3 /home/jonfrey/workspaces/catkin_ws/src/wild_visual_navigation/wild_visual_navigation_ros/scripts/wvn_learning_node.py";
         host = "localhost";
     }
-    cmd "1.3 - wild_visual_navigation_feature_extractor" {
+    cmd "1.5 - wild_visual_navigation_feature_extractor" {
         exec = "python3 /home/jonfrey/workspaces/catkin_ws/src/wild_visual_navigation/wild_visual_navigation_ros/scripts/wvn_feature_extractor_node.py";
         host = "localhost";
     }
-    cmd "1.4 - kill_wvn" {
-        exec = "rosnode kill /wild_visual_navigation_node";
+    cmd "1.6 - kill_wvn" {
+        exec = "rosnode kill /wvn_learning_node /wvn_feature_extractor_node";
         host = "localhost";
     }
 }
 
-
-group "2 - Visu" {
-    cmd "2.1 - rviz" {
-        exec = "roslaunch wild_visual_navigation_ros view.launch";
+group "3 - Config" {
+    cmd "3.1 - dynamic_reconfigure" {
+        exec = "rosrun rqt_reconfigure rqt_reconfigure";
         host = "localhost";
     }
 }
 
-group "3 - Config" {
-    cmd "3.1 - dynamic_reconfigure" {
-        exec = "rosrun rqt_reconfigure rqt_reconfigure";
+group "4 - Uncompress" {
+    cmd "4.1 - front" {
+        exec = "rosrun image_transport republish compressed in:=/wide_angle_camera_front/image_color_rect  raw out:=/wide_angle_camera_front/image_color_rect";
+        host = "localhost";
+    }
+    cmd "4.2 - rear" {
+        exec = "rosrun image_transport republish compressed in:=/wide_angle_camera_rear/image_color_rect  raw out:=/wide_angle_camera_rear/image_color_rect";
+        host = "localhost";
+    }
+    cmd "4.3 - hdr" {
+        exec = "rosrun image_transport republish compressed in:=/hdr_camera/image_raw  raw out:=/hdr_camera/image_raw";
         host = "localhost";
     }
 }
 
-group "4 - Interface" {
-    cmd "4.1 - pause_training" {
+group "5 - Interface" {
+    cmd "5.1 - pause_training" {
         exec = "rosservice call /wild_visual_navigation_node/pause_learning 'data: true'";
         host = "localhost";
     }
-    cmd "4.2 - resume_training" {
+    cmd "5.2 - resume_training" {
         exec = "rosservice call /wild_visual_navigation_node/pause_learning 'data: false'";
         host = "localhost";
     }
-    cmd "4.3 - save_checkpoint" {
+    cmd "5.3 - save_checkpoint" {
         exec = "rosservice call /wild_visual_navigation_node/save_checkpoint ''";
         host = "localhost";
     }
-    cmd "4.4 - load_checkpoint" {
+    cmd "5.4 - load_checkpoint" {
         exec = "rosservice call /wild_visual_navigation_node/load_checkpoint 'path: 'absolute_path_in_robot_filesystem'";
         host = "localhost";
     }
-Original file line number
+Diff line change
@@ Expand Up / @@ -5,6 +5,8 @@ NPC: @@
     ```
     sudo apt-get install ros-noetic-anymal-msgs-dev
     catkin build wild_visual_navigation_anymal
+    catkin build wild_visual_navigation_anymal --cmake-args -DBUILD_ANYMAL=1
     ```
     Jetson:
@@ Expand Down @@