Feature/logger for bagfiles

.gitignore

@@ -27,6 +27,6 @@ venv
 **/.history
 tests/report.txt
 .ipynb_checkpoints
-logger/output_data
+*/*/*/*/output_data
 *.cache
 bags

ros2_ws/src/utils/logger/README.md

@@ -2,8 +2,15 @@
 Нода логирущая по таймеру истинные координаты, состояние rosbot-а, текущее управление и временную отметку. Залогированные данные нода сохраняет в outut_path в файлах формата csv, а также графики по полученным данным.
 
 Пример запуска
+Запуск с логированием позиции и скорости из топика одометрии
 ```
 ros2 launch logger logger.launch.py
 ``` 
+
+Запуск с логированием позиции и скорости из топика отдельного топика скорости и позы из TF
+```
+ros2 launch logger logger_t265.launch.py
+``` 
+
 Аргументы:
 * output_path - путь до выходной директории (куда будут сохранены залогированые данные)

ros2_ws/src/utils/logger/launch/logger.launch.py

@@ -4,21 +4,21 @@
 from launch_ros.actions import Node
 # from ament_index_python.packages import get_package_share_directory
 
-def generate_launch_description():
 
+def generate_launch_description():
     """
     I can't use get_package_share_directory, cause it will return
     'ros2_ws/install/logger/share/logger', BUT when using 'ros2 launch' 
     ROS launches from its workspace so I can easy get desired directory
-    
+
     * I checked it works well when launch from any directory *
     """
-    output_dir = os.path.join(os.getcwd(), 'src/logger/output_data/') 
+    output_dir = os.path.join(os.getcwd(), 'src/utils/logger/output_data/')
     output_path = launch.substitutions.LaunchConfiguration(
         'output_path',
         default=output_dir
     )
-                  
+
     return LaunchDescription([
 
         launch.actions.DeclareLaunchArgument(
@@ -45,4 +45,4 @@ def generate_launch_description():
             ]
         ),
 
-    ])
+    ])

ros2_ws/src/utils/logger/launch/logger_t265.launch.py

@@ -0,0 +1,49 @@
+import os
+import launch
+from launch import LaunchDescription
+from launch_ros.actions import Node
+# from ament_index_python.packages import get_package_share_directory
+
+
+def generate_launch_description():
+    """
+    I can't use get_package_share_directory, cause it will return
+    'ros2_ws/install/logger/share/logger', BUT when using 'ros2 launch' 
+    ROS launches from its workspace so I can easy get desired directory
+
+    * I checked it works well when launch from any directory *
+    """
+    output_dir = os.path.join(os.getcwd(), 'src/utils/logger/output_data/')
+    output_path = launch.substitutions.LaunchConfiguration(
+        'output_path',
+        default=output_dir
+    )
+
+    return LaunchDescription([
+
+        launch.actions.DeclareLaunchArgument(
+            'output_path',
+            default_value=output_dir,
+            description='output_folder path'
+        ),
+
+        # launh Logger node
+        Node(
+            package='logger',
+            executable='logger',
+            name='logger',
+            output='screen',
+            emulate_tty=True,
+            parameters=[
+                {"output_path": output_path},
+                {"control_topic": "/cmd_vel"},
+                {"tf_topic": "/tf"},
+                {"parent_frame": "odom_frame"},
+                {"robot_frame": "camera_pose_frame"},
+                {"kinetic_model_frame": "model_link"},
+                {"nn_model_frame": "nn_model_link"},
+                {"use_odom_topic": False}
+            ]
+        ),
+
+    ])

ros2_ws/src/utils/logger/logger/create_graphs.py

@@ -8,14 +8,15 @@
 Plot tools for rosbot
 """
 
+
 def build_data_from_T_graph(
     ax,
     robot_state_seq,
     time_seq,
     k_model_state_seq=None,
     nn_model_state_seq=None,
     control_seq=None,
-    legend = ['Rosbot state']
+    legend=['Rosbot state']
 ):
     """
     Args:
