diff --git a/.gitignore b/.gitignore index 29686d4..169ad9e 100644 --- a/.gitignore +++ b/.gitignore @@ -27,6 +27,6 @@ venv **/.history tests/report.txt .ipynb_checkpoints -logger/output_data +*/*/*/*/output_data *.cache bags diff --git a/ros2_ws/src/utils/logger/README.md b/ros2_ws/src/utils/logger/README.md index 847fa9b..43c8f1d 100644 --- a/ros2_ws/src/utils/logger/README.md +++ b/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 - путь до выходной директории (куда будут сохранены залогированые данные) diff --git a/ros2_ws/src/utils/logger/launch/logger.launch.py b/ros2_ws/src/utils/logger/launch/logger.launch.py index 73c933a..90d0aad 100755 --- a/ros2_ws/src/utils/logger/launch/logger.launch.py +++ b/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(): ] ), - ]) \ No newline at end of file + ]) diff --git a/ros2_ws/src/utils/logger/launch/logger_t265.launch.py b/ros2_ws/src/utils/logger/launch/logger_t265.launch.py new file mode 100755 index 0000000..daec0e7 --- /dev/null +++ b/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} + ] + ), + + ]) diff --git a/ros2_ws/src/utils/logger/logger/create_graphs.py b/ros2_ws/src/utils/logger/logger/create_graphs.py index 88f5b2e..9dfaff2 100755 --- a/ros2_ws/src/utils/logger/logger/create_graphs.py +++ b/ros2_ws/src/utils/logger/logger/create_graphs.py @@ -8,6 +8,7 @@ Plot tools for rosbot """ + def build_data_from_T_graph( ax, robot_state_seq, @@ -15,7 +16,7 @@ def build_data_from_T_graph( 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,12 +108,12 @@ 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() @@ -119,13 +121,13 @@ def build_group_of_graphs( """ """ 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() - diff --git a/ros2_ws/src/utils/logger/logger/logger.py b/ros2_ws/src/utils/logger/logger/logger.py index f72f41e..039cfa6 100644 --- a/ros2_ws/src/utils/logger/logger/logger.py +++ b/ros2_ws/src/utils/logger/logger/logger.py @@ -1,21 +1,21 @@ import os import pandas as pd -from matplotlib import pyplot as plt -import rclpy import numpy as np - +from scipy.spatial.transform import Rotation +import rclpy from rclpy.node import Node +from rclpy.qos import QoSProfile, QoSHistoryPolicy, QoSReliabilityPolicy +import tf2_ros from geometry_msgs.msg import Twist from nav_msgs.msg import Odometry from std_srvs.srv import Empty - -from logger.utils import convert_ros2_time_to_float +from logger.utils import convert_ros2_time_to_float, transform_to_pose from logger.create_graphs import build_general_graph_for_rosbot -from scipy.spatial.transform import Rotation + class Logger(Node): """ - Class for logging the state of the rosbot + Class for logging the state of the robot Node for logging the state of the robot, kinematic model (optional) and neural network model (optional), control and time stamps @@ -29,31 +29,33 @@ class Logger(Node): :parent_frame: (str) name of the origin tf frame :kinetic_model_frame: (str) name of the kinematic model tf frame :nn_model_frame: (str) name of the NN model tf frame - :robot_state: (pandas.DataFrame) container for rosbot state + :robot_state: (pandas.DataFrame) container for robot state :kinetic_model_state: (pandas.DataFrame) container for kinematic model state :nn_model_state: (pandas.DataFrame) container for NN model state - :robot_control: (pandas.DataFrame) container for rosbot control + :robot_control: (pandas.DataFrame) container for robot control :time: (list) container for time stamps :odom_sub: subscriber to /odom topic :control_sub: subscriber to control topic """ + def __init__(self): """ """ super().__init__('logger') - self.init_parameters() self.get_node_parametes() self.init_subs() self.init_containers() - self.first_tick = True self.init_time = None self.curr_control = list() - self.srv = self.create_service(Empty, 'shutdown_logger', self.shutdown_logger_callback) + self.curr_velocity = Twist() + self.tf_buffer = tf2_ros.Buffer(rclpy.duration.Duration(seconds=0.01)) + self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self) + self.srv = self.create_service( + Empty, 'shutdown_logger', self.shutdown_logger_callback) rclpy.get_default_context().on_shutdown(self.on_shutdown) - def init_parameters(self): """ @@ -62,20 +64,35 @@ def init_parameters(self): self.declare_parameter('output_path', "") self.declare_parameter('control_topic', '/cmd_vel') self.declare_parameter('parent_frame', 'odom') + self.declare_parameter('robot_frame', 'camera_pose_frame') self.declare_parameter('kinetic_model_frame', 'model_link') self.declare_parameter('nn_model_frame', 'nn_model_link') - # self.declare_parameter('tf_topic', '/tf') + self.declare_parameter('use_odom_topic', True) + self.declare_parameter('odom_topic', '/odom') + self.declare_parameter('velocity_topic', '/velocity') def get_node_parametes(self): """ Gets node parameters """ - self.output_path = self.get_parameter('output_path').get_parameter_value().string_value - self.control_topic = self.get_parameter('control_topic').get_parameter_value().string_value - self.parent_frame = self.get_parameter('parent_frame').get_parameter_value().string_value - self.kinetic_model_frame = self.get_parameter('kinetic_model_frame').get_parameter_value().string_value - self.nn_model_frame = self.get_parameter('nn_model_frame').get_parameter_value().string_value - # self.tf_topic = self.get_parameter('tf_topic').get_parameter_value().string_value + self.output_path = self.get_parameter( + 'output_path').get_parameter_value().string_value + self.control_topic = self.get_parameter( + 'control_topic').get_parameter_value().string_value + self.parent_frame = self.get_parameter( + 'parent_frame').get_parameter_value().string_value + self.robot_frame = self.get_parameter( + 'robot_frame').get_parameter_value().string_value + self.kinetic_model_frame = self.get_parameter( + 'kinetic_model_frame').get_parameter_value().string_value + self.nn_model_frame = self.get_parameter( + 'nn_model_frame').get_parameter_value().string_value + self.use_odom_topic = self.get_parameter( + 'use_odom_topic').get_parameter_value().bool_value + self.odom_topic = self.get_parameter( + 'odom_topic').get_parameter_value().string_value + self.velocity_topic = self.get_parameter( + 'velocity_topic').get_parameter_value().string_value def init_containers(self): """ @@ -110,63 +127,82 @@ def init_subs(self): """ Declares node subscribers """ - self.odom_sub = self.create_subscription( - Odometry, - '/odom', - self.odom_callback, - 1 - ) self.control_sub = self.create_subscription( Twist, self.control_topic, self.control_callback, 1 ) - - # prevent unused variable warning self.control_sub - self.odom_sub + + if self.use_odom_topic: + self.odom_sub = self.create_subscription( + Odometry, + self.odom_topic, + self.odom_callback, + 1 + ) + self.odom_sub + else: + qos_profile = QoSProfile( + reliability=QoSReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT, + history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST, + depth=1 + ) + self.velocity_sub = self.create_subscription( + Twist, + self.velocity_topic, + self.velocity_callback, + qos_profile=qos_profile + ) + self.velocity_sub + + def velocity_callback(self, vel_msg): + """ + Callback on velocity msg, update current velocity + :Args: + :vel_msg: (geometry_msgs.msg.Twist) velpcity msg + """ + if (len(self.curr_control) == 0): + return + try: + robot_pose_tf = self.tf_buffer.lookup_transform( + self.parent_frame, + self.robot_frame, + rclpy.time.Time() + ) + self.curr_velocity = vel_msg + curr_time = convert_ros2_time_to_float( + self.get_clock().now().seconds_nanoseconds() + ) + self.time.append(curr_time - self.init_time) + self.robot_control.loc[len(self.robot_control)] = self.curr_control + + robot_pose = transform_to_pose(robot_pose_tf) + self.upate_robot_state_container(robot_pose, self.curr_velocity) + except tf2_ros.TransformException as ex: + self.get_logger().info( + f'Could not transform {self.robot_frame} to {self.parent_frame}: {ex}') + return def odom_callback(self, odom_msg): """ Callback on odom message Robot position, current time and control are logged - Args: + :Args: :odom_msg: (nav_msgs.msg.Odometry): odom msg """ if (len(self.curr_control) == 0): - return + return curr_time = convert_ros2_time_to_float( self.get_clock().now().seconds_nanoseconds() - ) - # update time container + ) self.time.append(curr_time - self.init_time) - # update control container self.robot_control.loc[len(self.robot_control)] = self.curr_control - # update robot_state container - rosbot_pose = odom_msg.pose.pose - rosbot_velocities = odom_msg.twist.twist - x, y, z = rosbot_pose.position.x, rosbot_pose.position.y, rosbot_pose.position.z - rpy = Rotation.from_quat([ - np.float(rosbot_pose.orientation.x), - np.float(rosbot_pose.orientation.y), - np.float(rosbot_pose.orientation.z), - np.float(rosbot_pose.orientation.w)] - ).as_euler('xyz') - rpy = list(rpy) - - v_x = rosbot_velocities.linear.x # Linear velocity - v_y = rosbot_velocities.linear.y - v_z = rosbot_velocities.linear.z - - w_x = rosbot_velocities.angular.x - w_y = rosbot_velocities.angular.y - w_z = rosbot_velocities.angular.z # YAW velocity - - last_row = len(self.robot_state) - self.robot_state.loc[last_row] = [x,y,z] + rpy + [v_x, v_y, v_z, w_x, w_y, w_z] + self.upate_robot_state_container( + odom_msg.pose.pose, odom_msg.twist.twist) def control_callback(self, control): """ @@ -182,21 +218,49 @@ def control_callback(self, control): self.curr_control = [control.linear.x, control.angular.z] + def upate_robot_state_container(self, robot_pose, robot_velocities): + """ + Add new raw with data to the robot_state container + :Args: + :robot_pose: (geometry_msgs.msg.Pose) robot pose to be added + :robot_velocities: (geometry_msgs.msg.Twist) robot velocities (v and w) to be added + """ + x = robot_pose.position.x + y = robot_pose.position.y + z = robot_pose.position.z + rpy = Rotation.from_quat([ + np.float(robot_pose.orientation.x), + np.float(robot_pose.orientation.y), + np.float(robot_pose.orientation.z), + np.float(robot_pose.orientation.w)] + ).as_euler('xyz') + rpy = list(rpy) + + v_x = robot_velocities.linear.x # Linear velocity + v_y = robot_velocities.linear.y + v_z = robot_velocities.linear.z + + w_x = robot_velocities.angular.x + w_y = robot_velocities.angular.y + w_z = robot_velocities.angular.z # YAW velocity + + last_row = len(self.robot_state) + self.robot_state.loc[last_row] = [x, y, z] + \ + rpy + [v_x, v_y, v_z, w_x, w_y, w_z] + def save_collected_data_to_csv(self): """ Saves logged data in csv format """ - # if not os.path.exists(self.output_path): - # os.makedirs(self.output_path) - self.robot_state.to_csv( - path_or_buf=os.path.join(self.output_path, "rosbot_state.csv"), + path_or_buf=os.path.join(self.output_path, "robot_state.csv"), sep=' ', index=False ) self.kinetic_model_state.to_csv( - path_or_buf=os.path.join(self.output_path, "kinematic_model_state.csv"), + path_or_buf=os.path.join( + self.output_path, "kinematic_model_state.csv"), sep=' ', index=False ) @@ -208,17 +272,16 @@ def save_collected_data_to_csv(self): ) self.robot_control.to_csv( - path_or_buf= os.path.join(self.output_path,"control.csv"), + path_or_buf=os.path.join(self.output_path, "control.csv"), sep=' ', index=False ) pd.DataFrame(data=self.time, columns=['t']).to_csv( - path_or_buf= os.path.join(self.output_path, "time.csv"), + path_or_buf=os.path.join(self.output_path, "time.csv"), sep=' ', index=False ) - def shutdown_logger_callback(self): """ @@ -235,7 +298,7 @@ def on_shutdown(self): if not os.path.exists(self.output_path): os.makedirs(self.output_path) - + data_plots = build_general_graph_for_rosbot( robot_state_df=self.robot_state, control_df=self.robot_control, @@ -247,6 +310,7 @@ def on_shutdown(self): self.get_logger().warn("Output path = {}".format(self.output_path)) + def main(): """ Declares the logger node. @@ -266,6 +330,3 @@ def main(): if __name__ == '__main__': main() - - - diff --git a/ros2_ws/src/utils/logger/logger/utils.py b/ros2_ws/src/utils/logger/logger/utils.py index 6ffdabe..e33d93b 100644 --- a/ros2_ws/src/utils/logger/logger/utils.py +++ b/ros2_ws/src/utils/logger/logger/utils.py @@ -1,18 +1,20 @@ import os import math import pandas as pd +from geometry_msgs.msg import Pose TIME_CONVERSION_CONST_ = 10 ** 9 + def calculate_rosbot_velocities(x_new, y_new, rpy_new, x_prev, y_prev, rpy_prev, dt): """ Calculates the linear and angular velocity of the rosbot """ # get yaw from [roll, pitch, yaw] - - yaw_new = rpy_new[2] - yaw_prev = rpy_prev[2] - + + yaw_new = rpy_new[2] + yaw_prev = rpy_prev[2] + d_yaw = yaw_new - yaw_prev d_yaw = (d_yaw + math.pi) % (2 * math.pi) - math.pi w = d_yaw / dt @@ -21,21 +23,24 @@ def calculate_rosbot_velocities(x_new, y_new, rpy_new, x_prev, y_prev, rpy_prev, vy = (y_new - y_prev) / dt v = math.sqrt(vx**2 + vy**2) - alpha = math.atan2(vy,vx) + alpha = math.atan2(vy, vx) v = v * math.cos(alpha - yaw_new) return v, w + def calculate_next_kinematic_model_state(control_df): """ """ pass + def convert_ros2_time_to_float(time_tuple): """ Converts time from tuple (ROS2) format to floating point number """ return float(time_tuple[0] + time_tuple[1]/TIME_CONVERSION_CONST_) + def parse_csv_file(file_path): """ Args: @@ -47,7 +52,8 @@ def parse_csv_file(file_path): file_path, sep=' ', ) - + + def parse_logger_output_data(folder_path): """ :Args: @@ -58,17 +64,33 @@ def parse_logger_output_data(folder_path): data = 5 * [pd.DataFrame()] required_files = [ - 'rosbot_state.csv', - 'kinematic_model_state.csv', - 'nn_model_state.csv', - 'control.csv', + 'rosbot_state.csv', + 'kinematic_model_state.csv', + 'nn_model_state.csv', + 'control.csv', 'time.csv' ] for i, file in zip(range(5), required_files): data[i] = parse_csv_file(folder_path+'/'+file) - + return data +def transform_to_pose(trans): + """ + Convert TF transform to Pose + :Args: + :transform: + """ + result = Pose() + result.position.x = trans.transform.translation.x + result.position.y = trans.transform.translation.y + result.position.z = trans.transform.translation.z + + result.orientation.x = trans.transform.rotation.x + result.orientation.y = trans.transform.rotation.y + result.orientation.z = trans.transform.rotation.z + result.orientation.w = trans.transform.rotation.w + return result