reachy_sdk_server service robustness on kill/stop/restart

[cdussieux]

related to #146 ?

[cdussieux]

[ros2_control_node-1] [INFO] [1679915944.230628712] [ArmSystem]: (l_arm) READ SPEED_LIMIT ERROR!
[ros2_control_node-1] [INFO] [1679915944.257787186] [ArmSystem]: (l_arm) READ TORQUE_LIMIT ERROR!
[ros2_control_node-1] [INFO] [1679915944.261662902] [ArmSystem]: (l_arm) READ SPEED_LIMIT ERROR!
[ros2_control_node-1] thread '<unnamed>' panicked at 'assertion failed: self.is_input_buffer_empty(port)?', /home/reachy/.cargo/registry/src/github.com-1ecc6299db9ec823/rustypot-0.3.1/src/protocol/mod.rs:63:9
[ros2_control_node-1] note: run with `RUST_BACKTRACE=1` environment variable to display a backtrace
[ros2_control_node-1] fatal runtime error: failed to initiate panic, error 5
[ros2_control_node-1] Stack trace (most recent call last) in thread 23515:
[ros2_control_node-1] #26   Object "", at 0xffffffffffffffff, in 
[ros2_control_node-1] #25   Source "../sysdeps/unix/sysv/linux/x86_64/clone3.S", line 81, in __clone3 [0x7f1e3dd269ff]
[ros2_control_node-1] #24   Source "./nptl/pthread_create.c", line 442, in start_thread [0x7f1e3dc94b42]
[ros2_control_node-1] #23   Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x7f1e3e0dc2b2, in 
[ros2_control_node-1] #22   Object "/opt/ros/humble/lib/controller_manager/ros2_control_node", at 0x55d480278810, in 
[ros2_control_node-1] #21   Object "/opt/ros/humble/lib/libhardware_interface.so", at 0x7f1e3e275401, in hardware_interface::ResourceManager::read(rclcpp::Time const&, rclcpp::Duration const&)
[ros2_control_node-1] #20   Object "/opt/ros/humble/lib/libhardware_interface.so", at 0x7f1e3e29c7c4, in hardware_interface::System::read(rclcpp::Time const&, rclcpp::Duration const&)
[camera_zoom_service-7] [INFO] [1679915944.423045062] [camera_zoom_controller_service]: Launching "get_camera_zoom_level" service.
[camera_zoom_service-7] [INFO] [1679915944.424718669] [camera_zoom_controller_service]: Launching "set_camera_zoom_level" service.
[camera_zoom_service-7] [INFO] [1679915944.427329139] [camera_zoom_controller_service]: Launching "set_camera_zoom_speed" service.
[ros2_control_node-1] #19   Object "/home/reachy/reachy_ws/build/arm_system_hwi/libarm_system_hwi.so", at 0x7f1e36ffef60, in arm_system_hwi::ArmSystem::read(rclcpp::Time const&, rclcpp::Duration const&)
[ros2_control_node-1] #18   Source "src/lib.rs", line 132, in arm_hwi_get_torque_limit [0x7f1e37013df6]
[ros2_control_node-1] #17   Source "src/lib.rs", line 360, in get_mx_torque_limit [0x7f1e3702c82f]
[ros2_control_node-1] #16   Source "/home/reachy/.cargo/registry/src/github.com-1ecc6299db9ec823/cache_cache-0.1.0/src/lib.rs", line 471, in or_try_insert_with<u8, f64, arm_controller::{impl#0}::get_mx_torque_limit::{closure_env#0}> [0x7f1e3703dff3]
[ros2_control_node-1]         468:         }
[ros2_control_node-1]         469: 
[ros2_control_node-1]         470:         if !missing.is_empty() {
[ros2_control_node-1]       > 471:             let missing_values = default(&missing)?;
[ros2_control_node-1]         472: 
[ros2_control_node-1]         473:             assert_eq!(missing.len(), missing_values.len());
[ros2_control_node-1] #15   Source "src/lib.rs", line 365, in {closure#0} [0x7f1e3701f283]
[ros2_control_node-1] #14   Source "src/device/mod.rs", line 32, in sync_read_torque_limit [0x7f1e370bc57b]
[ros2_control_node-1] #13   Source "src/lib.rs", line 266, in sync_read [0x7f1e370dff7d]
[ros2_control_node-1] #12   Source "src/protocol/v1.rs", line 251, in sync_read [0x7f1e37095583]
[ros2_control_node-1] #11   Source "src/protocol/mod.rs", line 63, in send_instruction_packet<rustypot::protocol::v1::V1, rustypot::protocol::v1::PacketV1> [0x7f1e37096907]
[ros2_control_node-1] #10   Source "library/core/src/panicking.rs", line 114, in panic [0x7f1e36ff964c]
[camera_zoom_service-7] [INFO] [1679915944.484748395] [camera_zoom_controller_service]: Node ready!
[ros2_control_node-1] #9    Source "library/core/src/panicking.rs", line 64, in panic_fmt [0x7f1e36ff95b2]
[ros2_control_node-1] #8    Source "library/std/src/panicking.rs", line 575, in begin_panic_handler [0x7f1e37169341]
[ros2_control_node-1] #7    Source "library/std/src/sys_common/backtrace.rs", line 137, in __rust_end_short_backtrace<std::panicking::begin_panic_handler::{closure_env#0}, !> [0x7f1e371669fb]
[ros2_control_node-1] #6    Source "library/std/src/panicking.rs", line 577, in {closure#0} [0x7f1e371695f1]
[ros2_control_node-1] #5    Source "library/std/src/panicking.rs", line 710, in rust_panic_with_hook [0x7f1e371698f6]
[ros2_control_node-1] #4    Source "library/std/src/panicking.rs", line 742, in rust_panic [0x7f1e37169b01]
[ros2_control_node-1] #3    Source "library/std/src/sys/unix/mod.rs", line 350, in abort_internal [0x7f1e37174bc6]
[ros2_control_node-1] #2    Source "./stdlib/abort.c", line 79, in abort [0x7f1e3dc287f2]
[ros2_control_node-1] #1    Source "../sysdeps/posix/raise.c", line 26, in raise [0x7f1e3dc42475]
[ros2_control_node-1] #0  | Source "./nptl/pthread_kill.c", line 89, in __pthread_kill_internal
[ros2_control_node-1]     | Source "./nptl/pthread_kill.c", line 78, in __pthread_kill_implementation
[ros2_control_node-1]       Source "./nptl/pthread_kill.c", line 44, in __pthread_kill [0x7f1e3dc96a7c]
[ros2_control_node-1] Aborted (Signal sent by tkill() 23479 1000)
[gripper_safe_controller-4] [INFO] [1679915944.922033444] [grippers_controller]: Subscribe to "/grippers/commands".
[gripper_safe_controller-4] [INFO] [1679915944.925265382] [grippers_controller]: Subscribe to "/joint_states".
[camera_server-8] [INFO] [1679915945.122101361] [camera_server]: Launching sub/srv...
[camera_focus-6] [INFO] [1679915945.139014358] [camera_focus]: Create service /set_focus_state
[camera_focus-6] [INFO] [1679915945.142468287] [camera_focus]: Autofocus ready to run!
[camera_server-8] [INFO] [1679915945.176798394] [camera_server]: Camera server ready!