@@ -25,7 +26,7 @@ def build_data_from_T_graph(
         :nn_model_state_seq: (pandas.Series)
         :parameters: (dict)
     """
-    
+
     ax.set_xlabel('t')
     ax.set_ylabel(robot_state_seq.name)
     ax.set_title('{}(t)'.format(robot_state_seq.name))
@@ -35,10 +36,10 @@ def build_data_from_T_graph(
         np.array(robot_state_seq),
     )
 
-    kinetic_model_exist = k_model_state_seq is not None 
+    kinetic_model_exist = k_model_state_seq is not None
     nn_model_exist = nn_model_state_seq is not None
     control_seq_exist = control_seq is not None
-    
+
     if kinetic_model_exist:
         legend = legend + ['Kinematic model state']
         ax.plot(
@@ -63,6 +64,7 @@ def build_data_from_T_graph(
     ax.grid()
     ax.legend(legend)
 
+
 def build_XY_graph(
     ax,
     robot_state_df,
@@ -88,8 +90,8 @@ def build_XY_graph(
         linestyle='--'
     )
 
-    kinetic_model_exist = k_model_state_df is not None  
-    nn_model_exist = nn_model_state_df is not None 
+    kinetic_model_exist = k_model_state_df is not None
+    nn_model_exist = nn_model_state_df is not None
 
     if kinetic_model_exist:
         legend = legend + ['Kinematic model state']
@@ -106,26 +108,26 @@ def build_XY_graph(
         )
 
     ax.legend(legend)
-
 
-def build_group_of_graphs(    
+
+def build_group_of_graphs(
     robot_state_df,
     control_df=None,
-    time_list=None,     
+    time_list=None,
     k_model_state_df=None,
     nn_model_state_df=None,
     keys=list()
 ):
     """
     """
     fig, axs = plt.subplots(len(keys))
-    
+
     for i in range(len(keys)):
         key = keys[i]
-        k_model_state_seq_= k_model_state_df[key] if k_model_state_df is not None else None
-        nn_model_state_seq_= nn_model_state_df[key] if nn_model_state_df is not None else None
+        k_model_state_seq_ = k_model_state_df[key] if k_model_state_df is not None else None
+        nn_model_state_seq_ = nn_model_state_df[key] if nn_model_state_df is not None else None
         control_seq_ = control_df[key] if control_df is not None and key in control_df.columns else None
-        
+
         build_data_from_T_graph(
             axs[i],
             robot_state_df[key],
@@ -136,10 +138,11 @@ def build_group_of_graphs(
         )
     return fig
 
+
 def build_general_graph_for_rosbot(
     robot_state_df,
     control_df=None,
-    time_list=None,     
+    time_list=None,
     k_model_state_df=None,
     nn_model_state_df=None,
     save_to_png=True,
@@ -155,63 +158,64 @@ def build_general_graph_for_rosbot(
     """
 
     plt.grid(True)
-    plt.legend(loc='best') 
+    plt.legend(loc='best')
 
     plt.subplots_adjust(
         left=0.15,
-        bottom=0.15, 
-        right=0.8, 
-        top=0.8, 
-        wspace=0.5, 
+        bottom=0.15,
+        right=0.8,
+        top=0.8,
+        wspace=0.5,
         hspace=0.5
     )
 
     fig_x_y_z = build_group_of_graphs(
         robot_state_df,
         control_df=control_df,
-        time_list=time_list,     
+        time_list=time_list,
         k_model_state_df=k_model_state_df,
         nn_model_state_df=nn_model_state_df,
         keys=['x', 'y', 'z']
     )
-    
+
     if save_to_png and path is not None:
-        plt.savefig('{}.{}'.format(path + 'X_Y_Z_graph', 'png'), fmt='png')
+        plt.savefig('{}.{}'.format(path + 'X_Y_Z_graph', 'png'))
 
     fig_angs = build_group_of_graphs(
         robot_state_df,
         control_df=control_df,
-        time_list=time_list,     
+        time_list=time_list,
         k_model_state_df=k_model_state_df,
         nn_model_state_df=nn_model_state_df,
         keys=['roll', 'pitch', 'yaw']
     )
-    
+
     if save_to_png and path is not None:
-        plt.savefig('{}.{}'.format(path + 'Angles_graph', 'png'), fmt='png')
+        plt.savefig('{}.{}'.format(path + 'Angles_graph', 'png'))
 
     fig_lin_vels = build_group_of_graphs(
         robot_state_df,
         control_df=control_df,
-        time_list=time_list,     
+        time_list=time_list,
         k_model_state_df=k_model_state_df,
         nn_model_state_df=nn_model_state_df,
         keys=['v_x', 'v_y', 'v_z']
     )
     if save_to_png and path is not None:
-        plt.savefig('{}.{}'.format(path + 'Linear_velocities_graph', 'png'), fmt='png')
+        plt.savefig('{}.{}'.format(
+            path + 'Linear_velocities_graph', 'png'))
 
     fig_ang_vels = build_group_of_graphs(
         robot_state_df,
         control_df=control_df,
-        time_list=time_list,     
+        time_list=time_list,
         k_model_state_df=k_model_state_df,
         nn_model_state_df=nn_model_state_df,
         keys=['w_x', 'w_y', 'w_z']
     )
     if save_to_png and path is not None:
-        plt.savefig('{}.{}'.format(path + 'Angular_velocities_graph', 'png'), fmt='png')
-
+        plt.savefig('{}.{}'.format(
+            path + 'Angular_velocities_graph', 'png'))
 
     fig_xy, ax = plt.subplots(1)
     build_XY_graph(
@@ -222,9 +226,9 @@ def build_general_graph_for_rosbot(
     )
 
     if save_to_png and path is not None:
-        plt.savefig('{}.{}'.format(path + 'XY_graph', 'png'), fmt='png')
+        plt.savefig('{}.{}'.format(path + 'XY_graph', 'png'))
+
 
-
 def main():
     """
 
@@ -238,16 +242,16 @@ def main():
         required=True,
         help='absolute path to the folder with data.csv'
     )
-    
+
     # second arg
     parser.add_argument(
         '-output_folder',
-        action='store', 
-        dest='output_folder', 
+        action='store',
+        dest='output_folder',
         required=False,
-        default=None, 
+        default=None,
         help="absolute path to the output folder"
-    )       
+    )
 
     args = parser.parse_args()
 
@@ -256,26 +260,21 @@ def main():
     control_df, time = data[3:]
 
     k_model_state_df = None if len(k_model_state_df) < 2 else k_model_state_df
-    nn_model_state_df = None if len(nn_model_state_df) < 2 else nn_model_state_df
+    nn_model_state_df = None if len(
+        nn_model_state_df) < 2 else nn_model_state_df
 
     fig = build_general_graph_for_rosbot(
         rosbot_state_df,
         control_df=control_df,
-        time_list=list(time['t']),     
+        time_list=list(time['t']),
         k_model_state_df=k_model_state_df,
         nn_model_state_df=nn_model_state_df,
         save_to_png=True,
         path=args.output_folder
     )
 
-    # if args.output_folder is not None:
-    #     plt.savefig(
-    #         '{}'.format(args.output_folder + '/create_graphs.png'),
-    #         fmt='png'
-    #     )
-
     plt.show()
 
+
 if __name__ == '__main__':
     main()
-