Implement the display of progress info (#12)

* feat: support multi-file evalutaion for ros2bag

(a mixture of csv and ros2bag is allowed)

* fix: fix the dependencies of corresponding sub_scripts

* feat: update README

* feat: enrich error message for undefined data type

* hotfix: fix the dependencies for corresponding sub_scripts

(to be refactored)

* feat: implement the display of progress info

* feat: update the README

README.md

@@ -1,4 +1,4 @@
-# localization_evaluation_toolkit -Ver.6.0 (Update 2023/02/03)
+# localization_evaluation_toolkit -Ver.6.1 (Update 2023/03/14)
 You can evaluate your localization result by comparing it to a reliable pose trajectory.
 The start time, end time, and period can be different for each data.
 The evaluation is automatically aligned with the one with the smaller number of data.
@@ -148,8 +148,8 @@ degree_type: 0 # [0]:radian, [1]:degree
 dilution_step: 10 # at least 1, the larger the sparser for better performance
 
 # Trajectory graph numbering
-progress_info: 0 # [0]:no display, [1]: number, [2]:time, [3]:ros time, [4]:distance
-interval: 0      # progress_info is [2]:per second, [3]:per second, [4]:per meter
+progress_info: 0 # [0]:off, [1]:number, [2]:time, [3]:ros time, [4]:distance
+interval: 0      # progress_info is [2]:second, [3]:second, [4]:meter
 
 # Font
 title_font_size: 14 

config/evaluation.yaml

@@ -142,8 +142,8 @@ degree_type: 0 # [0]:radian, [1]:degree
 dilution_step: 10 # at least 1, the larger the sparser for better performance
 
 # Trajectory graph numbering
-progress_info: 0 # [0]:no display, [1]: number, [2]:time, [3]:ros time, [4]:distance
-interval: 0      # progress_info is [2]:per second, [3]:per second, [4]:per meter
+progress_info: 0 # [0]:off, [1]:number, [2]:time, [3]:ros time, [4]:distance
+interval: 0      # progress_info is [2]:second, [3]:second, [4]:meter
 
 # Font
 title_font_size: 14 

sample_data/config/sasashima_evaluation.yaml

@@ -167,8 +167,8 @@ degree_type: 0 # [0]:radian, [1]:degree
 dilution_step: 10 # at least 1, the larger the sparser for better performance
 
 # Trajectory graph numbering
-progress_info: 0 # [0]:no display, [1]: number, [2]:time, [3]:ros time, [4]:distance
-interval: 0      # progress_info is [2]:per second, [3]:per second, [4]:per meter
+progress_info: 0 # [0]:off, [1]:number, [2]:time, [3]:ros time, [4]:distance
+interval: 0      # progress_info is [2]:second, [3]:second, [4]:meter
 
 # Font
 title_font_size: 14 

scripts/util/packer.py

@@ -39,7 +39,7 @@ def read_csv(param: DataParam) -> pd.DataFrame:
         adjusted_data = pd.DataFrame()
 
         # time
-        if param.separate_time_stamp == True:
+        if param.separate_time_stamp:
             adjusted_data["time"] = original_data.iloc[:, param.secs_stamp_column] + original_data.iloc[:, param.nsecs_stamp_column] / 10 ** 9 + param.tf_time
         elif len(str(int(original_data.iloc[0, param.stamp_column]))) > 10:
             adjusted_data["time"] = original_data.iloc[:, param.stamp_column] / 10 ** 9 + param.tf_time
@@ -52,7 +52,7 @@ def read_csv(param: DataParam) -> pd.DataFrame:
         adjusted_data["z"] = original_data.iloc[:, param.z_column] + param.tf_z
 
         # rotation
-        if param.use_quaternion == True:
+        if param.use_quaternion:
             for i in range(len(original_data)):
                 ref_q_temp = [
                     original_data.iloc[i, param.ori_x_column],
@@ -68,17 +68,17 @@ def read_csv(param: DataParam) -> pd.DataFrame:
                     ref_e_temp.as_euler("ZYX", degrees=False)[1] + param.tf_pitch
                 ) * param.inv_pitch
                 adjusted_data.at[i, "yaw"] = (ref_e_temp.as_euler("ZYX", degrees=False)[0] + param.tf_yaw) * param.inv_yaw
-        elif param.use_quaternion == False and param.use_radian == True:
+        elif (not param.use_quaternion) and param.use_radian:
             adjusted_data["roll"] = (original_data.iloc[:, param.roll_column] + param.tf_roll) * param.inv_roll
             adjusted_data["pitch"] = (original_data.iloc[:, param.pitch_column] + param.tf_pitch) * param.inv_pitch
             adjusted_data["yaw"] = (original_data.iloc[:, param.yaw_column] + param.tf_yaw) * param.inv_yaw
-        elif param.use_quaternion == False and param.use_radian == False:
+        elif (not param.use_quaternion) and (not param.use_radian):
             adjusted_data["roll"] = (original_data.iloc[:, param.roll_column] * math.pi / 180 + param.tf_roll) * param.inv_roll
             adjusted_data["pitch"] = (original_data.iloc[:, param.pitch_column] * math.pi / 180 + param.tf_pitch) * param.inv_pitch
             adjusted_data["yaw"] = (original_data.iloc[:, param.yaw_column] * math.pi / 180 + param.tf_yaw) * param.inv_yaw
 
         # covariance
-        if param.display_ellipse == True:
+        if param.display_ellipse:
             adjusted_data["cov_xx"] = original_data.iloc[:, param.covariance_xx_column]
             adjusted_data["cov_xy"] = original_data.iloc[:, param.covariance_xy_column]
             adjusted_data["cov_yx"] = original_data.iloc[:, param.covariance_yx_column]
@@ -93,9 +93,7 @@ def read_ros2bag(param: DataParam) -> pd.DataFrame:
         from rosidl_runtime_py.utilities import get_message
 
         storage_options = rosbag2_py.StorageOptions(uri=param.path, storage_id=param.bag_id)
-        converter_options = rosbag2_py.ConverterOptions(
-            input_serialization_format=param.bag_format, output_serialization_format=param.bag_format
-        )
+        converter_options = rosbag2_py.ConverterOptions(input_serialization_format=param.bag_format, output_serialization_format=param.bag_format)
 
         reader = rosbag2_py.SequentialReader()
         reader.open(storage_options, converter_options)
@@ -132,7 +130,7 @@ def read_ros2bag(param: DataParam) -> pd.DataFrame:
             pose_data_dict["pitch"].append((e_temp.as_euler("ZYX", degrees=False)[1] + param.tf_pitch) * param.inv_pitch)
             pose_data_dict["yaw"].append((e_temp.as_euler("ZYX", degrees=False)[0] + param.tf_yaw) * param.inv_yaw)
             # covariance
-            if param.display_ellipse == True:
+            if param.display_ellipse:
                 cov_data_dict["cov_xx"].append(msg.pose.covariance[0])
                 cov_data_dict["cov_xy"].append(msg.pose.covariance[1])
                 cov_data_dict["cov_yx"].append(msg.pose.covariance[6])

scripts/util/plotter.py

@@ -15,16 +15,13 @@ def plot_2d_traj(ref_pack: RefDataPack, res_packs: List[ResDataPack], opt_param:
     fig_2d_trj = plt.figure("2D_Trajectory", figsize=(16, 9), dpi=120)
     ax_2d_trj = fig_2d_trj.add_subplot(111)
 
-    # dilute trajectory data for better performance
-    res_dfs = [(
-        res_pack,
-        res_pack.data.iloc[::opt_param.dilution_step],
-        res_pack.data_ref.iloc[::opt_param.dilution_step]
-    ) for res_pack in res_packs]
-    ref_df = ref_pack.accumulate_df(list(zip(*res_dfs))[2])
-
-    ax_2d_trj.scatter(ref_df["x"], ref_df["y"], c="k", zorder=1, label=ref_pack.label)
-    for res_pack, res_df, ref_df in res_dfs:
+    ref_dfs = list()
+    for res_pack in res_packs:
+        # dilute trajectory data for better performance
+        res_df = res_pack.data.iloc[::opt_param.dilution_step]
+        ref_df = res_pack.data_ref.iloc[::opt_param.dilution_step]
+        ref_dfs.append(ref_df)
+        # plot the traj of results
         ax_2d_trj.scatter(res_df["x"], res_df["y"], s=2, label=res_pack.label)
         ax_2d_trj.plot([ref_df["x"], res_df["x"]], [ref_df["y"], res_df["y"]], "-", linewidth=0.2, zorder=1)
         # show ellipse
@@ -37,7 +34,26 @@ def plot_2d_traj(ref_pack: RefDataPack, res_packs: List[ResDataPack], opt_param:
                     height=res_df["ellipse_short"][i] * 2,
                     alpha=0.3
                 ))
-        # show progress TODO
+        # show progress
+        if opt_param.progress_info:
+            if opt_param.progress_info == 1:
+                data = res_df.index
+            elif opt_param.progress_info == 2:
+                data = res_df["elapsed"]
+            elif opt_param.progress_info == 3:
+                data = res_df["time"]
+            elif opt_param.progress_info == 4:
+                data = res_df["distance"]
+            else:
+                raise RuntimeError("Unexpected progress info type")
+            counter = 0
+            for i in res_df.index:
+                if (data[i] - data[0]) < counter: continue
+                ax_2d_trj.text(res_df["x"][i], res_df["y"][i], round(data[i], 2), va="bottom")
+                counter += opt_param.interval
+    # plot the traj of reference
+    ref_df = ref_pack.accumulate_df(ref_dfs)
+    ax_2d_trj.scatter(ref_df["x"], ref_df["y"], c="k", zorder=-1, label=ref_pack.label)
 
     ax_2d_trj.set_title("2D trajectory", fontsize=opt_param.title_font_size)
     ax_2d_trj.set_xlabel("x[m]", fontsize=opt_param.label_font_size)