Skip to content

Commit

Permalink
:Merge branch 'dev/repair_develop' of github.com:leggedrobotics/wild_…
Browse files Browse the repository at this point in the history 
…visual_navigation into dev/repair_develop
  • Loading branch information
@RobinSchmid7
RobinSchmid7 committed Sep 1, 2023
2 parents 2877f28 + b11a435 commit 9030cd2
Show file tree
Hide file tree
Showing 6 changed files with 260 additions and 12 deletions.
235 changes: 235 additions & 0 deletions wild_visual_navigation_ros/config/procman/robot_dodo.pmd
View file
Original file line number Diff line number Diff line change
@@ -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";
}
}
2 changes: 1 addition & 1 deletion wild_visual_navigation_ros/launch/resize_images.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,7 +7,7 @@
<arg name="resize_alphasense_cam4" default="True"/>
<arg name="resize_alphasense_cam5" default="True"/>

<arg name="hdr_cam" default="True"/>
<arg name="hdr_cam" default="False"/>
<node if="$(arg hdr_cam)" name="hdr_cam_driver" pkg="usb_cam" type="usb_cam_node">
<rosparam command="load" file="$(find wild_visual_navigation_ros)/config/hdr_cam/hdr_cam_config.yaml"/>
</node>
Expand Down
4 changes: 2 additions & 2 deletions wild_visual_navigation_ros/launch/robot.launch
View file
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
<launch>
<!-- Arguments -->
<arg name="elevation_mapping" default="False"/>
<arg name="camera" default="alphasense_resize"/> <!-- alphasense, wide_angle_front, realsense_front, realsense_rear -->
<arg name="camera" default="wide_angle_front"/> <!-- alphasense, wide_angle_front, realsense_front, realsense_rear -->
<arg name="stack" default="anybotics"/> <!-- rsl or anybotics -->

<!-- Use Elevation Mapping-->
Expand All @@ -11,6 +11,6 @@
<include file="$(find wild_visual_navigation_ros)/launch/wild_visual_navigation.launch">
<arg name="camera" value="$(arg camera)"/>
<arg name="stack" value="$(arg stack)"/>
<arg name="reload_default_params" value="True"/>
<arg name="reload_default_params" value="False"/>
</include>
</launch>
4 changes: 2 additions & 2 deletions wild_visual_navigation_ros/launch/wild_visual_navigation.launch
View file
Original file line number Diff line number Diff line change
@@ -1,10 +1,10 @@
<launch>
<!-- Arguments -->
<arg name="camera" default="alphasense_resize"/> <!-- alphasense, wide_angle_front, realsense_front, realsense_rear -->
<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="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="True"/>
<arg name="resize_images" default="$(eval arg('camera') == 'alphasense_resize')"/>
<arg name="reload_default_params" default="False"/>

<!-- Load parameters -->
Expand Down
25 changes: 19 additions & 6 deletions wild_visual_navigation_ros/scripts/wvn_feature_extractor_node.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,43 +41,55 @@ 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(
method=self.exp_cfg["loss"]["method"], std_factor=self.exp_cfg["loss"]["confidence_std_factor"]
)
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)

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"]
Expand All @@ -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"""
Expand Down Expand Up @@ -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

Expand Down
2 changes: 1 addition & 1 deletion wild_visual_navigation_ros/scripts/wvn_learning_node.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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()
Expand Down

0 comments on commit 9030cd2

Please sign in to comment.