From 2c00a21415bd7e9c96689cf36a0eddae0ddc7c79 Mon Sep 17 00:00:00 2001 From: aarizona Date: Fri, 22 Nov 2024 08:58:55 +0300 Subject: [PATCH] some refactor --- app.log | 203 ++++++++++++++++++++++++++++++++++++ robot/client_socket.py | 166 ++--------------------------- robot/prepare_data.py | 57 ++++++++++ robot/socket_manager.py | 58 +++++++++++ robot/urdf_manager.py | 66 ++++++++++++ utils/test_socket_server.py | 14 +-- 6 files changed, 399 insertions(+), 165 deletions(-) create mode 100644 robot/prepare_data.py create mode 100644 robot/socket_manager.py create mode 100644 robot/urdf_manager.py diff --git a/app.log b/app.log index 2043dd9..6c90ef8 100644 --- a/app.log +++ b/app.log @@ -561,3 +561,206 @@ 2024-11-21 19:42:19,941 - logger - INFO - not_connected - robot.py - 52 2024-11-21 19:42:20,242 - logger - INFO - trying connect to ('127.0.0.1', 9760) - client_socket.py - 86 2024-11-21 19:42:22,090 - logger - INFO - connected - robot.py - 52 +2024-11-22 08:37:25,186 - logger - INFO - robot_app еще не инициализирован. Метод: get_status - main.py - 105 +2024-11-22 08:37:25,187 - logger - INFO - None - robot.py - 52 +2024-11-22 08:37:25,187 - logger - INFO - robot_app еще не инициализирован. Метод: get_status - main.py - 105 +2024-11-22 08:37:25,187 - logger - INFO - robot_app еще не инициализирован. Метод: get_status - main.py - 105 +2024-11-22 08:37:25,187 - logger - INFO - robot_app еще не инициализирован. Метод: get_status - main.py - 105 +2024-11-22 08:37:25,187 - logger - INFO - robot_app еще не инициализирован. Метод: get_world_coordinates - main.py - 105 +2024-11-22 08:37:25,187 - logger - INFO - robot_app еще не инициализирован. Метод: get_axis_coordinates - main.py - 105 +2024-11-22 08:37:25,187 - logger - INFO - robot_app еще не инициализирован. Метод: get_command_count - main.py - 105 +2024-11-22 08:37:26,220 - logger - INFO - Connect to 0 by SHARED_MEMORY - urdf_manager.py - 33 +2024-11-22 08:37:29,220 - logger - INFO - not_connected - robot.py - 52 +2024-11-22 08:37:29,521 - logger - INFO - trying connect to ('127.0.0.1', 9760) - socket_manager.py - 28 +2024-11-22 08:37:50,031 - logger - INFO - robot_app еще не инициализирован. Метод: get_status - main.py - 105 +2024-11-22 08:37:50,031 - logger - INFO - None - robot.py - 52 +2024-11-22 08:37:50,031 - logger - INFO - robot_app еще не инициализирован. Метод: get_status - main.py - 105 +2024-11-22 08:37:50,031 - logger - INFO - robot_app еще не инициализирован. Метод: get_status - main.py - 105 +2024-11-22 08:37:50,031 - logger - INFO - robot_app еще не инициализирован. Метод: get_status - main.py - 105 +2024-11-22 08:37:50,031 - logger - INFO - robot_app еще не инициализирован. Метод: get_world_coordinates - main.py - 105 +2024-11-22 08:37:50,031 - logger - INFO - robot_app еще не инициализирован. Метод: get_axis_coordinates - main.py - 105 +2024-11-22 08:37:50,031 - logger - INFO - robot_app еще не инициализирован. Метод: get_command_count - main.py - 105 +2024-11-22 08:37:51,064 - logger - INFO - Connect to 0 by SHARED_MEMORY - urdf_manager.py - 34 +2024-11-22 08:37:53,562 - logger - INFO - not_connected - robot.py - 52 +2024-11-22 08:37:53,862 - logger - INFO - trying connect to ('127.0.0.1', 9760) - socket_manager.py - 28 +2024-11-22 08:37:55,687 - logger - INFO - connected - robot.py - 52 +2024-11-22 08:37:56,686 - logger - INFO - Координаты осей [-39.428261, 10.391139, -32.562145, 176.81929, 38.895988, -159.779755] - client_socket.py - 172 +2024-11-22 08:37:56,688 - logger - INFO - Мировые координаты [894.159973, -738.295654, 782.748047, 172.274109, 28.079031, 120.377357] - client_socket.py - 172 +2024-11-22 08:37:57,823 - logger - INFO - Команд в очереди ['ok'] - client_socket.py - 172 +2024-11-22 08:37:59,239 - logger - INFO - Координаты осей [-17.409513, 10.897068, -33.003525, 176.892288, 38.957874, -159.934479] - client_socket.py - 172 +2024-11-22 08:37:59,241 - logger - INFO - Мировые координаты [1100.071655, -347.347412, 782.754578, 172.284241, 28.079557, 142.505508] - client_socket.py - 172 +2024-11-22 08:38:00,384 - logger - INFO - Команд в очереди ['ok'] - client_socket.py - 172 +2024-11-22 08:38:00,951 - logger - INFO - Координаты осей [-39.428261, 10.391139, -32.562145, 176.81929, 38.895988, -159.779755] - client_socket.py - 172 +2024-11-22 08:38:00,953 - logger - INFO - Мировые координаты [894.159973, -738.295654, 782.748047, 172.274109, 28.079031, 120.377357] - client_socket.py - 172 +2024-11-22 08:38:02,086 - logger - INFO - Команд в очереди ['ok'] - client_socket.py - 172 +2024-11-22 08:38:03,074 - logger - INFO - Координаты осей [-17.409513, 10.897068, -33.003525, 176.892288, 38.957874, -159.934479] - client_socket.py - 172 +2024-11-22 08:38:03,076 - logger - INFO - Мировые координаты [1100.071655, -347.347412, 782.754578, 172.284241, 28.079557, 142.505508] - client_socket.py - 172 +2024-11-22 08:38:04,211 - logger - INFO - Команд в очереди ['ok'] - client_socket.py - 172 +2024-11-22 08:38:13,406 - logger - ERROR - Error sending data: Expecting value: line 1 column 1 (char 0) - socket_manager.py - 57 +2024-11-22 08:38:13,406 - logger - INFO - Старт одиночного цикла - client_socket.py - 172 +2024-11-22 08:38:13,908 - logger - ERROR - Error sending data: Expecting value: line 1 column 1 (char 0) - socket_manager.py - 57 +2024-11-22 08:38:13,958 - logger - ERROR - Error sending data: [Errno 32] Обрыв канала - socket_manager.py - 57 +2024-11-22 08:38:14,009 - logger - ERROR - Error sending data: [Errno 32] Обрыв канала - socket_manager.py - 57 +2024-11-22 08:38:16,480 - logger - ERROR - Error sending data: [Errno 32] Обрыв канала - socket_manager.py - 57 +2024-11-22 08:38:16,481 - logger - INFO - Старт одиночного цикла - client_socket.py - 172 +2024-11-22 08:38:16,983 - logger - ERROR - Error sending data: [Errno 32] Обрыв канала - socket_manager.py - 57 +2024-11-22 08:38:17,034 - logger - ERROR - Error sending data: [Errno 32] Обрыв канала - socket_manager.py - 57 +2024-11-22 08:38:17,084 - logger - ERROR - Error sending data: [Errno 32] Обрыв канала - socket_manager.py - 57 +2024-11-22 08:40:40,926 - logger - INFO - robot_app еще не инициализирован. Метод: get_status - main.py - 105 +2024-11-22 08:40:40,926 - logger - INFO - None - robot.py - 52 +2024-11-22 08:40:40,926 - logger - INFO - robot_app еще не инициализирован. Метод: get_status - main.py - 105 +2024-11-22 08:40:40,926 - logger - INFO - robot_app еще не инициализирован. Метод: get_status - main.py - 105 +2024-11-22 08:40:40,926 - logger - INFO - robot_app еще не инициализирован. Метод: get_status - main.py - 105 +2024-11-22 08:40:40,927 - logger - INFO - robot_app еще не инициализирован. Метод: get_world_coordinates - main.py - 105 +2024-11-22 08:40:40,927 - logger - INFO - robot_app еще не инициализирован. Метод: get_axis_coordinates - main.py - 105 +2024-11-22 08:40:40,927 - logger - INFO - robot_app еще не инициализирован. Метод: get_command_count - main.py - 105 +2024-11-22 08:40:41,974 - logger - INFO - Connect to 0 by SHARED_MEMORY - urdf_manager.py - 34 +2024-11-22 08:40:44,391 - logger - INFO - not_connected - robot.py - 52 +2024-11-22 08:40:44,692 - logger - INFO - trying connect to ('127.0.0.1', 9760) - socket_manager.py - 28 +2024-11-22 08:40:46,493 - logger - INFO - connected - robot.py - 52 +2024-11-22 08:40:48,717 - logger - INFO - Координаты осей [-39.428261, 10.391139, -32.562145, 176.81929, 38.895988, -159.779755] - client_socket.py - 172 +2024-11-22 08:40:48,719 - logger - INFO - Мировые координаты [894.159973, -738.295654, 782.748047, 172.274109, 28.079031, 120.377357] - client_socket.py - 172 +2024-11-22 08:40:49,791 - logger - INFO - Команд в очереди ['ok'] - client_socket.py - 172 +2024-11-22 08:40:55,206 - logger - INFO - Старт одиночного цикла - client_socket.py - 172 +2024-11-22 08:40:55,759 - logger - ERROR - Error sending data: [Errno 104] Соединение разорвано другой стороной - socket_manager.py - 57 +2024-11-22 08:40:55,810 - logger - ERROR - Error sending data: [Errno 32] Обрыв канала - socket_manager.py - 57 +2024-11-22 08:42:23,648 - logger - INFO - robot_app еще не инициализирован. Метод: get_status - main.py - 105 +2024-11-22 08:42:23,648 - logger - INFO - None - robot.py - 52 +2024-11-22 08:42:23,648 - logger - INFO - robot_app еще не инициализирован. Метод: get_status - main.py - 105 +2024-11-22 08:42:23,649 - logger - INFO - robot_app еще не инициализирован. Метод: get_status - main.py - 105 +2024-11-22 08:42:23,649 - logger - INFO - robot_app еще не инициализирован. Метод: get_status - main.py - 105 +2024-11-22 08:42:23,649 - logger - INFO - robot_app еще не инициализирован. Метод: get_world_coordinates - main.py - 105 +2024-11-22 08:42:23,649 - logger - INFO - robot_app еще не инициализирован. Метод: get_axis_coordinates - main.py - 105 +2024-11-22 08:42:23,649 - logger - INFO - robot_app еще не инициализирован. Метод: get_command_count - main.py - 105 +2024-11-22 08:42:24,682 - logger - INFO - Connect to 0 by SHARED_MEMORY - urdf_manager.py - 34 +2024-11-22 08:42:27,179 - logger - INFO - not_connected - robot.py - 52 +2024-11-22 08:42:27,480 - logger - INFO - trying connect to ('127.0.0.1', 9760) - socket_manager.py - 28 +2024-11-22 08:42:29,285 - logger - INFO - connected - robot.py - 52 +2024-11-22 08:42:30,995 - logger - INFO - Координаты осей [-39.428261, 10.391139, -32.562145, 176.81929, 38.895988, -159.779755] - client_socket.py - 172 +2024-11-22 08:42:30,997 - logger - INFO - Мировые координаты [894.159973, -738.295654, 782.748047, 172.274109, 28.079031, 120.377357] - client_socket.py - 172 +2024-11-22 08:42:32,123 - logger - INFO - Команд в очереди ['ok'] - client_socket.py - 172 +2024-11-22 08:42:33,542 - logger - INFO - Координаты осей [-17.409513, 10.897068, -33.003525, 176.892288, 38.957874, -159.934479] - client_socket.py - 172 +2024-11-22 08:42:33,544 - logger - INFO - Мировые координаты [1100.071655, -347.347412, 782.754578, 172.284241, 28.079557, 142.505508] - client_socket.py - 172 +2024-11-22 08:42:34,682 - logger - INFO - Команд в очереди ['ok'] - client_socket.py - 172 +2024-11-22 08:42:39,189 - logger - INFO - Координаты осей [-39.428261, 10.391139, -32.562145, 176.81929, 38.895988, -159.779755] - client_socket.py - 172 +2024-11-22 08:42:39,190 - logger - INFO - Мировые координаты [894.159973, -738.295654, 782.748047, 172.274109, 28.079031, 120.377357] - client_socket.py - 172 +2024-11-22 08:42:40,313 - logger - INFO - Команд в очереди ['ok'] - client_socket.py - 172 +2024-11-22 08:42:40,875 - logger - INFO - Координаты осей [-17.409513, 10.897068, -33.003525, 176.892288, 38.957874, -159.934479] - client_socket.py - 172 +2024-11-22 08:42:40,876 - logger - INFO - Мировые координаты [1100.071655, -347.347412, 782.754578, 172.284241, 28.079557, 142.505508] - client_socket.py - 172 +2024-11-22 08:42:41,878 - logger - INFO - Команд в очереди ['ok'] - client_socket.py - 172 +2024-11-22 08:42:44,987 - logger - INFO - Координаты осей [-39.428261, 10.391139, -32.562145, 176.81929, 38.895988, -159.779755] - client_socket.py - 172 +2024-11-22 08:42:44,988 - logger - INFO - Мировые координаты [894.159973, -738.295654, 782.748047, 172.274109, 28.079031, 120.377357] - client_socket.py - 172 +2024-11-22 08:42:46,118 - logger - INFO - Команд в очереди ['ok'] - client_socket.py - 172 +2024-11-22 08:42:46,820 - logger - INFO - Координаты осей [-17.409513, 10.897068, -33.003525, 176.892288, 38.957874, -159.934479] - client_socket.py - 172 +2024-11-22 08:42:46,822 - logger - INFO - Мировые координаты [1100.071655, -347.347412, 782.754578, 172.284241, 28.079557, 142.505508] - client_socket.py - 172 +2024-11-22 08:42:47,829 - logger - INFO - Команд в очереди ['ok'] - client_socket.py - 172 +2024-11-22 08:42:48,682 - logger - INFO - Старт одиночного цикла - client_socket.py - 172 +2024-11-22 08:42:49,235 - logger - ERROR - Error sending data: [Errno 104] Соединение разорвано другой стороной - socket_manager.py - 57 +2024-11-22 08:42:49,286 - logger - ERROR - Error sending data: [Errno 32] Обрыв канала - socket_manager.py - 57 +2024-11-22 08:44:05,485 - logger - INFO - robot_app еще не инициализирован. Метод: get_status - main.py - 105 +2024-11-22 08:44:05,485 - logger - INFO - None - robot.py - 52 +2024-11-22 08:44:05,485 - logger - INFO - robot_app еще не инициализирован. Метод: get_status - main.py - 105 +2024-11-22 08:44:05,485 - logger - INFO - robot_app еще не инициализирован. Метод: get_status - main.py - 105 +2024-11-22 08:44:05,485 - logger - INFO - robot_app еще не инициализирован. Метод: get_status - main.py - 105 +2024-11-22 08:44:05,486 - logger - INFO - robot_app еще не инициализирован. Метод: get_world_coordinates - main.py - 105 +2024-11-22 08:44:05,486 - logger - INFO - robot_app еще не инициализирован. Метод: get_axis_coordinates - main.py - 105 +2024-11-22 08:44:05,486 - logger - INFO - robot_app еще не инициализирован. Метод: get_command_count - main.py - 105 +2024-11-22 08:44:06,538 - logger - INFO - Connect to 0 by SHARED_MEMORY - urdf_manager.py - 34 +2024-11-22 08:44:15,665 - logger - INFO - robot_app еще не инициализирован. Метод: get_status - main.py - 105 +2024-11-22 08:44:15,665 - logger - INFO - None - robot.py - 52 +2024-11-22 08:44:15,665 - logger - INFO - robot_app еще не инициализирован. Метод: get_status - main.py - 105 +2024-11-22 08:44:15,665 - logger - INFO - robot_app еще не инициализирован. Метод: get_status - main.py - 105 +2024-11-22 08:44:15,665 - logger - INFO - robot_app еще не инициализирован. Метод: get_status - main.py - 105 +2024-11-22 08:44:15,665 - logger - INFO - robot_app еще не инициализирован. Метод: get_world_coordinates - main.py - 105 +2024-11-22 08:44:15,665 - logger - INFO - robot_app еще не инициализирован. Метод: get_axis_coordinates - main.py - 105 +2024-11-22 08:44:15,665 - logger - INFO - robot_app еще не инициализирован. Метод: get_command_count - main.py - 105 +2024-11-22 08:44:16,702 - logger - INFO - Connect to 0 by SHARED_MEMORY - urdf_manager.py - 34 +2024-11-22 08:44:18,192 - logger - INFO - not_connected - robot.py - 52 +2024-11-22 08:44:18,493 - logger - INFO - trying connect to ('127.0.0.1', 9760) - socket_manager.py - 28 +2024-11-22 08:44:20,289 - logger - INFO - connected - robot.py - 52 +2024-11-22 08:44:21,577 - logger - INFO - Координаты осей [-39.428261, 10.391139, -32.562145, 176.81929, 38.895988, -159.779755] - client_socket.py - 172 +2024-11-22 08:44:21,578 - logger - INFO - Мировые координаты [894.159973, -738.295654, 782.748047, 172.274109, 28.079031, 120.377357] - client_socket.py - 172 +2024-11-22 08:44:22,703 - logger - INFO - Команд в очереди ['ok'] - client_socket.py - 172 +2024-11-22 08:44:23,410 - logger - INFO - Координаты осей [-17.409513, 10.897068, -33.003525, 176.892288, 38.957874, -159.934479] - client_socket.py - 172 +2024-11-22 08:44:23,411 - logger - INFO - Мировые координаты [1100.071655, -347.347412, 782.754578, 172.284241, 28.079557, 142.505508] - client_socket.py - 172 +2024-11-22 08:44:24,555 - logger - INFO - Команд в очереди ['ok'] - client_socket.py - 172 +2024-11-22 08:44:34,742 - logger - INFO - Старт одиночного цикла - client_socket.py - 172 +2024-11-22 08:44:35,294 - logger - ERROR - Error sending data: [Errno 104] Соединение разорвано другой стороной - socket_manager.py - 57 +2024-11-22 08:44:35,345 - logger - ERROR - Error sending data: [Errno 32] Обрыв канала - socket_manager.py - 57 +2024-11-22 08:46:07,619 - logger - INFO - robot_app еще не инициализирован. Метод: get_status - main.py - 105 +2024-11-22 08:46:07,619 - logger - INFO - None - robot.py - 52 +2024-11-22 08:46:07,619 - logger - INFO - robot_app еще не инициализирован. Метод: get_status - main.py - 105 +2024-11-22 08:46:07,619 - logger - INFO - robot_app еще не инициализирован. Метод: get_status - main.py - 105 +2024-11-22 08:46:07,620 - logger - INFO - robot_app еще не инициализирован. Метод: get_status - main.py - 105 +2024-11-22 08:46:07,620 - logger - INFO - robot_app еще не инициализирован. Метод: get_world_coordinates - main.py - 105 +2024-11-22 08:46:07,620 - logger - INFO - robot_app еще не инициализирован. Метод: get_axis_coordinates - main.py - 105 +2024-11-22 08:46:07,620 - logger - INFO - robot_app еще не инициализирован. Метод: get_command_count - main.py - 105 +2024-11-22 08:46:08,670 - logger - INFO - Connect to 0 by SHARED_MEMORY - urdf_manager.py - 34 +2024-11-22 08:46:14,075 - logger - INFO - robot_app еще не инициализирован. Метод: get_status - main.py - 105 +2024-11-22 08:46:14,076 - logger - INFO - None - robot.py - 52 +2024-11-22 08:46:14,076 - logger - INFO - robot_app еще не инициализирован. Метод: get_status - main.py - 105 +2024-11-22 08:46:14,076 - logger - INFO - robot_app еще не инициализирован. Метод: get_status - main.py - 105 +2024-11-22 08:46:14,076 - logger - INFO - robot_app еще не инициализирован. Метод: get_status - main.py - 105 +2024-11-22 08:46:14,076 - logger - INFO - robot_app еще не инициализирован. Метод: get_world_coordinates - main.py - 105 +2024-11-22 08:46:14,076 - logger - INFO - robot_app еще не инициализирован. Метод: get_axis_coordinates - main.py - 105 +2024-11-22 08:46:14,076 - logger - INFO - robot_app еще не инициализирован. Метод: get_command_count - main.py - 105 +2024-11-22 08:46:15,134 - logger - INFO - Connect to 0 by SHARED_MEMORY - urdf_manager.py - 34 +2024-11-22 08:46:16,731 - logger - INFO - not_connected - robot.py - 52 +2024-11-22 08:46:17,032 - logger - INFO - trying connect to ('127.0.0.1', 9760) - socket_manager.py - 28 +2024-11-22 08:46:18,834 - logger - INFO - connected - robot.py - 52 +2024-11-22 08:46:19,836 - logger - INFO - Координаты осей [-39.428261, 10.391139, -32.562145, 176.81929, 38.895988, -159.779755] - client_socket.py - 172 +2024-11-22 08:46:19,837 - logger - INFO - Мировые координаты [894.159973, -738.295654, 782.748047, 172.274109, 28.079031, 120.377357] - client_socket.py - 172 +2024-11-22 08:46:20,964 - logger - INFO - Команд в очереди ['ok'] - client_socket.py - 172 +2024-11-22 08:46:23,078 - logger - INFO - Координаты осей [-17.409513, 10.897068, -33.003525, 176.892288, 38.957874, -159.934479] - client_socket.py - 172 +2024-11-22 08:46:23,079 - logger - INFO - Мировые координаты [1100.071655, -347.347412, 782.754578, 172.284241, 28.079557, 142.505508] - client_socket.py - 172 +2024-11-22 08:46:24,209 - logger - INFO - Команд в очереди ['ok'] - client_socket.py - 172 +2024-11-22 08:46:31,838 - logger - INFO - Старт одиночного цикла - client_socket.py - 172 +2024-11-22 08:46:32,391 - logger - ERROR - Error sending data: [Errno 104] Соединение разорвано другой стороной - socket_manager.py - 57 +2024-11-22 08:46:32,441 - logger - ERROR - Error sending data: [Errno 32] Обрыв канала - socket_manager.py - 57 +2024-11-22 08:46:48,310 - logger - ERROR - Error sending data: [Errno 32] Обрыв канала - socket_manager.py - 57 +2024-11-22 08:46:48,310 - logger - INFO - Старт одиночного цикла - client_socket.py - 172 +2024-11-22 08:46:48,813 - logger - ERROR - Error sending data: [Errno 32] Обрыв канала - socket_manager.py - 57 +2024-11-22 08:46:48,864 - logger - ERROR - Error sending data: [Errno 32] Обрыв канала - socket_manager.py - 57 +2024-11-22 08:46:48,914 - logger - ERROR - Error sending data: [Errno 32] Обрыв канала - socket_manager.py - 57 +2024-11-22 08:47:33,298 - logger - INFO - robot_app еще не инициализирован. Метод: get_status - main.py - 105 +2024-11-22 08:47:33,298 - logger - INFO - None - robot.py - 52 +2024-11-22 08:47:33,298 - logger - INFO - robot_app еще не инициализирован. Метод: get_status - main.py - 105 +2024-11-22 08:47:33,298 - logger - INFO - robot_app еще не инициализирован. Метод: get_status - main.py - 105 +2024-11-22 08:47:33,298 - logger - INFO - robot_app еще не инициализирован. Метод: get_status - main.py - 105 +2024-11-22 08:47:33,299 - logger - INFO - robot_app еще не инициализирован. Метод: get_world_coordinates - main.py - 105 +2024-11-22 08:47:33,299 - logger - INFO - robot_app еще не инициализирован. Метод: get_axis_coordinates - main.py - 105 +2024-11-22 08:47:33,299 - logger - INFO - robot_app еще не инициализирован. Метод: get_command_count - main.py - 105 +2024-11-22 08:47:34,331 - logger - INFO - Connect to 0 by SHARED_MEMORY - urdf_manager.py - 34 +2024-11-22 08:47:35,419 - logger - INFO - not_connected - robot.py - 52 +2024-11-22 08:47:35,719 - logger - INFO - trying connect to ('127.0.0.1', 9760) - socket_manager.py - 28 +2024-11-22 08:47:37,528 - logger - INFO - connected - robot.py - 52 +2024-11-22 08:48:50,770 - logger - INFO - Координаты осей [-39.428261, 10.391139, -32.562145, 176.81929, 38.895988, -159.779755] - client_socket.py - 172 +2024-11-22 08:48:50,771 - logger - INFO - Мировые координаты [894.159973, -738.295654, 782.748047, 172.274109, 28.079031, 120.377357] - client_socket.py - 172 +2024-11-22 08:48:51,924 - logger - INFO - Команд в очереди ['ok'] - client_socket.py - 172 +2024-11-22 08:48:53,623 - logger - INFO - Старт одиночного цикла - client_socket.py - 172 +2024-11-22 08:48:54,175 - logger - ERROR - Error sending data: [Errno 104] Соединение разорвано другой стороной - socket_manager.py - 57 +2024-11-22 08:48:54,226 - logger - ERROR - Error sending data: [Errno 32] Обрыв канала - socket_manager.py - 57 +2024-11-22 08:50:20,313 - logger - INFO - robot_app еще не инициализирован. Метод: get_status - main.py - 105 +2024-11-22 08:50:20,313 - logger - INFO - None - robot.py - 52 +2024-11-22 08:50:20,313 - logger - INFO - robot_app еще не инициализирован. Метод: get_status - main.py - 105 +2024-11-22 08:50:20,313 - logger - INFO - robot_app еще не инициализирован. Метод: get_status - main.py - 105 +2024-11-22 08:50:20,313 - logger - INFO - robot_app еще не инициализирован. Метод: get_status - main.py - 105 +2024-11-22 08:50:20,313 - logger - INFO - robot_app еще не инициализирован. Метод: get_world_coordinates - main.py - 105 +2024-11-22 08:50:20,314 - logger - INFO - robot_app еще не инициализирован. Метод: get_axis_coordinates - main.py - 105 +2024-11-22 08:50:20,314 - logger - INFO - robot_app еще не инициализирован. Метод: get_command_count - main.py - 105 +2024-11-22 08:50:21,346 - logger - INFO - Connect to 0 by SHARED_MEMORY - urdf_manager.py - 34 +2024-11-22 08:50:24,418 - logger - INFO - not_connected - robot.py - 52 +2024-11-22 08:50:24,719 - logger - INFO - trying connect to ('127.0.0.1', 9760) - socket_manager.py - 28 +2024-11-22 08:50:26,545 - logger - INFO - connected - robot.py - 52 +2024-11-22 08:50:27,871 - logger - INFO - Координаты осей [-39.428261, 10.391139, -32.562145, 176.81929, 38.895988, -159.779755] - client_socket.py - 172 +2024-11-22 08:50:27,873 - logger - INFO - Мировые координаты [894.159973, -738.295654, 782.748047, 172.274109, 28.079031, 120.377357] - client_socket.py - 172 +2024-11-22 08:50:28,927 - logger - INFO - Команд в очереди ['ok'] - client_socket.py - 172 +2024-11-22 08:50:32,992 - logger - INFO - Координаты осей [-17.409513, 10.897068, -33.003525, 176.892288, 38.957874, -159.934479] - client_socket.py - 172 +2024-11-22 08:50:32,994 - logger - INFO - Мировые координаты [1100.071655, -347.347412, 782.754578, 172.284241, 28.079557, 142.505508] - client_socket.py - 172 +2024-11-22 08:50:34,135 - logger - INFO - Команд в очереди ['ok'] - client_socket.py - 172 +2024-11-22 08:50:35,701 - logger - INFO - Старт одиночного цикла - client_socket.py - 172 +2024-11-22 08:50:38,614 - logger - INFO - Старт одиночного цикла - client_socket.py - 172 +2024-11-22 08:50:43,082 - logger - INFO - Старт одиночного цикла - client_socket.py - 172 +2024-11-22 08:50:45,854 - logger - INFO - Координаты осей [-39.428261, 10.391139, -32.562145, 176.81929, 38.895988, -159.779755] - client_socket.py - 172 +2024-11-22 08:50:45,855 - logger - INFO - Мировые координаты [894.159973, -738.295654, 782.748047, 172.274109, 28.079031, 120.377357] - client_socket.py - 172 +2024-11-22 08:50:46,981 - logger - INFO - Команд в очереди ['ok'] - client_socket.py - 172 diff --git a/robot/client_socket.py b/robot/client_socket.py index cdc4a4d..328224f 100644 --- a/robot/client_socket.py +++ b/robot/client_socket.py @@ -3,7 +3,6 @@ import json import os import math import numpy as np -import sys from typing import Literal from pprint import pprint @@ -12,162 +11,14 @@ import threading import pybullet as p import pybullet_industrial as pi -from robot.func import * from logger import logger +from robot.func import * +from robot.prepare_data import PrepareRobotData +from robot.urdf_manager import UrdfManager +from robot.socket_manager import SocketManager # os.environ["LIBGL_ALWAYS_SOFTWARE"] = "1" - -class PrepareRobotData: - line_speed = 100.0 - line_smooth = 0 - line_tool = 0 - - def make_step(self, type, point, start_coordinates): - step = { - "oneshot": "1", - "delay": "0.0", - "speed": str(self.line_speed), - "smooth": str(self.line_smooth), - "coord": "0", - "tool": str(self.line_tool), - "ckStatus": "0x3F", - } - if type == "line" or type == "free": - pairs = zip(start_coordinates, point) - data = [] - for pair in pairs: - data.append(round(sum(pair), 3)) - - m0, m1, m2, m3, m4, m5 = data - if type == "line": - step.update({"action": "10"}) - if type == "free": - step.update({"action": "4"}) - step.update({"m0": m0, "m1": m1, "m2": m2, "m3": m3, "m4": m4, "m5": m5}) - step.update({"m6": 0, "m7": 0}) - elif type == "curve": - pairs = zip(self.world_coordinates, point[:5]) - m0, m1, m2, m3, m4, m5 = [round(sum(i), 3) for i in pairs] - - pairs_p = zip(self.world_coordinates, point[6:]) - m0_p, m1_p, m2_p, m3_p, m4_p, m5_p = [round(sum(i), 3) for i in pairs_p] - step.update({"action": "17"}) - step.update({"m0": m0, "m1": m1, "m2": m2, "m3": m3, "m4": m4, "m5": m5}) - step.update({"m6": 0, "m7": 0}) - step.update( - { - "m0_p": m0_p, - "m1_p": m1_p, - "m2_p": m2_p, - "m3_p": m3_p, - "m4_p": m4_p, - "m5_p": m5_p, - } - ) - step.update({"m6_p": 0, "m7_p": 0}) - - for s in step: - step[s] = str(step[s]) - return step - - -class SocketManager: - def __init__(self): - self.host = None - self.port = 9760 - self.socket = None - self.status: Literal["connected", "not_connected", "error"] = "not_connected" - - def connect(self, host): - self.host = host - if self.socket is None: - self.socket = socket.socket() - logger.info(f"trying connect to {(self.host, self.port)}") - self.socket.connect((self.host, self.port)) - self.status = "connected" - - def close(self): - if self.socket: - self.socket.close() - self.socket = None - self.status = "not_connected" - - def send_data(self, data): - if not self.socket: - return - try: - self.socket.send(str.encode(json.dumps(data))) - response_data = self.socket.recv(1024) - response = json.loads(response_data) - - if data["reqType"] == "query": - return response["queryData"] - elif data["reqType"] == "command": - return response["cmdReply"] - elif data["reqType"] == "AddRCC" and "cmdReply" in response.keys(): - return response["cmdReply"] - else: - pprint(response_data) - - return json.loads(response_data) - except Exception as e: - logger.error(f"Error sending data: {e}") - return None - - -class UrdfManager: - urdf_filename = None - physics_client = None - body_id = None - - def __init__(self, robot_start_position, application_path): - self.robot_start_position = robot_start_position - self.application_path = application_path - - def start_loop(self, urdf): - self.urdf_filename = urdf - - p.resetSimulation() - self.load_models() - - def run_pybullet(self, type="DIRECT"): - self.physics_client = p.connect(getattr(p, type)) - logger.info(f"Connect to {self.physics_client} by {type}") - - def load_models(self): - # p.loadURDF("urdf/plane.urdf", physicsClientId=self.physics_client) - - urdf_path = os.path.join( - self.application_path, "urdf", f"{self.urdf_filename}.urdf" - ) - self.body_id = p.loadURDF(urdf_path, self.robot_start_position) - - time.sleep(1) - - def get_pybullet_image(self): - if self.physics_client is None: - return - - width, height, rgb, _, _ = p.getCameraImage( - width=500, - height=500, - viewMatrix=p.computeViewMatrix( - cameraEyePosition=[4, -4, 1.5], - # cameraEyePosition=[4,-4,4], - cameraTargetPosition=[0, 0, 0], # Центр фокусировки камеры - cameraUpVector=[0, 0, 1], # Направление вверх - ), - projectionMatrix=p.computeProjectionMatrixFOV( - fov=60.0, aspect=1.0, nearVal=0.1, farVal=10.0 - ), - # renderer=p.ER_TINY_RENDERER, - physicsClientId=self.physics_client, - ) - - return (rgb, width, height) - - class SocketRobotArm: global_speed = 10 physical_speed = 10 @@ -178,7 +29,7 @@ class SocketRobotArm: urdf_filename = "sample" pass_size = 4 - Status = Literal["connected", "not_connected", "error"] + connected_status = Literal["connected", "not_connected", "error"] axis_coordinates = [] world_coordinates = [] @@ -192,7 +43,7 @@ class SocketRobotArm: self.host = None self.port = 9760 self.slave_id = None - self.status: SocketRobotArm.Status = "not_connected" + self.status: SocketRobotArm.connected_status = "not_connected" self.q_app = None self.imitate_point = None @@ -238,9 +89,7 @@ class SocketRobotArm: def upd_model_func(self): # logger.info("UPDATE MODEL FUNC")\ - mode = "by_joint" - # mode = "by_world" - + self.get_axis() self.set_text(text=f"Координаты осей {self.axis_coordinates}") @@ -480,7 +329,6 @@ class SocketRobotArm: ) ) points = np.degrees(pj) - start_position = [0, 60, -100, 0, 40, 0] start_position = [0, 0, 0, 0, 0, 0] result.append( self.prepare_data.make_step("free", points, start_position) diff --git a/robot/prepare_data.py b/robot/prepare_data.py new file mode 100644 index 0000000..99346cb --- /dev/null +++ b/robot/prepare_data.py @@ -0,0 +1,57 @@ +from robot.func import * + +# os.environ["LIBGL_ALWAYS_SOFTWARE"] = "1" + + +class PrepareRobotData: + line_speed = 100.0 + line_smooth = 0 + line_tool = 0 + + def make_step(self, type, point, start_coordinates): + step = { + "oneshot": "1", + "delay": "0.0", + "speed": str(self.line_speed), + "smooth": str(self.line_smooth), + "coord": "0", + "tool": str(self.line_tool), + "ckStatus": "0x3F", + } + if type == "line" or type == "free": + pairs = zip(start_coordinates, point) + data = [] + for pair in pairs: + data.append(round(sum(pair), 3)) + + m0, m1, m2, m3, m4, m5 = data + if type == "line": + step.update({"action": "10"}) + if type == "free": + step.update({"action": "4"}) + step.update({"m0": m0, "m1": m1, "m2": m2, "m3": m3, "m4": m4, "m5": m5}) + step.update({"m6": 0, "m7": 0}) + elif type == "curve": + pairs = zip(self.world_coordinates, point[:5]) + m0, m1, m2, m3, m4, m5 = [round(sum(i), 3) for i in pairs] + + pairs_p = zip(self.world_coordinates, point[6:]) + m0_p, m1_p, m2_p, m3_p, m4_p, m5_p = [round(sum(i), 3) for i in pairs_p] + step.update({"action": "17"}) + step.update({"m0": m0, "m1": m1, "m2": m2, "m3": m3, "m4": m4, "m5": m5}) + step.update({"m6": 0, "m7": 0}) + step.update( + { + "m0_p": m0_p, + "m1_p": m1_p, + "m2_p": m2_p, + "m3_p": m3_p, + "m4_p": m4_p, + "m5_p": m5_p, + } + ) + step.update({"m6_p": 0, "m7_p": 0}) + + for s in step: + step[s] = str(step[s]) + return step \ No newline at end of file diff --git a/robot/socket_manager.py b/robot/socket_manager.py new file mode 100644 index 0000000..3e529d6 --- /dev/null +++ b/robot/socket_manager.py @@ -0,0 +1,58 @@ +import socket +import json +import os +import math +import numpy as np + +from typing import Literal +from pprint import pprint + +import threading +import pybullet as p +import pybullet_industrial as pi + +from logger import logger + + +class SocketManager: + def __init__(self): + self.host = None + self.port = 9760 + self.socket = None + self.status: Literal["connected", "not_connected", "error"] = "not_connected" + + def connect(self, host): + self.host = host + if self.socket is None: + self.socket = socket.socket() + logger.info(f"trying connect to {(self.host, self.port)}") + self.socket.connect((self.host, self.port)) + self.status = "connected" + + def close(self): + if self.socket: + self.socket.close() + self.socket = None + self.status = "not_connected" + + def send_data(self, data): + if not self.socket: + return + try: + self.socket.send(str.encode(json.dumps(data))) + response_data = self.socket.recv(1024) + response = json.loads(response_data) + + if data["reqType"] == "query": + return response["queryData"] + elif data["reqType"] == "command": + return response["cmdReply"] + elif data["reqType"] == "AddRCC" and "cmdReply" in response.keys(): + return response["cmdReply"] + else: + pprint(response_data) + + return json.loads(response_data) + except Exception as e: + logger.error(f"Error sending data: {e}") + return None diff --git a/robot/urdf_manager.py b/robot/urdf_manager.py new file mode 100644 index 0000000..a4bdce2 --- /dev/null +++ b/robot/urdf_manager.py @@ -0,0 +1,66 @@ +import socket +import json +import os +import math +import numpy as np +import time + +from typing import Literal +from pprint import pprint + +import threading +import pybullet as p +import pybullet_industrial as pi + +from logger import logger + +class UrdfManager: + urdf_filename = None + physics_client = None + body_id = None + + def __init__(self, robot_start_position, application_path): + self.robot_start_position = robot_start_position + self.application_path = application_path + + def start_loop(self, urdf): + self.urdf_filename = urdf + + p.resetSimulation() + self.load_models() + + def run_pybullet(self, type="DIRECT"): + self.physics_client = p.connect(getattr(p, type)) + logger.info(f"Connect to {self.physics_client} by {type}") + + def load_models(self): + # p.loadURDF("urdf/plane.urdf", physicsClientId=self.physics_client) + + urdf_path = os.path.join( + self.application_path, "urdf", f"{self.urdf_filename}.urdf" + ) + self.body_id = p.loadURDF(urdf_path, self.robot_start_position) + + time.sleep(1) + + def get_pybullet_image(self): + if self.physics_client is None: + return + + width, height, rgb, _, _ = p.getCameraImage( + width=500, + height=500, + viewMatrix=p.computeViewMatrix( + cameraEyePosition=[4, -4, 1.5], + # cameraEyePosition=[4,-4,4], + cameraTargetPosition=[0, 0, 0], # Центр фокусировки камеры + cameraUpVector=[0, 0, 1], # Направление вверх + ), + projectionMatrix=p.computeProjectionMatrixFOV( + fov=60.0, aspect=1.0, nearVal=0.1, farVal=10.0 + ), + # renderer=p.ER_TINY_RENDERER, + physicsClientId=self.physics_client, + ) + + return (rgb, width, height) diff --git a/utils/test_socket_server.py b/utils/test_socket_server.py index a630d3e..c98f351 100644 --- a/utils/test_socket_server.py +++ b/utils/test_socket_server.py @@ -92,14 +92,15 @@ def handle_client(conn, addr): print(f"Connected by {addr}") try: while True: - data = conn.recv(1024) + data = conn.recv(1024*2) if not data: break req = json.loads(data) + print(req) res = {"queryData": ["ok"]} - if "axis-0" in req["queryAddr"]: + if "queryAddr" in req and "axis-0" in req["queryAddr"]: global coordinates_index - print(coordinates_index) + print(f"----{coordinates_index}----") c = c_angles[coordinates_index] limit = len(c_angles) - 1 # limit = 2 @@ -108,14 +109,15 @@ def handle_client(conn, addr): else: coordinates_index = 0 - if "queryAddr" in req.keys() and "axis-0" in req["queryAddr"]: + if "queryAddr" in req and "axis-0" in req["queryAddr"]: res["queryData"] = c["axis"] if "queryAddr" in req and "world-0" in req["queryAddr"]: res["queryData"] = c["world"] - if req["reqType"] == "command": - res["cmdReply"] = ["ok"] + if "reqType" in req and req["reqType"] == "command" or req["reqType"] == "AddRCC": + res["cmdReply"] = ["it is local dear"] + # print(res) conn.sendall(json.dumps(res).encode()) except json.JSONDecodeError as e: print(f"JSON decode error: {e}")