From 83bd487fe3190b252ec96f9ce16bed8f7c6d21d2 Mon Sep 17 00:00:00 2001 From: Rogerio Bonatti Date: Thu, 11 Aug 2022 11:17:15 -0700 Subject: [PATCH] heatmap with pose --- .../launch/real/record_server.launch | 4 +- .../launch/sim/sim_server_eval.launch | 4 +- mushr_rhc_ros/src/pose_heatmap.py | 99 +++++++++++++++++++ mushr_rhc_ros/src/preprocessing_utils.py | 2 +- mushr_rhc_ros/src/rhcnode_network_pcl_new.py | 42 ++++++-- 5 files changed, 140 insertions(+), 11 deletions(-) create mode 100644 mushr_rhc_ros/src/pose_heatmap.py diff --git a/mushr_rhc_ros/launch/real/record_server.launch b/mushr_rhc_ros/launch/real/record_server.launch index b675178..47cf169 100644 --- a/mushr_rhc_ros/launch/real/record_server.launch +++ b/mushr_rhc_ros/launch/real/record_server.launch @@ -2,14 +2,14 @@ - + --> - + - + diff --git a/mushr_rhc_ros/src/pose_heatmap.py b/mushr_rhc_ros/src/pose_heatmap.py new file mode 100644 index 0000000..6169bf9 --- /dev/null +++ b/mushr_rhc_ros/src/pose_heatmap.py @@ -0,0 +1,99 @@ +import rosbag +import os +import glob +import numpy as np +import matplotlib.pyplot as plt +import seaborn as sns + +def extract_bag_variables(bag_name): + bag = rosbag.Bag(bag_name) + # initialize data structures of the correct size + + poses = np.zeros((bag.get_message_count('car_pose'),3), dtype=np.float) + idx_dict = { + "experiments/finished": 0, + "mux/ackermann_cmd_mux/input/navigation": 0, + "mux/ackermann_cmd_mux/active": 0, + "scan": 0, + "car_pose": 0 + } + # fill the data structures + for topic, msg, t in bag.read_messages(topics=['car_pose']): + idx = idx_dict[topic] + poses[idx, 0] = msg.header.stamp.to_sec() + poses[idx, 1] = msg.pose.position.x + poses[idx, 2] = msg.pose.position.y + idx_dict[topic] += 1 + + bag.close() + return poses + +def plot_poses(bag_location): + bag_fname = bag_location.split(".")[0] + poses = extract_bag_variables(bag_location) + + cmap_list = [ + # "crest", + # "flare", + # "magma", + "viridis", + "rocket_r", + # "cubehelix" + ] + + resolution = 0.05 + num_pixels_per_meter = 1. / resolution + origin = np.array([-32.925,-37.3]) + origin_pixel = origin * num_pixels_per_meter + + poses = poses[:,1:] + poses *= num_pixels_per_meter + np.swapaxes(poses, 0,1) # swap x and y + poses *= np.array([1, -1]) # flip y + poses -= origin_pixel # translate + img = plt.imread('/home/rb/hackathon_data_premium/hackathon_data_2p5_nonoise3/bravern_floor.png') + alpha = 0.66 + traj_color = "magenta" + + for cmap in cmap_list: + plt.figure() + plt.imshow(img, cmap=plt.cm.gray, alpha=alpha) + res=sns.kdeplot(poses[:,0], poses[:,1], cmap="viridis", shade=True, fill=True, alpha=alpha) + plt.axis('off') + plt.savefig("/home/rb/data/0.jpg", bbox_inches = 'tight', dpi=1200) + + # plt.figure() + # plt.imshow(img, cmap=plt.cm.gray) + # res=sns.kdeplot(poses[:,0], poses[:,1], cmap=cmap, fill=False, alpha=alpha, linewidth=0.25) + # plt.axis('off') + # plt.savefig("/home/rb/data/1.jpg", bbox_inches = 'tight', dpi=1200) + + # plt.figure() + # plt.imshow(img, cmap=plt.cm.gray) + # plt.scatter(poses[:,0], poses[:,1], s=0.25, marker='o', c=traj_color) + # plt.axis('off') + # plt.savefig("/home/rb/data/2.jpg", bbox_inches = 'tight', dpi=1200) + + # plt.figure() + # plt.imshow(img, cmap=plt.cm.gray) + # plt.scatter(poses[:,0], poses[:,1], s=0.25, marker='o', c=traj_color) + # res=sns.kdeplot(poses[:,0], poses[:,1], cmap=cmap, fill=True, alpha=alpha, linewidth=0.25) + # plt.axis('off') + # plt.savefig("/home/rb/data/3.jpg", bbox_inches = 'tight', dpi=1200) + + # plt.figure() + # plt.imshow(img, cmap=plt.cm.gray) + # plt.scatter(poses[:,0], poses[:,1], s=0.25, marker='o', c=traj_color) + # res=sns.kdeplot(poses[:,0], poses[:,1], cmap=cmap, fill=False, alpha=alpha, linewidth=0.25) + # plt.axis('off') + # plt.savefig("/home/rb/data/4.jpg", bbox_inches = 'tight', dpi=1200) + +def main(): + bag_list = ["/home/rb/data/data.bag"] + # bag_list = ["/home/rb/data/data_bias.bag"] + + for bag in bag_list: + plot_poses(bag) + +if __name__=="__main__": + main() \ No newline at end of file diff --git a/mushr_rhc_ros/src/preprocessing_utils.py b/mushr_rhc_ros/src/preprocessing_utils.py index 022f004..f28d68c 100644 --- a/mushr_rhc_ros/src/preprocessing_utils.py +++ b/mushr_rhc_ros/src/preprocessing_utils.py @@ -19,7 +19,7 @@ def load_params(data_col, is_real_deployment=True): if not is_real_deployment: # print("num of nans before: {}".format(np.isnan(data_col[1:]).sum())) # insert artificial drop in lasers - random_indices_to_replace = np.random.choice(np.arange(1,data_col.shape[0]), int(data_col[1:].shape[0]*0.2), replace=False) + random_indices_to_replace = np.random.choice(np.arange(1,data_col.shape[0]), int(data_col[1:].shape[0]*0.0), replace=False) data_col[random_indices_to_replace] = np.nan # print("num of nans after: {}".format(np.isnan(data_col[1:]).sum())) diff --git a/mushr_rhc_ros/src/rhcnode_network_pcl_new.py b/mushr_rhc_ros/src/rhcnode_network_pcl_new.py index 70c0932..a6f0335 100755 --- a/mushr_rhc_ros/src/rhcnode_network_pcl_new.py +++ b/mushr_rhc_ros/src/rhcnode_network_pcl_new.py @@ -71,6 +71,7 @@ def __init__(self, dtype, params, logger, name): self.hp_world = None self.time_started_goal = None self.num_trials = 0 + self.global_counter = 0 self.act_inference_time_sum = 0.0 self.act_inference_time_count = 0 @@ -84,8 +85,8 @@ def __init__(self, dtype, params, logger, name): self.events = [self.goal_event, self.map_metadata_event, self.ready_event] self.run = True - # self.default_speed = 2.5 - self.default_speed = 1.0 + self.default_speed = 2.5 + # self.default_speed = 0.0 self.default_angle = 0.0 self.nx = None self.ny = None @@ -545,6 +546,17 @@ def publish_vel_marker(self): def apply_network(self): + + # return the default action if the queue doesn't have enough length + # find the middle pose for plotting reference + self.small_queue_lock.acquire() + pos_queue_list = list(self.small_queue.queue) + self.small_queue_lock.release() + pos_size = len(pos_queue_list) + if pos_size<16: + print("returning early!!!!!") + return self.default_angle + start_zero = time.time() x_imgs, x_act, t = self.prepare_model_inputs(queue_type='small') start = time.time() @@ -636,6 +648,10 @@ def check_reset(self, rate_hz): self.distance_so_far = 0.0 self.time_so_far = 0.0 self.last_reset_time = time.time() + # need to empty the small queue as well, and zero the last action so we don't bias the next episode + self.last_action = self.default_angle + self.small_queue = Queue(maxsize = self.clip_len) # stores current scan, action, pose. up to 16 elements + self.global_counter = 0 return True else: return False @@ -651,10 +667,19 @@ def send_initial_pose(self): # msg.pose.pose.position.y = hp_world_valid[new_pos_idx][1] # msg.pose.pose.position.z = 0.0 # quat = utilss.angle_to_rosquaternion(hp_world_valid[new_pos_idx][1]) - msg.pose.pose.position.x = 4.12211 + (np.random.rand()-0.5)*2.0*0.5 - msg.pose.pose.position.y = -7.49623 + (np.random.rand()-0.5)*2.0*0.5 + + # usual eval place + # msg.pose.pose.position.x = 4.12211 + (np.random.rand()-0.5)*2.0*0.5 + # msg.pose.pose.position.y = -7.49623 + (np.random.rand()-0.5)*2.0*0.5 + # msg.pose.pose.position.z = 0.0 + # quat = utilss.angle_to_rosquaternion(np.radians(68 + (np.random.rand()-0.5)*2.0*360)) # 360 instead of zero at the end + + # prompting place + msg.pose.pose.position.x = 6.53540635068 + (np.random.rand()-0.5)*2.0*0.1 + msg.pose.pose.position.y = -1.8433986464 + (np.random.rand()-0.5)*2.0*0.1 msg.pose.pose.position.z = 0.0 - quat = utilss.angle_to_rosquaternion(np.radians(68 + (np.random.rand()-0.5)*2.0*360)) # 360 instead of zero at the end + quat = utilss.angle_to_rosquaternion(np.radians(68 + (np.random.rand()-0.5)*2.0*0)) # 360 instead of zero at the end + msg.pose.pose.orientation = quat self.did_reset = True @@ -720,11 +745,16 @@ def cb_scan(self, msg): # first process all the information processed_scan = self.process_scan(msg) - current_action = copy.deepcopy(self.last_action) + if self.global_counter >16: + current_action = copy.deepcopy(self.last_action) + else: + current_action = -0.3 + self.global_counter += 1 self.pos_lock.acquire() current_pose_gt = copy.deepcopy(self.curr_pose) self.pos_lock.release() current_pose_pred = copy.deepcopy(self.current_frame) + queue_element = [processed_scan, current_action, current_pose_gt, current_pose_pred] # update the small queue