From e015a7dc08a9cc16cc4c2b128244ef3db2aa40ca Mon Sep 17 00:00:00 2001
From: RobinSchmid7 <schmirob@student.ethz.ch>
Date: Wed, 6 Sep 2023 16:04:02 +0200
Subject: [PATCH] Add missing dep, fix default overlay param, add procman
 replay, add resize image

---
 environment.yaml                              |  3 +++
 .../config/procman/replay.pmd                 | 22 +++++++++++++++++++
 .../launch/replay_launch.launch               |  9 +++++---
 .../scripts/overlay_images.py                 |  7 ++++--
 4 files changed, 36 insertions(+), 5 deletions(-)
 create mode 100644 wild_visual_navigation_ros/config/procman/replay.pmd

diff --git a/environment.yaml b/environment.yaml
index 65cb20f0..fbccc8d1 100644
--- a/environment.yaml
+++ b/environment.yaml
@@ -47,3 +47,6 @@ dependencies:
     - git+https://github.com/kornia/kornia#egg=kornia
     - git+https://github.com/mmattamala/liegroups#egg=liegroups
     - --editable git+https://github.com/leggedrobotics/stego.git#egg=stego==0.0.1
+    - pytictac
+    - prettytable
+    - termcolor
diff --git a/wild_visual_navigation_ros/config/procman/replay.pmd b/wild_visual_navigation_ros/config/procman/replay.pmd
new file mode 100644
index 00000000..c231065b
--- /dev/null
+++ b/wild_visual_navigation_ros/config/procman/replay.pmd
@@ -0,0 +1,22 @@
+group "01.startup" {
+    cmd "1.1.roscore" {
+        exec = "roscore";
+        host = "localhost";
+    }
+    cmd "1.2.rviz replay" {
+        exec = "roslaunch wild_visual_navigation_ros replay_launch.launch";
+        host = "localhost";
+    }
+    cmd "1.3.feat extractor" {
+        exec = "/home/rschmid/anaconda3/envs/wvn/bin/python3 /home/rschmid/git/wild_visual_navigation/wild_visual_navigation_ros/scripts/wvn_feature_extractor_node.py";
+        host = "localhost";
+    }
+    cmd "1.4.learning node" {
+        exec = "/home/rschmid/anaconda3/envs/wvn/bin/python3 /home/rschmid/git/wild_visual_navigation/wild_visual_navigation_ros/scripts/wvn_learning_node.py";
+        host = "localhost";
+    }
+    cmd "1.5.image overlay" {
+        exec = "/home/rschmid/anaconda3/envs/wvn/bin/python3 /home/rschmid/git/wild_visual_navigation/wild_visual_navigation_ros/scripts/overlay_images.py _image_sub_topic:=/wild_visual_navigation_node/front/image_input _value_sub_topic:=/wild_visual_navigation_node/front/traversability _image_pub_topic:=traversability_overlayed --nr 0";
+        host = "localhost";
+    }
+}
diff --git a/wild_visual_navigation_ros/launch/replay_launch.launch b/wild_visual_navigation_ros/launch/replay_launch.launch
index e922eec3..d29a062a 100644
--- a/wild_visual_navigation_ros/launch/replay_launch.launch
+++ b/wild_visual_navigation_ros/launch/replay_launch.launch
@@ -14,7 +14,9 @@
   <arg name="uncompress_alphasense_cam3"     default="False"/>
   <arg name="uncompress_alphasense_cam4"     default="False"/>
   <arg name="uncompress_alphasense_cam5"     default="False"/>
-  <arg name="uncompress_wide_angle_cam"     default="True"/>
+  <arg name="uncompress_wide_angle_front"     default="True"/>
+  <arg name="resize_images_wide_angle_front"     default="True"/>
+
 
   <!-- Set sim time to true -->
   <param name="/use_sim_time" type="bool" value="True" />
@@ -74,7 +76,8 @@
   <node if="$(arg uncompress_alphasense_cam4)" name="uncompress_cam4" pkg="image_transport" type="republish" args="compressed in:=/alphasense_driver_ros/cam4/debayered raw out:=/alphasense_driver_ros/cam4/debayered" />
   <node if="$(arg uncompress_alphasense_cam5)" name="uncompress_cam5" pkg="image_transport" type="republish" args="compressed in:=/alphasense_driver_ros/cam5/debayered raw out:=/alphasense_driver_ros/cam5/debayered" />
 
-  <node if="$(arg uncompress_wide_angle_cam)" name="uncompress_wide_angle_cam" pkg="image_transport" type="republish" args="compressed in:=/wide_angle_camera_front/image_color raw out:=/wide_angle_camera_front/image_color" />
+  <node if="$(arg uncompress_wide_angle_front)" name="uncompress_wide_angle_front" pkg="image_transport" type="republish" args="compressed in:=/wide_angle_camera_front/image_color raw out:=/wide_angle_camera_front/image_color" />
+  <include if="$(arg resize_images_wide_angle_front)" file="$(find wild_visual_navigation_ros)/launch/resize_images_wide_angle_front.launch"/>
+
 
-  
 </launch>
\ No newline at end of file
diff --git a/wild_visual_navigation_ros/scripts/overlay_images.py b/wild_visual_navigation_ros/scripts/overlay_images.py
index 943cfc36..fae09177 100644
--- a/wild_visual_navigation_ros/scripts/overlay_images.py
+++ b/wild_visual_navigation_ros/scripts/overlay_images.py
@@ -29,8 +29,11 @@ def callback(self, image_msg, trav_msgs):
 if __name__ == "__main__":
     print("Stated ImageOverlayNode")
     try:
-        nr = "_" + rospy.myargv(argv=sys.argv)[-1].split(" ")[-1]
-        rospy.init_node(f"wild_visual_navigation_visu{nr}")
+        nr = "0"
+        if len(sys.argv) > 1:
+            nr = rospy.myargv(argv=sys.argv)[-1].split(" ")[-1]
+        nr_str = "_" + nr
+        rospy.init_node(f"wild_visual_navigation_visu{nr_str}")
     except:
         rospy.init_node("wild_visual_navigation_visu")