throttle timing msg

rogeriobonatti · Jun 5, 2022 · d8fb9d0 · d8fb9d0
1 parent 4323728
commit d8fb9d0
Show file tree

Hide file tree

Showing 2 changed files with 8 additions and 6 deletions.
diff --git a/mushr_rhc_ros/launch/sim/sim_server_eval.launch b/mushr_rhc_ros/launch/sim/sim_server_eval.launch
@@ -10,7 +10,7 @@
     <arg name="model_path_act" default="/home/rb/hackathon_data_premium/aml_outputs/log_output/hvd_test_16/GPTcorl_scratch_trainm_e2e_statet_pointnet_traini_1_nla_12_nhe_8_statel_0.01_2022-06-02_1654131996.2524076_2022-06-02_1654131996.2524228/model/epoch30.pth.tar" />
 
     <!-- map model -->
-    <arg name="use_map" default="true" />
+    <arg name="use_map" default="false" />
     <arg name="model_path_map" default="/home/rb/hackathon_data_premium/aml_outputs/log_output/mapscratch_new_0/GPTcorl_map_trainm_map_sta_pointnet_traini_1_nla_12_nhe_8_2022-05-31_1653978768.732001_2022-05-31_1653978768.7320147/model/epoch28.pth.tar" />
 
     <!-- localization model -->

diff --git a/mushr_rhc_ros/src/rhcnode_network_pcl_new.py b/mushr_rhc_ros/src/rhcnode_network_pcl_new.py
@@ -183,6 +183,7 @@ def __init__(self, dtype, params, logger, name):
             map_model.eval()
             map_model.to(device)
             self.map_model = map_model
+            self.map_viz_timer = rospy.Timer(rospy.Duration(1.0 / rate_map_display), self.map_viz_cb)
 
         # localization model
         if self.use_loc:
@@ -213,6 +214,7 @@ def __init__(self, dtype, params, logger, name):
             loc_model.eval()
             loc_model.to(device)
             self.loc_model = loc_model
+            self.map_viz_loc = rospy.Timer(rospy.Duration(1.0 / rate_loc_display), self.loc_viz_cb)
 
 
         self.small_queue = Queue(maxsize = self.clip_len) # stores current scan, action, pose. up to 16 elements
@@ -235,8 +237,7 @@ def __init__(self, dtype, params, logger, name):
         # set timer callbacks for visualization
         rate_map_display = 1.0
         rate_loc_display = 20
-        self.map_viz_timer = rospy.Timer(rospy.Duration(1.0 / rate_map_display), self.map_viz_cb)
-        self.map_viz_loc = rospy.Timer(rospy.Duration(1.0 / rate_loc_display), self.loc_viz_cb)
+
 
 
     def start(self):
@@ -300,7 +301,7 @@ def map_viz_cb(self, timer):
             with torch.inference_mode():
                 self.map_recon, _ = self.map_model(states=x_imgs, actions=x_act, targets=x_act, gt_map=None, timesteps=t, poses=None, compute_loss=False)
             finished_map_network = time.time()
-            rospy.loginfo("map network delay: "+str(finished_map_network-start))
+            # rospy.loginfo("map network delay: "+str(finished_map_network-start))
 
             # publish the GT pose of the map center
             self.pose_marker_pub.publish(self.create_position_marker(pose_mid))
@@ -347,7 +348,7 @@ def loc_viz_cb(self, timer):
             with torch.inference_mode():
                 pose_preds, _, _, _, _ = self.loc_model(states=x_imgs, actions=x_act, targets=x_act, gt_map=None, timesteps=t, poses=None, compute_loss=False)
             finished_loc_network = time.time()
-            rospy.loginfo("loc network delay: "+str(finished_loc_network-start))
+            # rospy.loginfo("loc network delay: "+str(finished_loc_network-start))
             # publish anchor pose of the map center
             self.loc_anchor_pose_marker_pub.publish(self.create_position_marker(self.pose_anchor, color=[0,0,1,1]))
             # publish the current accumulated pose
@@ -522,7 +523,8 @@ def apply_network(self):
             # action_pred = 0.0
             action_pred, _, _, _ = self.model(states=x_imgs, actions=x_act, targets=x_act, gt_map=None, timesteps=t, poses=None, compute_loss=False)
             finished_action_network = time.time()
-            rospy.loginfo("action network delay: "+str(finished_action_network-start))
+            # rospy.loginfo("action network delay: "+str(finished_action_network-start))
+            rospy.loginfo_throttle(10, "action network delay: "+str(finished_action_network-start))
             action_pred = action_pred[0,-1,0].cpu().flatten().item()
             # if self.use_map:
             #     map_pred, loss = self.map_model(states=x_imgs, actions=x_act, targets=x_act, gt_map=None, timesteps=t, poses=None)