heatmap with pose

mushr_rhc_ros/launch/real/record_server.launch

@@ -2,14 +2,14 @@
 <launch>
     <arg name="car_name" default="car" />
 
-    <arg name="record_path" default="/project/recordings"/>
+    <arg name="record_path" default="/home/rb/data"/>
     <arg name="trial_name" default=""/>
 
     <rosparam command="dump" file="$(arg record_path)/params.yaml" />
 
     <group ns="$(arg car_name)">
         <node pkg="rosbag" type="record" name="debug_recorder"
-            args="record -O $(arg record_path)/data.bag
+            args="record -O $(arg record_path)/data_bias.bag
             car_pose
             experiments/finished
             experiments/language_bias

mushr_rhc_ros/launch/sim/sim_server_eval.launch

@@ -15,13 +15,13 @@
     <!-- <arg name="model_path_act" type="str" default="/home/rb/hackathon_data_premium/aml_outputs/log_output/nonoise_resnet_0/GPTcorl_12m_pre_trainm_e2e_statet_resnet18_traini_0.2_nla_12_nhe_8_statel_0.1_weightdecay_0.0001_weightdecayb_0.1_lr_6e-4_2022-06-14_1655235143.077357_2022-06-14_1655235143.0773675/model/epoch17.pth.tar" /> -->
 
     <!-- for pt drop -->
-    <arg name="model_path_act" type="str" default="/home/rb/hackathon_data_premium/aml_outputs/log_output/nonoise_ptdrop_1/GPTcorl_12m_pre_trainm_e2e_statet_pointnet_traini_0.5_nla_12_nhe_8_statel_0.1_weightdecay_0.0001_weightdecayb_0.1_lr_6e-4_2022-06-14_1655247552.6891448_2022-06-14_1655247552.6891577/model/epoch10.pth.tar" />
+    <!-- <arg name="model_path_act" type="str" default="/home/rb/hackathon_data_premium/aml_outputs/log_output/nonoise_ptdrop_1/GPTcorl_12m_pre_trainm_e2e_statet_pointnet_traini_0.5_nla_12_nhe_8_statel_0.1_weightdecay_0.0001_weightdecayb_0.1_lr_6e-4_2022-06-14_1655247552.6891448_2022-06-14_1655247552.6891577/model/epoch10.pth.tar" /> -->
 
     <!-- <arg name="model_path_act" default="/home/rb/hackathon_data_premium/aml_outputs/log_output/partialnoise8m/GPTcorl_8m_pre_trainm_e2e_statet_pointnet_traini_1_nla_12_nhe_8_statel_0.1_weightdecay_0.0001_weightdecayb_0.1_lr_6e-4_2022-06-14_1655185151.5858376_2022-06-14_1655185151.5858524/model/epoch30.pth.tar" /> -->
     <!-- <arg name="model_path_act" default="/home/rb/hackathon_data_premium/aml_outputs/log_output/fullnoise8m/GPTcorl_8m_pre_trainm_e2e_statet_pointnet_traini_1_nla_12_nhe_8_statel_0.1_weightdecay_0.0001_weightdecayb_0.1_lr_6e-4_2022-06-14_1655184530.3072667_2022-06-14_1655184530.307283/model/epoch30.pth.tar" /> -->
 
     <!-- <arg name="model_path_act" default="/home/rb/hackathon_data_premium/aml_outputs/log_output/6m_scratch/GPTcorl_6m_pre_trainm_e2e_statet_pointnet_traini_1_nla_12_nhe_8_statel_0.1_weightdecay_0.0001_weightdecayb_0.1_lr_6e-4_2022-06-13_1655139613.6754146_2022-06-13_1655139613.6754277/model/epoch10.pth.tar" /> -->
-    <!-- <arg name="model_path_act" type="str" default="/home/rb/hackathon_data_premium/aml_outputs/log_output/model_sizes_3/GPTcorl_scratch_trainm_e2e_statet_pointnet_traini_1_nla_12_nhe_8_statel_0.1_weightdecay_0.0001_weightdecayb_0.1_lr_6e-4_2022-06-10_1654843603.5495381_2022-06-10_1654843603.5495553/model/epoch31.pth.tar" /> -->
+    <arg name="model_path_act" type="str" default="/home/rb/hackathon_data_premium/aml_outputs/log_output/model_sizes_3/GPTcorl_scratch_trainm_e2e_statet_pointnet_traini_1_nla_12_nhe_8_statel_0.1_weightdecay_0.0001_weightdecayb_0.1_lr_6e-4_2022-06-10_1654843603.5495381_2022-06-10_1654843603.5495553/model/epoch31.pth.tar" />
 
     <!-- <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" /> -->
     <!-- <arg name="model_path_act" default="/home/rb/hackathon_data_premium/aml_outputs/log_output/model_sizes_0/GPTcorl_scratch_trainm_e2e_statet_pointnet_traini_0_nla_6_nhe_8_statel_0.01_2022-06-03_1654253046.3212142_2022-06-03_1654253046.3212266/model/epoch30.pth.tar" /> -->

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()

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()))
 

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