From f94d1b9affd9b8f98cec9352e3bd11f66e76dc6b Mon Sep 17 00:00:00 2001 From: Rogerio Bonatti Date: Mon, 6 Jun 2022 08:22:01 -0700 Subject: [PATCH] changes for real deploy --- .../launch/sim/sim_server_eval.launch | 2 +- mushr_rhc_ros/src/rhcnode_network_pcl_new.py | 33 +++++++++++++------ 2 files changed, 24 insertions(+), 11 deletions(-) diff --git a/mushr_rhc_ros/launch/sim/sim_server_eval.launch b/mushr_rhc_ros/launch/sim/sim_server_eval.launch index d4eb742..71d7f8a 100644 --- a/mushr_rhc_ros/launch/sim/sim_server_eval.launch +++ b/mushr_rhc_ros/launch/sim/sim_server_eval.launch @@ -15,7 +15,7 @@ - + diff --git a/mushr_rhc_ros/src/rhcnode_network_pcl_new.py b/mushr_rhc_ros/src/rhcnode_network_pcl_new.py index af52bb5..5ba14fd 100755 --- a/mushr_rhc_ros/src/rhcnode_network_pcl_new.py +++ b/mushr_rhc_ros/src/rhcnode_network_pcl_new.py @@ -100,6 +100,8 @@ def __init__(self, dtype, params, logger, name): self.device = device self.clip_len = 16 + self.is_real_deployment = rospy.get_param("~is_real_deployment", False) + self.map_type = rospy.get_param("~deployment_map", 'train') self.use_map = rospy.get_param("~use_map", False) @@ -186,7 +188,7 @@ def __init__(self, dtype, params, logger, name): map_model.to(device) self.map_model = map_model rate_map_display = 1.0 - self.map_viz_timer = rospy.Timer(rospy.Duration(1.0 / rate_map_display), self.map_viz_cb) + # localization model if self.use_loc: @@ -218,7 +220,7 @@ def __init__(self, dtype, params, logger, name): loc_model.to(device) self.loc_model = loc_model rate_loc_display = 20 - 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 @@ -238,6 +240,12 @@ def __init__(self, dtype, params, logger, name): self.time_so_far = 0.0 self.file_name = os.path.join(self.out_path,'info.csv') + # define timer callbacks: + if self.use_map: + self.map_viz_loc = rospy.Timer(rospy.Duration(1.0 / rate_loc_display), self.loc_viz_cb) + if self.use_loc: + self.map_viz_timer = rospy.Timer(rospy.Duration(1.0 / rate_map_display), self.map_viz_cb) + @@ -272,8 +280,9 @@ def start(self): while not rospy.is_shutdown() and self.run: # check if we should reset the vehicle if crashed - if self.check_reset(rate_hz): - rospy.loginfo("Resetting the car's position") + if not self.is_real_deployment: + if self.check_reset(rate_hz): + rospy.loginfo("Resetting the car's position") # publish next action if self.compute_network_action: @@ -295,16 +304,20 @@ def map_viz_cb(self, timer): pos_queue_list = list(self.small_queue.queue) self.small_queue_lock.release() pos_size = len(pos_queue_list) - pose_mid = pos_queue_list[int(pos_size/2) -1][2] - + if pos_size==16: + pose_mid = pos_queue_list[int(pos_size/2) -1][2] + # if not self.is_real_deployment: + # pose_mid = pos_queue_list[int(pos_size/2) -1][2] + # else: + # pose_mid = PoseStamped() x_imgs, x_act, t = self.prepare_model_inputs(queue_type='small') start = time.time() # with torch.set_grad_enabled(False): 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)) @@ -351,7 +364,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 @@ -526,8 +539,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_throttle(10, "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)