diff --git a/wild_visual_navigation_ros/config/procman/robot_dodo.pmd b/wild_visual_navigation_ros/config/procman/robot_dodo.pmd
new file mode 100644
index 00000000..c2f87fc8
--- /dev/null
+++ b/wild_visual_navigation_ros/config/procman/robot_dodo.pmd
@@ -0,0 +1,235 @@
+group "1.startup" {
+ cmd "1.1.chrony" {
+ exec = "rosrun anymal_rsl_chrony reset_chrony.sh jonfrey dodo d020 false";
+ host = "localhost";
+ }
+ cmd "1.2.depth" {
+ exec = "rosservice call /depth_cameras/enable 'data: true' ";
+ host = "localhost";
+ }
+ cmd "1.3.lidar" {
+ exec = "rosservice call /lidar/enable 'data: true' ";
+ host = "localhost";
+ }
+ cmd "1.4.wide angle" {
+ exec = "rosservice call /wide_angle_cameras/enable 'data: true' ";
+ host = "localhost";
+ }
+ cmd "1.5.power jetson" {
+ exec = "/home/jonfrey/powerline.sh";
+ host = "lpc";
+ }
+ cmd "1.6.ping jetson" {
+ exec = "ping anymal-dodo-jetson";
+ host = "localhost";
+ }
+}
+
+group "2.start" {
+ cmd "2.1.lpc" {
+ exec = "rosrun anymal_dodo_rsl lpc.py";
+ host = "lpc";
+ }
+ cmd "2.2.npc" {
+ exec = "rosrun anymal_d_rsl npc.py";
+ host = "npc";
+ }
+ cmd "2.3.jetson" {
+ exec = "rosrun anymal_d_rsl jetson.py";
+ host = "jetson";
+ }
+ cmd "2.4.opc" {
+ exec = "rosrun anymal_d_rsl opc.py";
+ host = "localhost";
+ }
+}
+
+group "3.recording" {
+ cmd "3.1.rosbag_record" {
+ exec = "rosservice call /rosbag_record_robot_coordinator/record_bag 'yaml_file: /home/jonfrey/git/diffusion_navigation/diffusion_navigation_ros/config/recording/dodo.yaml' ";
+ host = "localhost";
+ }
+ cmd "3.2.rosbag_record_state" {
+ exec = "rosnode list | grep record";
+ host = "localhost";
+ }
+ cmd "3.3.rosbag_stop" {
+ exec = "rosservice call /rosbag_record_robot_coordinator/stop_bag 'verbose: false' ";
+ host = "localhost";
+ }
+ cmd "3.4.fetch_bags" {
+ exec = "/home/jonfrey/git/anymal_rsl/anymal_rsl/anymal_rsl_utils/anymal_rsl_recording/anymal_rsl_recording/bin/copy_mission_data_from_robot.sh jonfrey dodo /home/jonfrey/mission_data";
+ host = "localhost";
+ }
+ cmd "3.5.delete_bags" {
+ exec = "/home/jonfrey/git/anymal_rsl/anymal_rsl/anymal_rsl_utils/anymal_rsl_recording/anymal_rsl_recording/bin/remove_mission_data_from_robot.sh jonfrey dodo";
+ host = "localhost";
+ }
+ cmd "3.6.reset_recording" {
+ exec = "rosnode kill rosbag_record_coordinator rosbag_record_robot_jetson rosbag_record_robot_npc rosbag_record_robot_lpc";
+ host = "localhost";
+ }
+}
+
+group "4.monitoring" {
+ cmd "4.1.lpc_disk" {
+ exec = "rostopic echo /disk_monitor_lpc/status/disks[1]";
+ host = "localhost";
+ }
+ cmd "4.2.npc_disk" {
+ exec = "rostopic echo /disk_monitor_npc/status/disks[1]";
+ host = "localhost";
+ }
+ cmd "4.3.jetson_disk" {
+ exec = "rostopic echo /disk_monitor_jetson/status/disks[1]";
+ host = "localhost";
+ }
+}
+
+group "5.network" {
+ cmd "5.1.switch_to_access_point" {
+ exec = "rosrun anymal_router_utils router_utils.bash -i 192.168.0.173 -p d020 -m 1";
+ host = "localhost";
+ }
+ cmd "5.2.switch_to_client_mode" {
+ exec = "rosrun anymal_router_utils router_utils.bash -i 192.168.0.173 -p d020 -m 3";
+ host = "localhost";
+ }
+}
+
+
+group "6.disk_free" {
+ cmd "6.1.lpc" {
+ exec = "df -h /dev/sda4";
+ host = "lpc";
+ }
+ cmd "6.2.npc" {
+ exec = " df -h /dev/sda4";
+ host = "npc";
+ }
+ cmd "6.3.jetson" {
+ exec = "df -h /dev/nvme0n1p1";
+ host = "jetson";
+ }
+ cmd "6.3.opc" {
+ exec = "df -h /dev/nvme0n1p5";
+ host = "localhost";
+ }
+
+}
+
+group "7.tools for bags" {
+ cmd "7.1.collect_lpc" {
+ exec = " rsync -rzPav anymal-dodo-lpc:/home/jonfrey/catkin_ws/src/anymal_rsl/anymal_rsl/anymal_rsl_utils/anymal_rsl_recording/anymal_rsl_recording/data /home/jonfrey/mission_data/lpc";
+ host = "jetson";
+ }
+ cmd "7.2.collect_npc" {
+ exec = " rsync -rzPav anymal-dodo-npc:/home/jonfrey/catkin_ws/src/anymal_rsl/anymal_rsl/anymal_rsl_utils/anymal_rsl_recording/anymal_rsl_recording/data /home/jonfrey/mission_data/npc";
+ host = "jetson";
+ }
+ cmd "7.3.delete lpc" {
+ exec = "rm -r -f /home/jonfrey/catkin_ws/src/anymal_rsl/anymal_rsl/anymal_rsl_utils/anymal_rsl_recording/anymal_rsl_recording/data/*.bag";
+ host = "lpc";
+ }
+ cmd "7.4.delete npc" {
+ exec = "rm -r -f /home/jonfrey/catkin_ws/src/anymal_rsl/anymal_rsl/anymal_rsl_utils/anymal_rsl_recording/anymal_rsl_recording/data/*.bag";
+ host = "npc";
+ }
+}
+
+
+group "8.npc" {
+ cmd "8.1.alphasense_ptp" {
+ exec = "sudo service phc2sys restart";
+ host = "npc";
+ }
+ cmd "8.2.anymal_msg_converter" {
+ exec = "rosrun wild_visual_navigation_anymal anymal_msg_converter_node.py";
+ host = "npc";
+ }
+ cmd "8.3.local_planner_visual" {
+ exec = "roslaunch wild_visual_navigation_ros field_local_planner.launch traversability:=visual base_inverted:=false";
+ host = "npc";
+ }
+ cmd "8.4.local_planner_visual_inverted" {
+ exec = "roslaunch wild_visual_navigation_ros field_local_planner.launch traversability:=visual base_inverted:=true";
+ host = "npc";
+ }
+ cmd "8.5.local_planner_geometric" {
+ exec = "roslaunch wild_visual_navigation_ros field_local_planner.launch traversability:=geometric base_inverted:=false";
+ host = "npc";
+ }
+ cmd "8.6.local_planner_geometric_inverted" {
+ exec = "roslaunch wild_visual_navigation_ros field_local_planner.launch traversability:=geometric base_inverted:=true";
+ host = "npc";
+ }
+ cmd "8.7.local_planner_debug_on_robot" {
+ exec = "roslaunch wild_visual_navigation_ros field_local_planner.launch traversability:=geometric base_inverted:=true output_twist_topic:=/dummy_twist";
+ host = "npc";
+ }
+ cmd "8.8.smart_carrot" {
+ exec = "rosrun wild_visual_navigation_ros smart_carrot.py";
+ host = "npc";
+ }
+}
+
+
+group "9.orin" {
+ cmd "9.1.wild_visual_navigation" {
+ exec = "roslaunch wild_visual_navigation_ros robot.launch";
+ host = "jetson";
+ }
+
+ cmd "9.2.kill_wvn" {
+ exec = "rosnode kill /wild_visual_navigation_node";
+ host = "localhost";
+ }
+}
+
+group "10.visualization" {
+ cmd "10.1.rviz" {
+ exec = "roslaunch wild_visual_navigation_ros view.launch";
+ host = "localhost";
+ }
+}
+
+group "11.configuration" {
+ cmd "11.1.dynamic_reconfigure" {
+ exec = "rosrun rqt_reconfigure rqt_reconfigure";
+ host = "localhost";
+ }
+}
+
+group "12.white_balance" {
+ cmd "12.1.white_balance_front_reset" {
+ exec = "rosservice call /alphasense_raw_image_pipeline_front/reset_white_balance";
+ host = "localhost";
+ }
+ cmd "12.2.white_balance_left_reset" {
+ exec = "rosservice call /alphasense_raw_image_pipeline_left/reset_white_balance";
+ host = "localhost";
+ }
+ cmd "12.3.white_balance_right_reset" {
+ exec = "rosservice call /alphasense_raw_image_pipeline_right/reset_white_balance";
+ host = "localhost";
+ }
+}
+
+group "x.learning_utils" {
+ cmd "x.1.pause_training" {
+ exec = "rosservice call /wild_visual_navigation_node/pause_learning 'data: true'";
+ host = "npc";
+ }
+ cmd "x.2.resume_training" {
+ exec = "rosservice call /wild_visual_navigation_node/pause_learning 'data: false'";
+ host = "npc";
+ }
+ cmd "x.3.save_checkpoint" {
+ exec = "rosservice call /wild_visual_navigation_node/save_checkpoint ''";
+ host = "anymal_coyote_orin";
+ }
+ cmd "x.4.load_checkpoint" {
+ exec = "rosservice call /wild_visual_navigation_node/load_checkpoint 'path: 'absolute_path_in_robot_filesystem'";
+ host = "npc";
+ }
+}
\ No newline at end of file
diff --git a/wild_visual_navigation_ros/launch/resize_images.launch b/wild_visual_navigation_ros/launch/resize_images.launch
index b1b9801e..813c8de1 100644
--- a/wild_visual_navigation_ros/launch/resize_images.launch
+++ b/wild_visual_navigation_ros/launch/resize_images.launch
@@ -7,7 +7,7 @@
-
+
diff --git a/wild_visual_navigation_ros/launch/robot.launch b/wild_visual_navigation_ros/launch/robot.launch
index 86ce0bc9..c8090112 100644
--- a/wild_visual_navigation_ros/launch/robot.launch
+++ b/wild_visual_navigation_ros/launch/robot.launch
@@ -1,7 +1,7 @@
-
+
@@ -11,6 +11,6 @@
-
+
diff --git a/wild_visual_navigation_ros/launch/wild_visual_navigation.launch b/wild_visual_navigation_ros/launch/wild_visual_navigation.launch
index 1bce2bdd..c8457ed5 100644
--- a/wild_visual_navigation_ros/launch/wild_visual_navigation.launch
+++ b/wild_visual_navigation_ros/launch/wild_visual_navigation.launch
@@ -1,10 +1,10 @@
-
+
-
+
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 13542b19..b849c029 100644
--- a/wild_visual_navigation_ros/scripts/wvn_feature_extractor_node.py
+++ b/wild_visual_navigation_ros/scripts/wvn_feature_extractor_node.py
@@ -41,11 +41,13 @@ def __init__(self):
slic_num_components=self.slic_num_components,
dino_dim=self.dino_dim,
)
- self.i = 0
- self.setup_ros()
+ self.i = 0
+
self.model = get_model(self.exp_cfg["model"]).to(self.device)
self.model.eval()
+
+
if not self.anomaly_detection:
self.confidence_generator = ConfidenceGenerator(
@@ -53,24 +55,28 @@ def __init__(self):
)
self.scale_traversability = True
else:
- self.traversability_loss = AnomalyLoss(**self.exp_cfg["loss_anomaly"],
- log_enabled=self.exp_cfg["general"]["log_confidence"],
- log_folder=self.exp_cfg["general"]["model_path"])
+ self.traversability_loss = AnomalyLoss(**self.exp_cfg["loss_anomaly"], log_enabled=self.exp_cfg["general"]["log_confidence"], log_folder=self.exp_cfg["general"]["model_path"])
self.traversability_loss.to(self.device)
self.scale_traversability = False
if self.verbose:
+ self.log_data = {}
+ self.status_thread_stop_event = Event()
self.status_thread = Thread(target=self.status_thread_loop, name="status")
self.run_status_thread = True
self.status_thread.start()
+ self.setup_ros()
+
rospy.on_shutdown(self.shutdown_callback)
signal.signal(signal.SIGINT, self.shutdown_callback)
signal.signal(signal.SIGTERM, self.shutdown_callback)
def shutdown_callback(self, *args, **kwargs):
self.run_status_thread = False
+ self.status_thread_stop_event.set()
self.status_thread.join()
+
rospy.signal_shutdown(f"Wild Visual Navigation Feature Extraction killed {args}")
sys.exit(0)
@@ -78,6 +84,12 @@ def status_thread_loop(self):
rate = rospy.Rate(self.status_thread_rate)
# Learning loop
while self.run_status_thread:
+
+ self.status_thread_stop_event.wait(timeout=0.01)
+ if self.status_thread_stop_event.is_set():
+ rospy.logwarn("Stopped learning thread")
+ break
+
t = rospy.get_time()
x = PrettyTable()
x.field_names = ["Key", "Value"]
@@ -102,6 +114,8 @@ def status_thread_loop(self):
except Exception as e:
rate = rospy.Rate(self.status_thread_rate)
print("Ignored jump pack in time!")
+ self.status_thread_stop_event.clear()
+
def read_params(self):
"""Reads all the parameters from the parameter server"""
@@ -146,7 +160,6 @@ def setup_ros(self, setup_fully=True):
if self.verbose:
# DEBUG Logging
- self.log_data = {}
self.log_data[f"time_last_model"] = -1
self.log_data[f"nr_model_updates"] = -1
diff --git a/wild_visual_navigation_ros/scripts/wvn_learning_node.py b/wild_visual_navigation_ros/scripts/wvn_learning_node.py
index 90bece6e..8613f7fa 100644
--- a/wild_visual_navigation_ros/scripts/wvn_learning_node.py
+++ b/wild_visual_navigation_ros/scripts/wvn_learning_node.py
@@ -116,7 +116,7 @@ def __init__(self):
signal.signal(signal.SIGTERM, self.shutdown_callback)
# Launch processes
- print("─" * 80)
+ print("-" * 80)
print("Launching [learning] thread")
if self.mode != WVNMode.EXTRACT_LABELS:
self.learning_thread_stop_event = Event()