From db18fd79d91b24e9d2c938d0ee628c4133a49af2 Mon Sep 17 00:00:00 2001 From: 1Feishu <1291113686@qq.com> Date: Sun, 2 Nov 2025 21:56:50 +0800 Subject: [PATCH 1/7] new --- rmcs_ws/src/rmcs_bringup/config/flight.yaml | 178 +++++++++++++ rmcs_ws/src/rmcs_core/plugins.xml | 3 + .../gimbal/hero_gimbal_controller.cpp | 3 +- .../gimbal/simple_gimbal_controller.cpp | 3 +- .../gimbal/two_axis_gimbal_solver.hpp | 26 +- rmcs_ws/src/rmcs_core/src/hardware/flight.cpp | 243 ++++++++++++++++++ 6 files changed, 452 insertions(+), 4 deletions(-) create mode 100644 rmcs_ws/src/rmcs_bringup/config/flight.yaml create mode 100644 rmcs_ws/src/rmcs_core/src/hardware/flight.cpp diff --git a/rmcs_ws/src/rmcs_bringup/config/flight.yaml b/rmcs_ws/src/rmcs_bringup/config/flight.yaml new file mode 100644 index 00000000..183835e7 --- /dev/null +++ b/rmcs_ws/src/rmcs_bringup/config/flight.yaml @@ -0,0 +1,178 @@ +rmcs_executor: + ros__parameters: + update_rate: 1000.0 + components: + - rmcs_core::hardware::Flight -> flight_hardware + - rmcs_core::referee::Status -> referee_status + + - rmcs_core::controller::gimbal::SimpleGimbalController -> gimbal_controller + - rmcs_core::controller::pid::ErrorPidController -> yaw_angle_pid_controller + - rmcs_core::controller::pid::PidController -> yaw_velocity_pid_controller + - rmcs_core::controller::pid::ErrorPidController -> pitch_angle_pid_controller + - rmcs_core::controller::pid::PidController -> pitch_velocity_pid_controller + + - rmcs_core::controller::shooting::FrictionWheelController -> friction_wheel_controller + - rmcs_core::controller::shooting::HeatController -> heat_controller + - rmcs_core::controller::shooting::BulletFeederController17mm -> bullet_feeder_controller + - rmcs_core::controller::pid::PidController -> left_friction_velocity_pid_controller + - rmcs_core::controller::pid::PidController -> right_friction_velocity_pid_controller + - rmcs_core::controller::pid::PidController -> bullet_feeder_velocity_pid_controller + + # - rmcs_core::broadcaster::TfBroadcaster -> tf_broadcaster + + - rmcs_core::referee::command::Interaction -> referee_interaction + #- rmcs_core::referee::command::interaction::Ui -> referee_ui + #- rmcs_core::referee::app::ui::Flight -> referee_ui_flight + + - rmcs_core::referee::Command -> referee_command + + # - rmcs_auto_aim::AutoAimInitializer -> auto_aim_initializer + # - rmcs_auto_aim::AutoAimController -> auto_aim_controller + +flight_hardware: + ros__parameters: + usb_pid: -1 + yaw_motor_zero_point: 0 + pitch_motor_zero_point: 0 + +referee_status: + ros__parameters: + path: /dev/ttyUSB0 + +gimbal_controller: + ros__parameters: + upper_limit: -0.4598 + lower_limit: 0.4362 + yaw_upper_limit: 0.872665 + yaw_lower_limit: -0.872665 + +yaw_angle_pid_controller: + ros__parameters: + measurement: /gimbal/yaw/control_angle_error + control: /gimbal/yaw/control_velocity + kp: 3.0 + ki: 0.0 + kd: 0.0 + +yaw_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/yaw/velocity_imu + setpoint: /gimbal/yaw/control_velocity + control: /gimbal/yaw/control_torque + kp: 0.5 + ki: 0.0 + kd: 0.0 + +pitch_angle_pid_controller: + ros__parameters: + measurement: /gimbal/pitch/control_angle_error + control: /gimbal/pitch/control_velocity + kp: 3.0 + ki: 0.0 + kd: 0.0 + +pitch_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/pitch/velocity_imu + setpoint: /gimbal/pitch/control_velocity + control: /gimbal/pitch/control_torque + kp: 0.5 + ki: 0.0 + kd: 0.0 + +friction_wheel_controller: + ros__parameters: + friction_wheels: + - /gimbal/left_friction + - /gimbal/right_friction + friction_velocities: + - 740.0 + - 740.0 + friction_soft_start_stop_time: 1.0 + +heat_controller: + ros__parameters: + heat_per_shot: 10000 + reserved_heat: 0 + +bullet_feeder_controller: + ros__parameters: + bullets_per_feeder_turn: 8.0 + shot_frequency: 20.0 + safe_shot_frequency: 10.0 + eject_frequency: 10.0 + eject_time: 0.05 + deep_eject_frequency: 5.0 + deep_eject_time: 0.2 + single_shot_max_stop_delay: 2.0 + +shooting_recorder: + ros__parameters: + friction_wheel_count: 2 + # 1: trigger, 2: timing + log_mode: 1 + +left_friction_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/left_friction/velocity + setpoint: /gimbal/left_friction/control_velocity + control: /gimbal/left_friction/control_torque + kp: 0.003436926 + ki: 0.00 + kd: 0.009373434 + +right_friction_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/right_friction/velocity + setpoint: /gimbal/right_friction/control_velocity + control: /gimbal/right_friction/control_torque + kp: 0.003436926 + ki: 0.00 + kd: 0.009373434 + +bullet_feeder_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/bullet_feeder/velocity + setpoint: /gimbal/bullet_feeder/control_velocity + control: /gimbal/bullet_feeder/control_torque + kp: 0.583 + ki: 0.0 + kd: 0.0 + +auto_aim_controller: + ros__parameters: + # capture + use_video: false # If true, use video stream instead of camera. + video_path: "/workspaces/RMCS/rmcs_ws/resources/1.avi" + exposure_time: 3 + invert_image: false + # identifier + armor_model_path: "/models/mlp.onnx" + # pnp + fx: 1.722231837421459e+03 + fy: 1.724876404292754e+03 + cx: 7.013056440882832e+02 + cy: 5.645821718351237e+02 + k1: -0.064232403853946 + k2: -0.087667493884102 + k3: 0.792381808294582 + # tracker + armor_predict_duration: 500 + # controller + gimbal_predict_duration: 100 + yaw_error: 0.02 + pitch_error: -0.01 + shoot_velocity: 28.0 + predict_sec: 0.095 + # etc + buff_predict_duration: 200 + buff_model_path: "/models/buff_nocolor_v6.onnx" + omni_exposure: 1000.0 + record_fps: 120 + debug: false # Setup in actual using.Debug mode is used when referee is not ready + debug_color: 0 # 0 For blue while 1 for red. mine + debug_robot_id: 4 + debug_buff_mode: false + record: true + raw_img_pub: false # Set false in actual use + image_viewer_type: 2 \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/plugins.xml b/rmcs_ws/src/rmcs_core/plugins.xml index ac7d1368..4d3bb141 100644 --- a/rmcs_ws/src/rmcs_core/plugins.xml +++ b/rmcs_ws/src/rmcs_core/plugins.xml @@ -8,6 +8,9 @@ Test plugin. + + Test plugin. + Test plugin. diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/hero_gimbal_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/hero_gimbal_controller.cpp index f20bd98f..65417862 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/hero_gimbal_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/hero_gimbal_controller.cpp @@ -25,7 +25,8 @@ class HeroGimbalController rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) , imu_gimbal_solver( *this, get_parameter("upper_limit").as_double(), - get_parameter("lower_limit").as_double()) + get_parameter("lower_limit").as_double(),get_parameter("yaw_upper_limit").as_double() + ,get_parameter("yaw_lower_limit").as_double()) , encoder_gimbal_solver( *this, get_parameter("upper_limit").as_double(), get_parameter("lower_limit").as_double()) { diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/simple_gimbal_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/simple_gimbal_controller.cpp index d27704d5..517dc0fc 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/simple_gimbal_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/simple_gimbal_controller.cpp @@ -22,7 +22,8 @@ class SimpleGimbalController rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) , two_axis_gimbal_solver( *this, get_parameter("upper_limit").as_double(), - get_parameter("lower_limit").as_double()) { + get_parameter("lower_limit").as_double(),get_parameter("yaw_upper_limit").as_double() + ,get_parameter("yaw_lower_limit").as_double()) { register_input("/remote/joystick/left", joystick_left_); register_input("/remote/switch/right", switch_right_); diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/two_axis_gimbal_solver.hpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/two_axis_gimbal_solver.hpp index 5d937aae..ae590f10 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/two_axis_gimbal_solver.hpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/two_axis_gimbal_solver.hpp @@ -26,11 +26,14 @@ class TwoAxisGimbalSolver { }; public: - TwoAxisGimbalSolver(rmcs_executor::Component& component, double upper_limit, double lower_limit) + TwoAxisGimbalSolver(rmcs_executor::Component& component, double upper_limit, double lower_limit,double yaw_upper_limit, double yaw_lower_limit) : upper_limit_(std::cos(upper_limit), -std::sin(upper_limit)) - , lower_limit_(std::cos(lower_limit), -std::sin(lower_limit)) { + , lower_limit_(std::cos(lower_limit), -std::sin(lower_limit)) + , yaw_upper_limit_(std::cos(yaw_upper_limit),-std::sin(yaw_upper_limit)) + , yaw_lower_limit_(std::cos(yaw_lower_limit),-std::sin(yaw_lower_limit)){ component.register_input("/gimbal/pitch/angle", gimbal_pitch_angle_); + component.register_input("/gimbal/yaw/angle", gimbal_yaw_angle_); component.register_input("/tf", tf_); } @@ -167,6 +170,23 @@ class TwoAxisGimbalSolver { *control_direction << upper_limit_.x() * projection, upper_limit_.y(); else if (z < lower_limit_.y()) *control_direction << lower_limit_.x() * projection, lower_limit_.y(); + + const auto& [x_,y_,z_] = *control_direction; + + Eigen::Vector2d yaw_projection{x_,y_}; + double yaw_norm = yaw_projection.norm(); + if (yaw_norm > 0) + yaw_projection /=yaw_norm; + else{ + control_enabled_ =false; + return; + } + + if (y_ > yaw_upper_limit_.y()) + *control_direction << yaw_upper_limit_.x() * yaw_projection.x(), yaw_upper_limit_.y(), z_; + else if (y_ < yaw_lower_limit_.y()) + *control_direction << yaw_lower_limit_.x() * yaw_projection.x(), yaw_lower_limit_.y(), z_; + } static AngleError calculate_control_errors( @@ -185,8 +205,10 @@ class TwoAxisGimbalSolver { static constexpr double nan_ = std::numeric_limits::quiet_NaN(); const Eigen::Vector2d upper_limit_, lower_limit_; + const Eigen::Vector2d yaw_upper_limit_, yaw_lower_limit_; rmcs_executor::Component::InputInterface gimbal_pitch_angle_; + rmcs_executor::Component::InputInterface gimbal_yaw_angle_; rmcs_executor::Component::InputInterface tf_; OdomImu::DirectionVector yaw_axis_filtered_{Eigen::Vector3d::UnitZ()}; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp b/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp new file mode 100644 index 00000000..af218545 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp @@ -0,0 +1,243 @@ +#include +#include + +#include +#include + +#include +#include +#include +#include + +#include +#include + +#include "hardware/device/bmi088.hpp" +#include "hardware/device/dji_motor.hpp" +#include "hardware/device/dr16.hpp" +#include "hardware/device/lk_motor.hpp" +#include "librmcs/utility/ring_buffer.hpp" + +namespace rmcs_core::hardware { + +class Flight + : public rmcs_executor::Component + , public rclcpp::Node + , private librmcs::client::CBoard { +public: + Flight() + : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} + , librmcs::client::CBoard{static_cast(get_parameter("usb_pid").as_int())} + , logger_(get_logger()) + , flight_command_( + create_partner_component(get_component_name() + "_command", *this)) + , gimbal_yaw_motor_(*this, *flight_command_, "/gimbal/yaw") + , gimbal_pitch_motor_(*this, *flight_command_, "/gimbal/pitch") + , gimbal_left_friction_(*this, *flight_command_, "/gimbal/left_friction") + , gimbal_right_friction_(*this, *flight_command_, "/gimbal/right_friction") + , gimbal_bullet_feeder_(*this, *flight_command_, "/gimbal/bullet_feeder") + , dr16_(*this) + , bmi088_(500.0, 0.3, 0.005) + + , transmit_buffer_(*this, 32) + , event_thread_([this]() { handle_events(); }) { + gimbal_yaw_motor_.configure( + device::LkMotor::Config{device::LkMotor::Type::MHF7015}.set_encoder_zero_point( + static_cast(get_parameter("yaw_motor_zero_point").as_int()))); + gimbal_pitch_motor_.configure( + device::LkMotor::Config{device::LkMotor::Type::MG4010E_I10}.set_encoder_zero_point( + static_cast(get_parameter("pitch_motor_zero_point").as_int()))); + gimbal_left_friction_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::M3508} + .set_reversed() + .set_reduction_ratio(1.)); + gimbal_right_friction_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::M3508} + + .set_reduction_ratio(1.)); + gimbal_bullet_feeder_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::M2006}.enable_multi_turn_angle()); + + register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_); + register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_imu_); + register_output("/tf", tf_); + + bmi088_.set_coordinate_mapping( + [](double x, double y, double z) { return std::make_tuple(-x, z, y); }); + + using namespace rmcs_description; + + constexpr double rotor_distance_x = 0.83637; + constexpr double rotor_distance_y = 0.83637; + + tf_->set_transform( + Eigen::Translation3d{rotor_distance_x / 2, rotor_distance_y / 2, 0}); + tf_->set_transform( + Eigen::Translation3d{rotor_distance_x / 2, -rotor_distance_y / 2, 0}); + tf_->set_transform( + Eigen::Translation3d{-rotor_distance_x / 2, rotor_distance_y / 2, 0}); + tf_->set_transform( + Eigen::Translation3d{-rotor_distance_x / 2, -rotor_distance_y / 2, 0}); + + constexpr double gimbal_center_x = 0; + constexpr double gimbal_center_y = 0; + constexpr double gimbal_center_z = 0.20552; + tf_->set_transform( + Eigen::Translation3d{gimbal_center_x, gimbal_center_y, gimbal_center_z}); + + tf_->set_transform(Eigen::Translation3d{0.0557, 0, 0.053}); + + gimbal_calibrate_subscription_ = create_subscription( + "/gimbal/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { + gimbal_calibrate_subscription_callback(std::move(msg)); + }); + register_output("/referee/serial", referee_serial_); + referee_serial_->read = [this](std::byte* buffer, size_t size) { + return referee_ring_buffer_receive_.pop_front_multi( + [&buffer](std::byte byte) { *buffer++ = byte; }, size); + }; + referee_serial_->write = [this](const std::byte* buffer, size_t size) { + transmit_buffer_.add_uart1_transmission(buffer, size); + return size; + }; + } + + ~Flight() override { + stop_handling_events(); + event_thread_.join(); + } + + void update() override { + update_motors(); + update_imu(); + dr16_.update_status(); + } + + void command_update() { + uint16_t can_commands[4]; + + transmit_buffer_.add_can1_transmission(0x141, gimbal_yaw_motor_.generate_command()); + transmit_buffer_.add_can1_transmission(0x142, gimbal_pitch_motor_.generate_command()); + + can_commands[0] = gimbal_bullet_feeder_.generate_command(); + can_commands[1] = gimbal_left_friction_.generate_command(); + can_commands[2] = gimbal_right_friction_.generate_command(); + can_commands[3] = 0; + transmit_buffer_.add_can2_transmission(0x200, std::bit_cast(can_commands)); + } + +private: + void update_motors() { + using namespace rmcs_description; + gimbal_yaw_motor_.update_status(); + tf_->set_state(gimbal_yaw_motor_.angle()); + + gimbal_pitch_motor_.update_status(); + tf_->set_state(gimbal_pitch_motor_.angle()); + + gimbal_bullet_feeder_.update_status(); + gimbal_left_friction_.update_status(); + gimbal_right_friction_.update_status(); + } + + void update_imu() { + bmi088_.update_status(); + Eigen::Quaterniond gimbal_imu_pose{bmi088_.q0(), bmi088_.q1(), bmi088_.q2(), bmi088_.q3()}; + tf_->set_transform( + gimbal_imu_pose.conjugate()); + + *gimbal_yaw_velocity_imu_ = bmi088_.gz(); + *gimbal_pitch_velocity_imu_ = bmi088_.gy(); + } + void gimbal_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr) { + RCLCPP_INFO( + logger_, "[gimbal calibration] New yaw offset: %ld", + gimbal_yaw_motor_.calibrate_zero_point()); + RCLCPP_INFO( + logger_, "[gimbal calibration] New pitch offset: %ld", + gimbal_pitch_motor_.calibrate_zero_point()); + } + +protected: + void can1_receive_callback( + uint32_t can_id, uint64_t can_data, bool is_extended_can_id, bool is_remote_transmission, + uint8_t can_data_length) override { + if (is_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] + return; + + if (can_id == 0x141) { + gimbal_yaw_motor_.store_status(can_data); + } else if (can_id == 0x142) { + gimbal_pitch_motor_.store_status(can_data); + } + } + + void can2_receive_callback( + uint32_t can_id, uint64_t can_data, bool is_extended_can_id, bool is_remote_transmission, + uint8_t can_data_length) override { + if (is_remote_transmission || is_extended_can_id || can_data_length < 8) [[unlikely]] + return; + if (can_id == 0x201) { + gimbal_bullet_feeder_.store_status(can_data); + } else if (can_id == 0x203) { + gimbal_right_friction_.store_status(can_data); + } else if (can_id == 0x202) { + gimbal_left_friction_.store_status(can_data); + } + } + + void uart1_receive_callback(const std::byte* uart_data, uint8_t uart_data_length) override { + referee_ring_buffer_receive_.emplace_back_multi( + [&uart_data](std::byte* storage) { *storage = *uart_data++; }, uart_data_length); + } + + void dbus_receive_callback(const std::byte* uart_data, uint8_t uart_data_length) override { + dr16_.store_status(uart_data, uart_data_length); + } + + void accelerometer_receive_callback(int16_t x, int16_t y, int16_t z) override { + bmi088_.store_accelerometer_status(x, y, z); + } + + void gyroscope_receive_callback(int16_t x, int16_t y, int16_t z) override { + bmi088_.store_gyroscope_status(x, y, z); + } + +private: + rclcpp::Logger logger_; + class FlightCommand : public rmcs_executor::Component { + public: + explicit FlightCommand(Flight& flight) + : flight_(flight) {} + void update() override { flight_.command_update(); } + Flight& flight_; + }; + + std::shared_ptr flight_command_; + rclcpp::Subscription::SharedPtr gimbal_calibrate_subscription_; + device::LkMotor gimbal_yaw_motor_; + device::LkMotor gimbal_pitch_motor_; + + device::DjiMotor gimbal_left_friction_; + device::DjiMotor gimbal_right_friction_; + device::DjiMotor gimbal_bullet_feeder_; + + device::Dr16 dr16_; + device::Bmi088 bmi088_; + + OutputInterface gimbal_yaw_velocity_imu_; + OutputInterface gimbal_pitch_velocity_imu_; + + OutputInterface tf_; + + librmcs::utility::RingBuffer referee_ring_buffer_receive_{256}; + OutputInterface referee_serial_; + + librmcs::client::CBoard::TransmitBuffer transmit_buffer_; + std::thread event_thread_; +}; +} // namespace rmcs_core::hardware + +#include + +PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::Flight, rmcs_executor::Component) \ No newline at end of file From 8c0422a4d5ca2b6cc5b3253e64476f00e6984cdd Mon Sep 17 00:00:00 2001 From: 1Feishu <1291113686@qq.com> Date: Wed, 26 Nov 2025 21:13:43 +0800 Subject: [PATCH 2/7] flight.2 --- .script/sync-remote | 2 +- ip.conf | 1 + rmcs_ws/src/rmcs_bringup/config/flight.yaml | 48 ++++++++++--------- .../gimbal/simple_gimbal_controller.cpp | 3 +- rmcs_ws/src/rmcs_core/src/hardware/flight.cpp | 12 ++--- 5 files changed, 35 insertions(+), 31 deletions(-) create mode 100644 ip.conf diff --git a/.script/sync-remote b/.script/sync-remote index f9c915fc..60a2b816 100755 --- a/.script/sync-remote +++ b/.script/sync-remote @@ -8,7 +8,7 @@ import subprocess from colorama import Fore, Style -SRC_DIR = "${RMCS_PATH}/rmcs_ws/install" +SRC_DIR = "/workspaces/RMCS/rmcs_ws/install" DST_DIR = "ssh://remote//rmcs_install" SOCKET_PATH = "/tmp/sync-remote" diff --git a/ip.conf b/ip.conf new file mode 100644 index 00000000..24dd82fa --- /dev/null +++ b/ip.conf @@ -0,0 +1 @@ +169.254.233.233 diff --git a/rmcs_ws/src/rmcs_bringup/config/flight.yaml b/rmcs_ws/src/rmcs_bringup/config/flight.yaml index 183835e7..151db7db 100644 --- a/rmcs_ws/src/rmcs_bringup/config/flight.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/flight.yaml @@ -7,9 +7,8 @@ rmcs_executor: - rmcs_core::controller::gimbal::SimpleGimbalController -> gimbal_controller - rmcs_core::controller::pid::ErrorPidController -> yaw_angle_pid_controller - - rmcs_core::controller::pid::PidController -> yaw_velocity_pid_controller + - rmcs_core::controller::pid::ErrorPidController -> yaw_velocity_pid_controller - rmcs_core::controller::pid::ErrorPidController -> pitch_angle_pid_controller - - rmcs_core::controller::pid::PidController -> pitch_velocity_pid_controller - rmcs_core::controller::shooting::FrictionWheelController -> friction_wheel_controller - rmcs_core::controller::shooting::HeatController -> heat_controller @@ -29,37 +28,48 @@ rmcs_executor: # - rmcs_auto_aim::AutoAimInitializer -> auto_aim_initializer # - rmcs_auto_aim::AutoAimController -> auto_aim_controller + - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster + +value_broadcaster: + ros__parameters: + forward_list: + - /gimbal/yaw/angle + - /gimbal/yaw/velocity + - /gimbal/yaw/torque + - /gimbal/yaw/control_torque + - /gimbal/yaw/control_angle_error + - /gimbal/yaw/control_velocity + flight_hardware: ros__parameters: usb_pid: -1 - yaw_motor_zero_point: 0 - pitch_motor_zero_point: 0 + yaw_motor_zero_point: 5450 + pitch_motor_zero_point: 30473 referee_status: ros__parameters: - path: /dev/ttyUSB0 + path: /dev/tty0 gimbal_controller: ros__parameters: - upper_limit: -0.4598 - lower_limit: 0.4362 - yaw_upper_limit: 0.872665 - yaw_lower_limit: -0.872665 + upper_limit: -0.249 + lower_limit: 0.249 + yaw_upper_limit: -180.0 + yaw_lower_limit: 180.0 yaw_angle_pid_controller: ros__parameters: measurement: /gimbal/yaw/control_angle_error control: /gimbal/yaw/control_velocity - kp: 3.0 + kp: 1.0 ki: 0.0 kd: 0.0 yaw_velocity_pid_controller: ros__parameters: - measurement: /gimbal/yaw/velocity_imu - setpoint: /gimbal/yaw/control_velocity + measurement: /gimbal/yaw/control_velocity control: /gimbal/yaw/control_torque - kp: 0.5 + kp: 1.0 ki: 0.0 kd: 0.0 @@ -67,16 +77,7 @@ pitch_angle_pid_controller: ros__parameters: measurement: /gimbal/pitch/control_angle_error control: /gimbal/pitch/control_velocity - kp: 3.0 - ki: 0.0 - kd: 0.0 - -pitch_velocity_pid_controller: - ros__parameters: - measurement: /gimbal/pitch/velocity_imu - setpoint: /gimbal/pitch/control_velocity - control: /gimbal/pitch/control_torque - kp: 0.5 + kp: 16.0 ki: 0.0 kd: 0.0 @@ -156,6 +157,7 @@ auto_aim_controller: k1: -0.064232403853946 k2: -0.087667493884102 k3: 0.792381808294582 + # tracker armor_predict_duration: 500 # controller diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/simple_gimbal_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/simple_gimbal_controller.cpp index 517dc0fc..19bdbaa8 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/simple_gimbal_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/simple_gimbal_controller.cpp @@ -62,11 +62,12 @@ class SimpleGimbalController if (!two_axis_gimbal_solver.enabled()) return two_axis_gimbal_solver.update(TwoAxisGimbalSolver::SetToLevel()); - constexpr double joystick_sensitivity = 0.006; + constexpr double joystick_sensitivity = 0.06; constexpr double mouse_sensitivity = 0.5; double yaw_shift = joystick_sensitivity * joystick_left_->y() + mouse_sensitivity * mouse_velocity_->y(); + RCLCPP_INFO(get_logger(), "%lf",yaw_shift); double pitch_shift = -joystick_sensitivity * joystick_left_->x() - mouse_sensitivity * mouse_velocity_->x(); diff --git a/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp b/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp index af218545..96c836a8 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp @@ -117,7 +117,7 @@ class Flight uint16_t can_commands[4]; transmit_buffer_.add_can1_transmission(0x141, gimbal_yaw_motor_.generate_command()); - transmit_buffer_.add_can1_transmission(0x142, gimbal_pitch_motor_.generate_command()); + transmit_buffer_.add_can2_transmission(0x144, gimbal_pitch_motor_.generate_command()); can_commands[0] = gimbal_bullet_feeder_.generate_command(); can_commands[1] = gimbal_left_friction_.generate_command(); @@ -167,9 +167,7 @@ class Flight if (can_id == 0x141) { gimbal_yaw_motor_.store_status(can_data); - } else if (can_id == 0x142) { - gimbal_pitch_motor_.store_status(can_data); - } + } } void can2_receive_callback( @@ -179,10 +177,12 @@ class Flight return; if (can_id == 0x201) { gimbal_bullet_feeder_.store_status(can_data); - } else if (can_id == 0x203) { - gimbal_right_friction_.store_status(can_data); } else if (can_id == 0x202) { gimbal_left_friction_.store_status(can_data); + } else if (can_id == 0x203) { + gimbal_right_friction_.store_status(can_data); + } else if (can_id == 0x144) { + gimbal_pitch_motor_.store_status(can_data); } } From 9ffd126ab51a6cf3588d09a981325bbcb26cfa5c Mon Sep 17 00:00:00 2001 From: 1Feishu <1291113686@qq.com> Date: Wed, 3 Dec 2025 17:20:31 +0800 Subject: [PATCH 3/7] draft --- frames_2025-11-27_16.09.16.gv | 1 + frames_2025-11-27_16.09.16.pdf | Bin 0 -> 6023 bytes frames_2025-11-27_16.24.56.gv | 1 + frames_2025-11-27_16.24.56.pdf | Bin 0 -> 6022 bytes frames_2025-11-27_16.25.48.gv | 1 + frames_2025-11-27_16.25.48.pdf | Bin 0 -> 6022 bytes frames_2025-11-27_16.27.10.gv | 1 + frames_2025-11-27_16.27.10.pdf | Bin 0 -> 6021 bytes frames_2025-12-02_19.18.18.gv | 1 + frames_2025-12-02_19.18.18.pdf | Bin 0 -> 6022 bytes rmcs_ws/src/rmcs_bringup/config/flight.yaml | 41 ++++++++++-------- rmcs_ws/src/rmcs_core/plugins.xml | 3 ++ .../gimbal/simple_gimbal_controller.cpp | 5 ++- rmcs_ws/src/rmcs_core/src/hardware/flight.cpp | 18 ++++---- 14 files changed, 43 insertions(+), 29 deletions(-) create mode 100644 frames_2025-11-27_16.09.16.gv create mode 100644 frames_2025-11-27_16.09.16.pdf create mode 100644 frames_2025-11-27_16.24.56.gv create mode 100644 frames_2025-11-27_16.24.56.pdf create mode 100644 frames_2025-11-27_16.25.48.gv create mode 100644 frames_2025-11-27_16.25.48.pdf create mode 100644 frames_2025-11-27_16.27.10.gv create mode 100644 frames_2025-11-27_16.27.10.pdf create mode 100644 frames_2025-12-02_19.18.18.gv create mode 100644 frames_2025-12-02_19.18.18.pdf diff --git a/frames_2025-11-27_16.09.16.gv b/frames_2025-11-27_16.09.16.gv new file mode 100644 index 00000000..545c92c6 --- /dev/null +++ b/frames_2025-11-27_16.09.16.gv @@ -0,0 +1 @@ +digraph G { "No tf data received" } \ No newline at end of file diff --git a/frames_2025-11-27_16.09.16.pdf b/frames_2025-11-27_16.09.16.pdf new file mode 100644 index 0000000000000000000000000000000000000000..829e93354c7f484d29c422182fb068b640f6a7f9 GIT binary patch literal 6023 zcmcJTc|6qX+sADwOSYj18EaW*F*8QiY}sYY9z%oCn6Zo{BeG@Rh3rdqAu19fOV&uo zb|fWPk|Jd1nbC62dHv3_{PWCrUNduF_w~J(&vkvTdEIZ}%W4{u5GfgY;YZ6=yYw(1 z7>Kubp_i8jf^^Y1CxSB&PBIzM1A#!01_nz&dyu}dC<0my?SQ|ArdL#?_au0rQLgm9 zS$YOKI@A!BXM@%04j!5plCP~b&>NjUUrH2LNO@=_CNV6wvzcvE7abb|BIbDEClhGtS*psL7w|L%g%xldm;OK#S)9@Owa`@~CjUO_#ecWP*+J2?2o&7Zr$B zMXPXl)`-3=2R(MvQFSmf;bxpLGE4Q(D^kB~(-$V;aQJQI(12t5Pv|Ct;(BWvTOvcg zWb<%Gsz*<-z@&N9hv5AanQaM)ZR5-}W`h}ZJ42s2TmXNhA*9Ms!O}{Y4fy?eNNfU!iK57u6e~N8_&j+%@SJSue6k%$_>b`_a#srdfKzVtxf%F2Qz@!Ecqdk-APqc@@HcNqNw6ds zDDx{KBq!4Q=VZv=lL0{{H{8%bkb%96F~Rj{>YuTc{dFfu9|$?>(7 zN*jaUMa@Nhq@tijQri5d2;|X!iU2|US{4NIw{`to1_b(h9sib7WL&H1T`< z-8$mqFJ+sbM#uK28tZYUl++Z052=EQof&8^J}z6_@u9$zK=}syc0Zp>?>4w8iaRN= zW?oa}a5JmG>Em_>NgA$Jj~XBBbS?UvGzkdl>aVbPB9Xpnu%=BZu~b&oa9hlNs`%_o z886gjv=X0QL!%^cFxC-o+w9#5z13<#QK7ll0q;BWse^@?MJdeJXUai*=`8+_DWL;? ziUaV8%lEw_NA_6q?*I)U(N4-fw-;{|0fXzS1KoJK_e1tW6rT<_^sSG}j|jw6w%pW+9n!R^~TbJvuZ+@|P>iurW%Q z&(3f*c)QxYtKg8xEAOv9Xf7|G*HR4WDKrCTjwP?35f*-XqPD1=|5Lu!xz9c(!kV?Q zAGv)I!D()~TB*nQ z!_Cbp&2^0s?2``PvJtlQL3}HKPl`9XHRyfA_f!MC7WfSMR1eIJ-fLbGo4CX7?hf->if*$Q3We!_glZbui`$ry3an3$M|GV(zURJb-V$>MWd11qV`F$gr>^J?+$Z4X{OC}3=A%aq=O1o&zoHrAowVgw;CUBxO#4dKc{x_=rxFg{ zT8~AD3NS6xR0Ka_TGRp27hW){=77tXW4hPPK45VCI4!&cm_4IK*GaMH;iuz=v=KTS zs=+LYvB$=b)L?QVeQa*0+AxNWD0Ei4e%hT|GS0jz9*}Ih`5Alt>t`%}(!y%j#Z5F5 zEZ-lMMjMv(Cw`hZJJqtgo*W-Y{8BZD6U>D_H}^GK6}h1+l`v4*^T>D_wfV3FZ?N3$ zQfGEIvobh3qy#vSl*>syA(E4RDlJoRdZ94;^CmM3){;2DN&RVP05qGQc`cL8N=u?F ziGRLoz?E|{QydYyjBZu|eUH4^WH2`WIeIDjtKNdwhKQwD&oy*{Q|Ya%D- zzQpG;Ht^)D+NSN3ZN18Z)3$<+ayxb@z$s7 zGAb%BPe81xzpO)aRXaFYjlt4M#*?A3S81zEe5ORIpfuNLJrY^6R^{NAW%v;^`fb~@ zo^Z9%>tF<2A*%Ssq(-IR5HFP;C{QVqWGodhoBfD?iFZ;_`N9Pt-JEz?@Y@NRo#v+Y ztRYGeCP>r*?2F+$Sn&I4St_JF2HZYqzVKGiR{z zYjV5$-rY$85yGoMi516fG5(nTs=cLi)n&PL4bulXbo-4jB_-vqQLd*cxnK@3=>=ma zxT~)QEG_#4+>meqoW#?=*9#R8X8qpflZUR~R<{bzKUtAfd2CM2P3V(H@(Il0?tXpy zLd5Mqe6G3HKAtZ$zZUT&abZUt6(u@x$TM2tJd-9qSj8w?ue3jtGiAd)37BXOk7r@$ zM1N>f-0*R|Z`QIk(|0aIkQm~xRyt#LK1UAqm@WSHxX6nDdYvoE*}~IVdaK=Y(Sl{rs<4* zOALLz^vy*@({ZtCXY2~wDeR-uPhMFt)Q+LO7}#q6Nb@)$=}v(023sWSlf2Ac&NrYS zi@`)Vm%Ea=ByICO02imdu1d_CL&Y#o6&-h_oVd<~vhL~^yTTTM3K{S?ea&V=Iew?b zdq`0$51o(0^q3H##SK5zr1Xw#+7bxBqtiO)dN+K|@?5Y%&M|38>HA8z=$FJwAK+MI zR$?1zZ-(TzjoWDzCoG{7RYzF%A&5!@qV{^Y#o0c&)(eR8Sb}ya4o%M&rXq+otX3Yv zDLJx)ZLb>~yX#Upq;g8!sn496n6%8I&%pPz?v3K6&?a}z!&ipOSE;73tQ7^{=O&lE zT|iGJ4_*KSx%ldCVwH$trMrl1kqdnbr__cI{Ui6|{j~C?Xua$x&c4^1#sCCt70}C% z9S@QVb_4IEvyQ%U*=QxIoi2khyX2qE3Bid4GGz*EoKtVsFj?at+qoKgHdS?>CN}-_ z!;J&s0YKj4!LmU3>Qs&Vuwu--;6vs?2JVZkVJ4QGT(3?lAT8Ke{b83hnvZX`PP&v z5mzd#U@-JKykJ8jT=KbCr6b3^{8pw966I5jkL)=Ny+eBicGSICLiL0WA3t-u>Z{q! zS~r18D(pcTtnu9@p1{NTv(+tzRzAQNGzQkJ_k{Wr=N9^P&%xX6U8X`Q9~-1mnGVH7 zV{UJo)bq~O^$CT=l@(r)Drf~xONNK_Pqfv1Yo_0v5Y7qSJM3sg_we2hL+KPgbs`W@ z$J7gjQ8_$ax;M@Hbq_zs!vn4)DEXkmNZXDx)$CZ}_Xjoh%fasYw z*{v1o@RDN1+}Mdm#15a|FF8z^>K}Ft-A^ESVGR(saVJ17Px2ZJF2PZDr?y=5Vk;^RBrn zjA1)vaXEb58^HW(_d3HZoB%q?#3=0h+i1=D6SUnS3aly6&m#g6&i02IP2++O#d32D zS0C~kY@k<+FUl81+~nG-Keac~gFUDK=Ja>H)%LIFGDu%(pqo~ldxzg$O{XUAZ831f zmv4D;1QyS*a!k_?&f-rx_=+5ubjoC|R-5{_M^*D7*2Z{!$77UMKiD2yePkj(`+|>; z@f%--FE~Q!Q|&iCw7pMAqL0%|(37pG7|`jN1IaS~mMGA2^0tm>gF#m_vCi|pI;^(+9X5koTT&c8UuxE}?=}>^e(p2weu>$bJ!E`4IMK(l zrY<}PYUSS+0a}mZy`FjFg*)~zKeB$Z#)h@t7_%#co%GJCFKbxd)_%5(#9imeozCdV zt?roTa2J37XGWw|PTi_xSwoT2Lo+Up+`9LUY|XL}gIg<$NdIz%KS$OB%|&3uL_Rt^9D0pZ=x3@2_ASVKVs)|6G0Hc9#vs(cj8;HRR5so%HEUo zOWL!tU;|c9IzJqXXV;GJOBjz%Yu64qGhjl@QSZ5~4%J{~N6fl;j$zy)OhVz*YYvEB z{8=BWO_Ks*Ozcy@(3E%_Kgt1%EH>4J+m{Oz-*fO_6hYe+^t}M|y;84h^iR*Tm#2$0 zgXn9Nw6|sR6ULX{yjKnv_;Z<4XWu@)FgA6pSfE+9!8!^Os#MHy{|`sgS1JH=wxNOu zM-IJ?^pr?`m=T|Y^qJt&G_3Vk<(}IYzRWi|&cfn$4e#}IP~FoyCRpo1!(qY9fP4G$ z+rgm7)->(Pw8({H2h;+HpveC`4IUlxejW#O6wE!5wmtOWF%rKJH><;-GI5Nr)`c#VUP zlfndl(V+$xIyHtqKV0K?7y@WKk>$nf3?ocMuUC7hlzRA1UoW3*SQh<0zx765C3(;b zqcw2mG>C(CQR70LUV$`yI_U-bXQa!AK%=-=f3g(}F{;Rh!mLk2apr-KK^jez&~fdWV- zeUvK+B3xE6xU6O@u7-9&UGXwTdte-YJ7}O>G1wcx^FR84N1dWSda)=cPoOmPx2uW@ z-WO;El$1fp0wtwk2p|*!h65oGuq@E#$VrQU!eSgQ;+(K(Ao$2~(bEA%vZ>SKo#YtiN-iN6OM*CG8q%lu2+D_ANP|Rn!g(4Es{_J8cK3LO0~bZ37Mvl z)AXNu{20&6o^V7`Ogy~MWUY>@Dkx9%5tsRIwIMSVRcDmPF9mc_Kid$wTs`N%>3|GJPenWsDU-;v!K7qiM^hi=5hMBuK`g{QZBG#?*k5D={z&a=Zox;-fAQPXkiZ)IIB>UtX-JcawS^pT4OKU}3v z?!GcB2dk>@18?voGRL`M+ifv4G6iQ=vCS>{WZ5L}-4y|OEFkrsnwCBnvG{W&M|{lv zQ^_snzHXxdEpwJTJc<)EZ?C;xZQgNrzf06=Q?B~R)>yBPt9h~%Zy8J7JIoxAqaf0l zS67&^>9}%rrt#frbA?`XdDbUjiyXhfi{@~ty7_aMKT5-3=FlZ@=|KZjoo@vo=R+z9=nnaQDI(^XVfnLQbC=Y$YVfwCHF0!z~|0Yw#E6b);C pj6H`k?OvAozNaHU*cFfzt~Ms~c$0{}<(eRBZJPc^D`3jZ^%x^VP0c^oB~<9(-Fq(B&k)hHNk(*IHWH5*TtM zn}$15+`5AVCQTw|g7%K3x5g*5jx$#p58SKS9{kMd#QsMrT&frqB&~><8PO}W*FJW3 z0eN@6L`}}B8OC+i(6^g!n^*q(GG+6;sLlvI8fW)&*Q9S`y~rYw?5) z`4th86Y2AFGQ{u606@bVE@%Ky*T%_!;Cwjs&sfU-dJ=dU06y$c$GEu@0MMV`|L6Eh z8-U(L&P9Hpq@Y0@vG}J5Y?a=IP6c@2Hnv@N&v%MgELJ->r}>IX+gps$q-D052M_j}yk z8sFM4#T%bS$M&Wg>TpI!s3-&dt158>+USFL)g{^bdaCS8Do1B5gx=_0kcE#p3e%+hR6T1!ty< zc_B`tW%#s8YDIzlv5q*aCeKdDtrk;?QjOgXSnsJ%9W2Z&ilN?KQ?}xZXYhYa3GMS! z?1PS7dFUB2vdfZl2cQRza!~TRy>KHR5L8zY;BvZaFL*B)`Ly4*cWqpLL?F7X{XxRO zh*bCXdNZ=1_Xthvka&afmT;v?hCZP8yyn?%uv_0=bw{6%`SsaeXY4~xMs9uVZER1x zjf8+*7dYqzKauP7@&+?gm+DN5uTNKidrv$OFD|;$FIf?9x6+W3`heF}j=R*b%-jY! zPw9|RGjgM)TRaDHg`+b6IDTKwKC4$7V4Gw!ttE;vc+&`X=7pQ+?u|4uB3FOah&5eb2MX*%ClzI zr=tzjSMM$WrA}qt$t-X`KRx{_+fkl+_Z{KZm741tUo4(_ex2CL-#+SY@$|X*r;(j! zr?~1povq)Ma!O>E^i}LPm6XhDB7?j0j6vyRNo%Ksh2I{l&Tr@cl%sj}vzMW;Ms>^w z9&dP1s*5)F8uMDS6!G!BNv}o!!FaUY9Q4 zOi9k)Xt-w1TTUfqo3D^@Lfm>4p#Z;V-eM)sZ;f8Sn5t!+miLZUD0+5&|Cwoi8~mp4 ze4xX!YIwh1+-;l@Qs(Q9mR#1gkT<&2>Ak~tRs20Z^6B=f?3?Jn*SIJ)afidzDe^(| z_rzol0pZ*(U`ci)-9`Zf0@VTvRW@)Gv@#$+mn9DN69@;L27Q7SFI ziIpiVZV$U%lm7FWJ+a;)Z0C2T!4 zYejq&pqfT0aDF&Z)E3?wmOHCzi@P_+^q`BQUze7a2382jnAW80q*!qC(ego92ptSo zViv_XV&XFV=3H)@Po2#p|;;rosn7&CgxTN=&`Fw;QNiUcHs&?09X*P)^Qu zkDh4A&fF+!-2r6gP{-rwgTWi?a0BA`peE<+eWe^*#VcziqkK z5iA=#_D4XaqR2OfmCAj3c&W4ifijUq11bO6jK}`D{}7hVZWD5bT+_+k3WcNfoA6lc}c6ZbRe_8MMFO3K+CSxZ%P!t7(xa>tJG zR9N~iE_wOikZ@u@j;DXG6CxnY`u(|AHo9(0%`7bEcxhtU(K%HYp-*l}$1n#wdv)y} z!*BoLW#?R7JD+D_7yc#TuwXP~<2ovw?taQyq363qt zF*5sk5Hjmt#FAWix@uEkZ9PY)%tCc0F10it)8?sB`ft9+TU%dfT{el*ZV* z$k5wE-;|$EjEzw}Wu4bfVH1^h{OU)&OVKnJ0$OYyYtRxB@Aw<6vqi8z$xiR#dIJnJ z9Y}z2yDFMU(lk9_=jO7}R*rsifDGkQ)^b(MjP3kb+*R>nN7yt#;T|mZvPP4h9KXZD z1B9rVo7RV6dQ7m;!n%)2Vp>NAO(B@wtRpD4< zmSY-dZU*PHj$3OM#4n-}R7P0#!0<9Sy!v{W>6u=+mhdHE)m`LK{4pkB0P?EGeh3tfjf%=O&jt zoq$g!_g}CJa`V;P#47rR7VW?@M9%kqJfS*#;1{tM=cAcDMdM*japt`a5yLKErGQ?l zwck(5-3hpp#yUFWwBF*Yda@YG?38mRGZ-fpz?3eqepanX-EftEY}+#AOp3}LbxhjH zN9+5-{p{Jb1H}Qbm8nYkVPy2Y;3MV%2A&Hop@!GExQC7_AWS({{Gb=rn`k#$CS6R# zS62ts_D-~-bM^1tWV(NgdE%8zxJ8itIMX8Zl`_5b@Lqk2qii!r4zX2*wfmZdWV32r z0MdEE%zRleK4$M7)z3^miiv-pCdWAw`i}TJ)<+cEbl3}5J6Rnlsn&>wsgfoM74>}sq-MqI$QCfLV9S8)} zQMEi_ROadD+BZ%5v=2VV!ThhrD|(?q<@|8!6HBHJhIjNVZXD%=#g-KNtD9aR0O^@G zILsAl@R+{jfH3fOIMMI?ZhE5J0UlWo)7)1&5OtKhT;JroApTI#fC4K@^aJUi4Oq7F ze603>Gf(FAKg{81HPj=>^o$Cy@a02K^L;cY2Z=*Eoj?r`lE3hU*K92~5JK7wmH;xNF63fce zTY1E*yN+HqxFDY&ev^B%?!@j$H+H`iklFYA?IpiDZr!xydO9Ln*Zx*eR2~QeouR9$CQ$UmfH19*#!Ku+?3+<`ck=?akoD2Rh!qi>qTY*j^OdFpaidL zl{I035Hr8laNt@b@AdQ>FI=$)IT3Y}l@_dZ2ACZo?4;-Yy5jn!txM0A5V-4{S;TwY zSrr}goUY>UU*C%`%dAll0{?|Q+viz@Z@V&zPCx16|*>mvE3Z*x?osE#tt2*O@$35t!D zmCV9kR23MR8)bAG8%>L;mA&k^^9SYwh$gtcEuz=RyufB;SlKdvdRW=dr1KId-@T}S zp=jok`Ui~ShP#20%%@*`Ej2mQp~keE$#+sJ{5?au>jlP_Sp~_{^6W3^rawa!vZg|7 zb`k7o-I9*P2)HF%6uUE`HKkM(4^AIliwg zw}@R-o@;T^F3qfadOOk5*#qe~-3-wI~P5I-=K1GWB8?pb`ovUU7nT z;?8(cZW!kJM#ns5ADj}84Ss3aHu7{a z#^B59h3%~w{DkqPH}93g1YR$3Y3

<;A3o6$muR)|*FyLlg@b9{yo(G^EUK!Zw&2 zZqKRHk(M0657pe^{FnI#`&nr0j^2ar4$23bM+K|hs5woU8E|i3 ze%l`q*(A~|6GhG+-KP?O2S)t!GgQp zToi`*3$~TGkf|~Bx#3EmgJAZCWA{CH9ifD&sI>|=0XhdQ)uD+y8bJUrm)`rTvQ;M94yUQM5 zD47Xg$*nW3V5mrH7Tp(FD9p`bQ{?d1*frmj-RjMtE`D$3P`nPE+0p*v6lIKHfNG}7 zy;~mM2jYr5$vulFQ@A56|3&{_EQfTQhWthTU#NnN9e!{EGGtIiyW6^9TnKo#A1Hui zx{PurL4+&Hx>r;U#8uHwsH+|ZXg7@gZwGahGX{GDaPCJR;ILElM=uuT;0}<6{B~7V z#(M+I0Fp9rS%9Q86b^uZK`;Oq43Y&{96D(dP*{xZ1)KvG4FDZlF1XvGN$^7!f;eoF z7f5bPlKBdm5U8MBG|(6aN5bJShb998+W9H~@#A@NL*rM2yhRdfKto8*hpF}_HzCs$ za+>~Kk00ZC*boj$ilLhanyl5KRT<@uKIAh0RU0x>QE^1M{Zc?1^|K8I|Jcmm6$gO- zWNd0U5>~_D8~{LL4DJHX9rMd@IJGgxj^OA{5>gs0BZZJ5$wvPui46Hz;oybZd zAW#U&>Cc(qAP8v?kPLzxED!)HgOCD2WTc_w-9kZNDFh4#C1nzMHmH;=^l<9KJR+qu z-Ux>wqe%dmTxj(FLdd}6vHl)v-G3l5FnL)&O3~%tT#U@fAkv2+_%~ohN~Yfs)F?sk zdK-u%l(-}Qve%@yMWkv(pp%xm>qZ0L{M`Na57u;6 z*>({Wz#DC;X~3ul@)wRkG;YD;DwS_fd$FjH~j zbJJz+3VX%{SjfwYZ+_V^NmC=|6l==6t5u(Jpg3H%Gy*PX{E>1Ft-~SE25aORdMiYJXkBak?9m>f5*D#Q)f&s<{ z4S@Wp0LB0_FaQF8{HQSccs!|8{AQ;>O`JU*04AsGVUGL`psls_=0R3OU{v4{RS+2R z90CGCmZ|8}z2doORZ?Y*XK6etfK=8-QM2|%+k8+0 kS0QO=C}#8MGXA-9cLK_d;O$1*6ih}2N-r#|rmIf>AHWbMUH||9 literal 0 HcmV?d00001 diff --git a/frames_2025-11-27_16.25.48.gv b/frames_2025-11-27_16.25.48.gv new file mode 100644 index 00000000..545c92c6 --- /dev/null +++ b/frames_2025-11-27_16.25.48.gv @@ -0,0 +1 @@ +digraph G { "No tf data received" } \ No newline at end of file diff --git a/frames_2025-11-27_16.25.48.pdf b/frames_2025-11-27_16.25.48.pdf new file mode 100644 index 0000000000000000000000000000000000000000..bfdd0964c7f71fbd20609ea280a196b8713b5b18 GIT binary patch literal 6022 zcmcJTc|6qX+s7r@vrk!)v1Xse%otg-MM$>n%g|snW-JpzWXqnd5ZU)7L`5Qd_9)qo zG$mP6gzWrgw48HZ=lq`M`RAFL*Y`X3bzk3m`CQjr^Sa-{m((?-Akwn*!jG3KcIaV1 zFc5F=La(3z1nHu2P6THloMbYj2Lgd0O$?TR_8{G{C<0m??SQ|6rdLv;_au0rQLgkp z_x1F3bdEw;o)1)}I(TTEO}esLM{g)9TI?sGc;}I&xa4c`?TrlUn&{BbuN|^nnu!i( zr)MIrh4KuRH{#qqg&O=RKgK!BKmA&z3^Z?L2Y+C%R~ZqGvwq<&3>jzRd`dtd!^H%m z)zGQ{&nmH(rJyHnI%*Dv#ypJUhNj8>*##Pxtb4;G9S*)NALs*!e?T`Fl-8PC*b*3W zq#9qhCwp`U3rv_reGJ~CO>c=$Xc=R!G95^(*&h51aAALwf{-pj1x6P~YeVMXpR!nc09*w*5bJwJAWWC5Dk?$m#pfYfBKe;9I zTg^lLG<*o>jR+*|fC>s^1L*}qfk_P@NbMXD0t7jrNLP}gq>dj~kR~2S_=~rr6j%xj zl>HSEk`w9kb28-b$$%i^>uzWuNZ;PYh~RoS_0L$!{dy8~5ePZ#(8PFn5`eIu-~Z?M z${2y)N6kciqNJck94*aFxE(JTK`fOu$M79|3xSRRKKdN^lV-vh+;~RYKw!j71;u3%cg@IPo!04i|H;F z)zV_L+hh@LtEvyF7=p^{vYcweZd#!4nYZSaeR;(nRmbn-nDWV)5#(np3(lOB@%tWk zug34mmy(Tzk;->>sf6n0Qx zO|MOs!c8p$i8OZmiJGpKkLw?9znJ$vZX6KuqOZ*SsbuPg{_2Gzl8Yr3b+^RrCkszc zmGDAcM#}N2RaD9X`=jk~wvApL(3{QX6lGew?eJdig?1KZ7UeJ>?@0%V#nbpVlS2Fa z6#HP>OAovvhj&?W?g9-U(M~Gfx8|=G0E25Q1Kmz_?S<@xC_U?U=v^CA7#4^rZ_7^@ z7?$qdUT;z=>^(x=@=BuKbW6BOEyEDldsh2QH^ifFue!a@*XrtYuPgQefRW1(dkfnW zZ!alur3(_=$xq}wwY6faEzp_%FoRZJ$F3(kFTyABr zG)w7}Q8RqKxmzL!dWoZ|;NYukT5{XUvrEw5a-2Zl^SgH1w)SR-2n}dBsD2 zTsF$~#*T;71FCsNUyX7=Z!^_Oe9>4=Tlr#ytBk3zU2aWZJZDn6${5ahn>mKDN9}o& z`?HaHs>}E0LDJk=cQXq;&rVGZWjiZS?Y<}6yi{{_46wt)IQHS{UAW z&dpip*yA)@V;>I zM)IA4jd}|!-io8r4h4!CCnW4vk&1}(R?W5w{C4PhjJZbEDFvSx#p35@_n(^=v?6Z! z%?3Fwt4H)3#NEP~D9L`^(UH%Z+-E<&dWxcaR1V0sa92`{$v#%kSL=kl)q3^Q?i?Z> zZf17GOxF;>G2!qn17S-a#J9}8pme=UlinwMS1sV$9G`x#+P<0L2d(qs<99jSU83@1 zz9-({5D?Do0+nV*(QOn$p)ehgP*pugVGA=-k@Xo()QgieyY4GyO)+;tW{*=p6{pbP z8(EpcaVr!emBzTi;5yW(*xx?5K2I*qK>0O1F^+NHW>@jJ^8xGZsH#Zn@*)CYBCX z=!;6B4!i#cp6E9{*|f8k6c_0CrD6alm<4Y&^D$fzxvnc6-(TMS*ocVQcvOVfU+QwH zF};^w9vmG~1nf`D;ygMol9_rUC0&p>mzVK*gP8?u;n&Z3bYZX`G@X-vC7sPuTe2jP zf3~9Em2)Cp0uj4}Zd3(*kG#>KKRWw4dNKN|-rTiy5exC|D;nPG4=279-!Hh=#y)Iv z0hHFG6}VH9K=YD>3Er-fob7MfFyetFy(9vAn$d zBA?4>|I@GP8@8u2ChO$tc0DdK9eW}l;GM8`@#e|9u*Q~`T?#byPgyE|q-8<(TtATlvJyu>l?j8;I-|HEx zprW#~1;iWrO4`L%E(9m3GgvsudNS1aC~uZXOchDz7H1i*MIwtU2?%`!b?EY)Z9^8 zo=Tg5#U<~6>yj?)$MN(Z^g;!MS--#V&PLa6X;_Bm94|{OKQ^Q8CbZy@M2k7t*{f}v zi@5d1`-*Gzli578D-mB3=C(CZQDWl*1cUHm^N)r^`1!+^b7G4J{T5xnZ zj?vktA!MI5Sd&$BU!GM|+I}3FTZ~>x*qkEFbv<43j`ee_uXXb+8I|9&eb=yVlFHb- z$k5wE-&jyUydA5~ZI{BD1~vV>bNUs-tL$y>8kA95jGE0OoQLPsMTm7&+jy! zj})`?(E0S59up!ozwWD+nA)B}T?AqG=&;JX+6AAnI1{X&c}zx1=7I7}`bF{Lhd375 z<=A@a8zDI@V|Lnw@r$SgwPBV$2%;Q;sJ}D(lEAm-S{p^^+wqW|y4PnISmwK&Eto^)nicn#QaAquVy2r<2w8sA5x3 zK3d-w?q|<_GEfo-Uzw~@c&!vOEBJ_cfPv>+bC|IOC)cavib!*g6@S=y%|@Dy<_R}5 ziPhCXjlB~s=v>3J8%+0aGLH|rMOX(rjxjC5hE(ZgUhmZAyh3{fwl%J?gz5<$JbCVBd(d8w?&iG}hSJG<=0qT% zj%nlxqcTsu(7j>Sr+e@@4jynhUfCNJChw2a9A7ebGQMkIef<~!e!H|JK-2sj0YuNd z!C|FXgU9sU2@Hp9M-ctb?xrWoAK;bh;M)5t2V&0h7wa1R<|Q5&7*Sxwi2h*1Gl9#t zUXL~M0eO0-I0BCjw<74p*zKy_*k_g@m!vX-T^eXT+u4oZsW0&wzbo(9*Loc+(70o! z24mQ|GrttR=EctZYUe7$O`HHa%GfaM`@3kZT3YI^5JlEI(9gpH5zh7pnhj%ukHoVw z4OSlU>aU}hjm{|)MBL!otUa+i+>PBY17`NUcz40SmP3VOO36$3gswJ%lT-xLK}O4`y9tJ8nc=vVHK7x|JC z;8$LEp4EG(d6Mbvq_~&)LmgJzzIN+@%}r^5_m`^GjC*x?L#^Ip?&q0}I6}s@f)l(g zs%pZ6pqBnE5umjw-mB@?JKeDdIgzy!Ro1MvMwlHT?1b0-+LF4ZtqaeWkhrUWEMi)B zR%QDvz+K|QA8C=6nKdg?C3OW(k4(7$Sv4OV*&5{{1~!-Hk^ZF&fzGWXcJZ~PW7QTj zQqum_<2_kx4mE(RF^;yZs*i=7*o9V29t5ZRM`~FjGKS~0{tVCCO&@4Q(YZw<_l&5x z5;I$;59{J1=2aJ2M-defuU6+MUcq#4%Z1CdHi}>77RSS6^%16zg7D5QLGh9D z(rI|-!$K1)lZ$m{mON$PC z6~kOwm(M6+yc-n7e5&(nnc3-f4W`{pzLU}s9~jcz&oREtD!enL!2XhM>N89+Yci~6 z7s-xR8oIsFF~!)@0?7V0u{ca-6sDn~z zJ)prUDugI=vPc*4O^oS&ZbVWnvP39ZzqCCu5@Ev@&F+e93GNVA3_I1J)v#tApG-G{Ogzpgpj$z{|z!&8}#DZ zfGiEG06$&kgojCWQK6D1^Bv(z#K~RL@jP zyLrv$Ktg%vPS4`WWUfe?f6@OJ%OM@7p?{J87pfp*haa4P3>nnXo(>)uHv-<{2MQpW zE}~pX5aE)l{v~xI33ap!>hd)sv-mizg<;T z@jgIHpp+~^4k#r9Lja)=FdPVhfaQSJhfdlA6c*!f4(EhL1Hp%ubDj=p68w;ZA`hG7 z1(MrRWWGWs1ZpTZEi}f-nQ%DFp~;AVcD)Ql{&=3;(E8ONZ;^x=&`^@|VXFPfO~^Eb zoTh)*QFBIl{8B&{^|K9u{MgLj6$e88 zWNaEZ5>~_DoPZ!x4DKAx6Z6Y(IJGI}3c=ZvB%};PRvIZwl8+qZ|F1Vvmb|Y2{SB8x zNJHcxFffAT3`c;a!Ehv;v@!$?EDe!C$U@|Ra5)lVltYk5l#wM_A+iu88CZcyJCTz{ zLSayn)1NaTz);d4U|A$NSYRMb7AXye%F4jVyM=)v(nvTQM#?1eY%pm#*x}TNc|=NS zya^6NMw37YxzHH?g^)qWWBonU`u{*=5c0Bql%k8jxfq#|L1hj@@NdA1luW-Ns7Zps z)eta87;z6Ui`M0GPIS2|aHCk2%i$81_~NuG<7H0GMC3?dlls-puAx1o?CdG&V{~&T zxac^YHIJq_Bo&=A7kK(sGjh6J9z#jdZAs+zzZsO>G8-cfKYju>_H{Nk>GG0LzNV;8 zCBRuXIu+PpA)jWQ*!O72zu|jp`vq@1e^hh}k_dR{H?jSIIya=`NH6l4gH z!8_~^&f8$C1vIrTXjd40GJd@KB@BC;hS}P2fCuv?KMXdepfG?l7!$5{H{CR?oJtNX z`SR|i^?RF_kpV|Q*Ku$Er8xbfmE__ii~RlH@CzaS`C~LQME|HbKiQ$I+3ev_o;(-uy${yy(?*Ka3*=-(VMFvF&9Z?5E zpf8|cFm(ASou+p@Cr!L?AJtSQC7Z62it6<7h`Y2xRJkhZtnn-j#{`sAbx~BTeKGc* mR3HzPsHrKY^XM}ExpPkf%7ftJLE02tRu)DtEUclgN&g?Vx+D$& literal 0 HcmV?d00001 diff --git a/frames_2025-11-27_16.27.10.gv b/frames_2025-11-27_16.27.10.gv new file mode 100644 index 00000000..545c92c6 --- /dev/null +++ b/frames_2025-11-27_16.27.10.gv @@ -0,0 +1 @@ +digraph G { "No tf data received" } \ No newline at end of file diff --git a/frames_2025-11-27_16.27.10.pdf b/frames_2025-11-27_16.27.10.pdf new file mode 100644 index 0000000000000000000000000000000000000000..83314931a71914c6661dc6910dee0c9c140f8c79 GIT binary patch literal 6021 zcmcJTc|6qX+s7p-OLj#igDBhVGsZ4kWXl$^4h=?Q#xjA=&~EB$Ey;0000fV{t@`6X}ja6ETVyE5c0-t&9wJqQocd?(2Sg(1lO6O(hGzJlNtb!!esyi0J1`pt|UcC9Y3xhWdfe~7jJ7(uqYTH z_A4SJC(`HVWT@Yh0YG}U9WVfpmZhC8(f)AipRpAG^(06W06FYX#yU9@0q~#S|L6F^ zbip6Pro%o{Qc$CgnEq1)^5{QBfFOS@3j+Dex_&ML0{y*?f6FPdESS#>~3KU7kk>0Bdj^9JU=(d+D@uf05BRXjGb#9LtCkP9!bcvS6++3+nOnS)WD9Kr*S8QS_ zGec>URxxn9zD+m_dX23t_w?=L?~Zw+efLE_(x*$lxl#7H*g|FqrQ=$Xs4zQf*L#-s zq*$1__l}e3W2!l652a%Ej(Vz(a@3}{Ei9Z-l?iCeF#x9y#jl;==l^h`Jhzc&K1=oD7dJhAmGa2X zXWfzhNe=3qYm94k<}3~JpJj{|`>%&|oisjWP;-ZEd&ruW)i32$2?}-R9SVn4isGO2CSucOlmNia5 z7#bciRM$bWjaq$6Lz>h2aWAvZ%iL~NrgabARq%0{<<{y@*f-Sqq;f@Q+)SO59+(*A-SFb;~B*|>@QpCzt z^|^qjG+b3b5y^ub7qmil1ZPhvTH#Zs86LK>b!ixeZdh%jf{8f90XIr zQk`K*)IksaAdGuXjn(a}#Yg*kel6+7^JO4j8@lVP2;5c&#&i|8J<}aWZ#>N>Xf3wd zRT$h)E%px&$Om-AWpErF5lBxym6XaiKAV&FWrL9kcf+%bEjl=rg`V|J$P-yTh1o<$elOOjOaC_-_oYoVz8)e+F8c%EB0-t=dS&vR+5}*-FK@9xoG#F3D(xB10@tXFoP-2`W?Hzw|HBB?c5O}L zgPtQGte>C}!9hTtC8sAr{gsM##lfB}@l1k#d4YAUp<7s7U!4ExkCZO~CJtsQ6K8PI ztCBl=uB~yrA^aztv0m8DlHG-ir3D!kRpa~VG<(%=MMWiV9$8D0wZraXle33T zoGmr;Sy*)Qxh-PHdXhlTPCJ*OUXDm>ts<}@v&(|hYRyg@h7kcJA0Ll zvmtlix!trce?F6Acr)Z{?CiD@I!tin;M`!g?L?ArcL}|CrR?5B`k3k2QPz>#;Akc` z4$NeY%(|QXV}rWQiH?gYe4YVbijO7?E~QJNpR+{Y9Ts@wL#uXOE{%WufyPSfbU5G8 zax}fIdriPTv%fO)rF}`}OF+vJRQ4mxV(jJwakllvqFbb=b#QT zKcntg1gE2{p(u6jLsm`>OLh5(_XjdT9P(<8vguLHvjwfCZ+7^Ne5F$mQJN~X+LAmr za}QC1#!hOV`)IKN7v|PI6ylPb(x~$xtWM1)>9<-D(>E^qYo#B9iNYSs-l1I(dh`U( zB(@w`P3;|!)i7+KnisQxj#U_7+JhjAk;w8}!A9phBQ6tuObb?m-fagY2dVVe%aqn>e#Oqz7uFDu^5Y+TqloBp&{d(n(?498rQ{e60L z(bW$0Vs!rvD<3C!g*Q&tGw9I{GEG3DWA>C{-+@=?UbKg5<`}h$1;zPK8sk`2UUO;8 z;&bc$`0O3ud&$g$y>{#Mo{Fan;EZ-z=hFl5LcR>Cyz3X0YL)d?d4{&l0?#KZ>`_G~ zpMJW&&)>zG`MkTp7qK!{CetBV@k?v9L(HR zTA3?G&~Z6!D6LiQyPhWq2%a=0qn_nS#H@1Hs>$w!&e-YMPW6k3MoYV~z$4GKk|_0i zBEqqExAZExrYkxw1Vt6(NC30z0pp^sFVP5}>oKx!=paciyz0)vK=z+C9#_tCwV}SsY0mk%N+>XN@`Zsdd>+y*4CxK366V#buRiY zo4YDyXTM2WE!E3IpjJ-Rz=|0JW#F~=6foX zk*>Y+luK(Jv#fhrDmTQNbF=c)?m!!EzX*`t+4AA4S0$%b@^Tf;xXkoN!p=(aQP15? zI`-(oO=ouBya{IZaoX-F!bvN4fqlJZvDB4PeXqu_Qf}nx5SROKgzU@tFAD+_1BLvydM*CHF~1PY~I;q+P%36WOw^owwiXoDyRRo+pyymMqRdm;Vu7I zw;N>@!G2IoeQ{8*9VmaRf}6!UoE2Wx7ahr zQ`$00n`YP@g+Kj~5^9`Yu_9Vfm230VfRjC=;*&K?t$0ZH=JFiMtB}su_Vu7eOl9G4 z`Hg8&pjY`wd&Zho1$)LYTVqDqWF7}@{9g-Q-JvD{MH z&|q=l6ynX3Jbe@Wv^E3%2_dE8w@vrnVLyYZ1FBw!cIcbrSq}8co8?aQ$$J?#UuEY` z3HR-dU@WY9NH46n>lem&?#)t>;rS*dhTU}T)4-5Vbg7P)>ECALB}_=MzNMM?0+-Gh z3#!;fv0`NUqc)l+=o?yXs>*QHX-Pq(65E+S`h=Jgn$(WcfGlGL^ynYiE*Jtoj0Wj8 zQ_8IS)YyatjKfant0TV&GCar*iH}4TTu9U^Y>Ep-nz4kl+M^o$n}wu<&ef{_ zAsW%=-6%KovOOarU$XX$2}kjut#GJ3eRYIoA#dJ8D<^sZj74_G8`h3qrHX2=;He z;qABmZh_5l>g90(iDUamd69mh|2z#I9`b%32Vrm&LmZj$$k7*;b_&O;P;u$_fqol z-7Z+wt}~}W?9_A0H~m)`0wG)q`3D2Dg5pkyd1GpUD3$APX_9RX(Zx7nt$#ZxqwTS{+ki_y`T&QWfAl)PRAKoDWm&Pi{h{ zDdaT$yBG{;M`*rlMercKW4&I{IfD0{O9-zbg)a z{K?pq@Fc8;#oGWt23Y)MyfgNf;c#jL>`kJrGf7AoL=1=$Bgsb&^8eQxB}QJ?|NcgZ zBY_Zc2po(gIU|r@AQ*u{kXD9-gMknjQVb#vK!}qVqd1a0B20{Ag@{2=WMBm*?L-`i zg2JIBr$1*xf}x~Az+xzJu)qMg7zzl6ioxLI-NL~TAPRwilQM}s8yqMOKb-n7k4Py^ z(8puRXc7P+7aEmP^=LSEL7Ql$Bti;)=_6m}Sbe*;#eWcm$3^<%s2 z8o^9~OCDh_5+vAgs#~yzsTeAG!JwF_>mE$y4phz1IFpphrS?$WupYFmH3Ft`2lk}? z0gMK6T3rNU`qofC3uP`DFp+tNmE+Dt*WeNEn2)FlXF21cN@oAZLyavBFRO=I`1;#D zj>m>WXIg;}SF}FcQTv*C;Pv45Th`DQ(vy*4gU=+{PnM5ebrL@<4{Y`a`hNNhn~Cq8 z0-Z{4m009X5EHy|6?w-^*Z7&EEL&24EPrrjqSY)iPRinY&r| zT04cLnl+cE(vVkTlTY~v-g+6^N9}0%Cy1})(3EWaL8Z=}e(}b;l~d9^pxvTMeQoa- z(;uRIKAJ@-*748d`~Rgj{eqR`+9W3S`>){_LHzT#XsCnvQE+~uLoxCH8U=Du(8YRS z0MH)=zyM$j0YCxJ9|cB-Kp<6$-{=&iink^JAmofaOp)L5v$C+*Jje+33->#s2!=pg zpkOd``6!LDTMWnX82(PGiF8U9bs0JNsgoi1PF$eMmQ!SoVX8UCDHq)$ literal 0 HcmV?d00001 diff --git a/frames_2025-12-02_19.18.18.gv b/frames_2025-12-02_19.18.18.gv new file mode 100644 index 00000000..545c92c6 --- /dev/null +++ b/frames_2025-12-02_19.18.18.gv @@ -0,0 +1 @@ +digraph G { "No tf data received" } \ No newline at end of file diff --git a/frames_2025-12-02_19.18.18.pdf b/frames_2025-12-02_19.18.18.pdf new file mode 100644 index 0000000000000000000000000000000000000000..4f66c84365103b97b7c874b39fca597bd518fb96 GIT binary patch literal 6022 zcmcJTc|6qX+sB3M*%cvUE$b|1#>kp2vSkY)h6W>MEW^l}Eqj)P$evw@L?V0Hx3V2+ z3R#jOWal@d<(%_6=l49%KhMm(zTdg8`}*F?=ep*a*Zmg0tfnCemXe_pu30YMrGo-M z0IaPOoxD5%sDr{d;2Z%kqREgB0000r(9SrNJMr#}#G%wscG&AEIz>e~51cy+=|boI zKv!Q|n*z-IVxS_`&Rz5T-Ro<0bcW~775R!QBt5YZlNc7;+03x2jt&j|+9AWGk!WXj zW-h`rlzXtO5##0|)Zj-x6Xz)R>}#bGz`T_W^ntBjc~m&g>ZO}7c!HJl84ivJ7Zr$B zMX7LjREqYO0H3;QtJ)bFb2CgBnkM^Y=c`|~>IsvuJN&kCsLw(81F}i4xZd2tnn0f; z**M&u?A{eDFliPw6TDB8-V&eCGR{IDE80JK9AuS7+O9Y3x>4J-!t7jJt>kR%8o z^D81mC*tSlWQgCB0f5FgT~Pp_zO9oH&gE$8pRttv^(0Ua06yx_K)ZY30MMV`|L6Eh z8-d?+& zJ*P@wrWSz&YMX;Z4Ht`=`o}vj7x5>I143T*mYP44NZr(5yL3!qskprEwwUcy!I|k| zUWn6Z88)?&Qc2)otUb=U(W?V;tJ$2aRCBK#)^qw(J2MlrQkXY>%1(Ug4ED{G&;dW$ z0f^@EBd^GjJ?5Ny00VHegEId1;>~O(Tfta$khY15C zQe8V6O^O9Q$EaF{#OqDBg)3Dv3;{jowa#{d-Fx?|+IxL0Z_M_%I6vZG;4*Z+?c5!2 zDb6CC`CpTNnpvdP5QsW#K>>(d$N(H&2~ii@uHNmj&PU#(A0dC2P~$5m=vW@)Q9 zPwtRWJ#w?TOFRd1nY}Xq)XmiIZUvLW_a)ymWXip|TKTceMsW;b=vAJqx-jW9#7%cX zCd%4>*In`v<)VU*dKpJgGv#W0;doA4*;0gyw5hO7Zgp=w=iPMWag5^*Qw&45>We0~ z=cDzMSMDzYrA}wv%PjCXKRx|A+fkl!?>+9;C?yjE}7Jf%lmEXqyDM#zxG* zZf|&SimMLSI@5ZSHEWB?M@94H*HJ@c614s<1gv? zCnx1^)?c;cEvJyO%U8%aDQ>fdP=H^wY_^u?w?QqU&DFDb}JCI`8m3)d0^0KK&lm12e-9nis_;?y%X{BVM(9lR$zcUH{~lQzfru#>${pPHHqRtU(L)}rkoTXgr)_CZ()9S&Ba zm&Dj(<3=jcnUQ#_+sRf8p(6?%6)vB4=a!7puZaiTHQD^^eB8uu762*!9 z^W}XmoRjI|@YrQkqYChQq<@3{*!<_{rRcA^3!WPySH-%ntK&BwPkt$Skbl38ZN%gf zFs)lNaQzx#=X3JL($h^IL#CcI! z_?*W2o_$r@v_6wDRVQ1w=dQdS2gAgx=;5-@~)jD zlU2Hs<Z54}87fR(8Wf`tVA_~{a?R*{>d<2eu+xDo% zU90yz7y*@vD!w(YROvOqN~H!0l!+u7Nd?Si)bKCyPAV#&KM$at6E6;aH$l16*wFT1 z@E8ysBx;Uz6;WZ$8%)x9p=nTkxbKKNon%r~Y~N^xk4hX)3_kUq{5inV^_u4NY3H~# zx!rxQ&P0I-;nkpoQfh0oAG)`EZ|Q7BaaMI5;UJTCzrJ5mQttY(^%Ny1^Z`0GcZ`O+ z;#$DcGCts@gcI8dEZqm)Pyu09Hi}0KirHN(7=hR$u^fJAue8LMD`=ffkEa2 z2{0}enQ!#Gv6-IOx#bSxBiR=nC3HV;%tgWb{7Y&4MLcUXLg z5VdgE{y0pB4iQ@1@KH@nZO@=81hct!SZ3bngw0(&8?2vsTv}54k9$pfQyA+B+(eZ_;2%-!slm{_N z_RL}1>-xvXk|}PdD@Vj`JhWcvk6!$pq8K7 zAKcB|4ZN4iGCJh6(d?^ssu;@TlyfFC1S1y6m@cq!R=rWfc#VH-=UV8QWYvAj*wj-` zHV%aQ*s`Av6bHgqrz+)#6=UWFpD+#3b6;o1rmv zwl=7~f3gLYYnbNG_}~`P#B0|Gt6=+a#wF-$6*}qR{kmjF*(UZJLW?R(*HtUYCbilG zOp&mH{@~~E+zp9v$yTv4dya=W&5Sb=B~uJFwj2gtq1^&I>YmJ@xgIzhGq44XzBS7|q27eKgUqM*OrDoI{${;8ho9qM0axOc@W?PZKa9r2vblrtJp-$o$2nkkN{RzC%rD@8bWEG< zmI~EabZ=5%ICv+5;CFs6JyGrut5^rqI#50ob(GVqYxG?de`H`p<}6C^0~wwTT(S15 zQGdvhr^~}0NHNk1rxRtfsd8nTTLE8|%nWvFpz&yDGk&MG%xnCvtm8m)I9Q-@*Gv^k zzn!$W9KPsq02O6y820^Lv}P?0RcD9-OA_Ssh(Ls+?V(1)xZo49tW1N| zC%pO_s1>6N^7#?|TwAp#_eQ#$4@v=z21Ky;w$IZg6A~(z>!L z+UGgk#6SFz7HN@Ly((E;m+$bzl#3&)`hz`dqin>$*2*HnuY^9(v31lYzP4n%>gt@N zlwZ|EchAAq!jh3&MpRs} znYF`*4Y5)4N+HqK8Sig9ahcXe@k`%kf1Io~$~Yqkd$lbnHdsHzy01D^hXd?NL_1WkBMc0?Z~jowfyN}6+g3%OB{S@(SbuT zOeJ*>8N`kEf})ssUVSY!JJYVtxR=RyN-E+5eY)EPhW@OAq-lA!e%k5JP=&0iu+QY#tcP{OJ-8QDb86V_E+>J#P3nlB9v?oTwudzn6xgc7CJH!;icp5Z2R*%W?eQmx) z=%nykkCS$7V%ay?jgihCNN@H>ElA?rvE)9!9j(TKK%6bI)XawFTd-8`fXB+-v($^1 zW@SP8EFQEz7-o-`JKp_J9_{9rJD~J{2{C)!Rvm5ffwK0Pb+b%^JBv^Wg_Ey2z`Ai~ z@Z_7uxxO*6&)Ej2#P9GU?VJ$>CORecmrsoA#jw6UgO zJ&nS)mJEK}`10Ei%HaZkEOTn_+s5U^CXW>eG|JXlMu9_>3g{obu{Rk~VKZYL%#E<; z&}~mmisXkH^4UqB4lYV@w*0Exb^H96`Fi_V=$&1Ihh6RD54Da9R=HDhm^0C1-t~Vw z7!cVaP^}O|&L2OZ5P%0o{_`|=bjbU89F&G4z`qZKf1Zke9rFNS5_kB&K?W;>9!wj6 zxnYgNSBEL#@!h(pPzjU8j&MceT0UhPw*;pckmA#e-X-2qRu zR^RDUKn|)!jqAZ{jG}E`pzU-hj^R>byBk9xE8AmZ_R{ z%hUT%TxmC{d+Ag%SLC&S(f=3AAs(k8f06$esvu#9ADn;$8Prf7cJ63b9M=5@3Lu*F zkS;`sa9KtFvYL^&8p;WI#nTAoj<)~ppn-HjJKqGH`_TtD>Jx^{p07yfAyQ-*Q zy#W>gNg22-KvEhC2SC6e7yt|g$pWm7oV0LAXSCe~jDs@@06MZ<@UTM>;fE{)anvL& zkkpnW@f8vwP(`|GqRWFmzrGO6dXB!Ouv6;Us4gmkj z*wisZtcJ!o0Dz`w%ms`G`j_EoYE$%eoTCR(NNKQ)6hek5A1TQHUvGpAXtm0m}klvP8xx3nz^zEkm?|Wxxm$umTZxA}fV} zKp{k@KWBo2AjCmHG6+(zKme!=LJ9SWVq>n=IZ@`L}Our$hNrJ(R zRuFp_VOPArClxg~cO@f(o$m?2jzP_z+m{Ai6gHG3#JfiLT2OO`h30LXhetm@CCiX> zvOu@8c9dh-wi64-@!_?a*P2XKqf+GYFJSlDs39~wtFqY{ixx~#@O0#ijqZrJ3)raY zF;&v#OqyP)cX&)j0mY+7pQU2mBOiP6#0{89`rG|s=o4%HtY7V^(2IpI_wFpUgL7)~ z_xCzlDJ;VGL!O+UrRkztDKFK)^fA5S@T;=CRris-enSFp_IAVf#tx%8%WY8H8H$X- z$l`mCmV#62^|m;8?Du5}#0t|ovAU`UMAr?z46~EajPzz&^1%OE@dz2%;188Ssvvrsi85A9KObrBv zyo7*2kQEAA4SYN&b-Zvd<#Z-FtB#_w%It}Vdo)6nxyouR@yrd!1r$|ukd!RFF}5F- l!H*TGsK{pXXfyt~a}ONS9p~*%+!Rbk21+L^tgf#?_a7qEC&T~% literal 0 HcmV?d00001 diff --git a/rmcs_ws/src/rmcs_bringup/config/flight.yaml b/rmcs_ws/src/rmcs_bringup/config/flight.yaml index 151db7db..56a182e7 100644 --- a/rmcs_ws/src/rmcs_bringup/config/flight.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/flight.yaml @@ -7,7 +7,7 @@ rmcs_executor: - rmcs_core::controller::gimbal::SimpleGimbalController -> gimbal_controller - rmcs_core::controller::pid::ErrorPidController -> yaw_angle_pid_controller - - rmcs_core::controller::pid::ErrorPidController -> yaw_velocity_pid_controller + #- rmcs_core::controller::pid::PidController -> yaw_velocity_pid_controller - rmcs_core::controller::pid::ErrorPidController -> pitch_angle_pid_controller - rmcs_core::controller::shooting::FrictionWheelController -> friction_wheel_controller @@ -17,8 +17,6 @@ rmcs_executor: - rmcs_core::controller::pid::PidController -> right_friction_velocity_pid_controller - rmcs_core::controller::pid::PidController -> bullet_feeder_velocity_pid_controller - # - rmcs_core::broadcaster::TfBroadcaster -> tf_broadcaster - - rmcs_core::referee::command::Interaction -> referee_interaction #- rmcs_core::referee::command::interaction::Ui -> referee_ui #- rmcs_core::referee::app::ui::Flight -> referee_ui_flight @@ -29,10 +27,19 @@ rmcs_executor: # - rmcs_auto_aim::AutoAimController -> auto_aim_controller - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster + - rmcs_core::broadcaster::TfBroadcaster -> tf_broadcaster value_broadcaster: ros__parameters: forward_list: + - /gimbal/pitch/angle + - /gimbal/pitch/velocity + - /gimbal/pitch/torque + - /gimbal/pitch/control_torque + - /gimbal/pitch/control_angle_error + - /gimbal/pitch/control_velocity + - /gimbal/pitch/velocity_imu + - /gimbal/yaw/angle - /gimbal/yaw/velocity - /gimbal/yaw/torque @@ -40,11 +47,15 @@ value_broadcaster: - /gimbal/yaw/control_angle_error - /gimbal/yaw/control_velocity +tf_broadcaster: + ros__parameters: + tf: /tf + flight_hardware: ros__parameters: usb_pid: -1 - yaw_motor_zero_point: 5450 - pitch_motor_zero_point: 30473 + yaw_motor_zero_point: 5807 + pitch_motor_zero_point: 29303 referee_status: ros__parameters: @@ -52,24 +63,16 @@ referee_status: gimbal_controller: ros__parameters: - upper_limit: -0.249 - lower_limit: 0.249 - yaw_upper_limit: -180.0 - yaw_lower_limit: 180.0 + upper_limit: -0.349 + lower_limit: 0.349 + yaw_upper_limit: -0.5 + yaw_lower_limit: 0.5 yaw_angle_pid_controller: ros__parameters: measurement: /gimbal/yaw/control_angle_error control: /gimbal/yaw/control_velocity - kp: 1.0 - ki: 0.0 - kd: 0.0 - -yaw_velocity_pid_controller: - ros__parameters: - measurement: /gimbal/yaw/control_velocity - control: /gimbal/yaw/control_torque - kp: 1.0 + kp: 0.5 ki: 0.0 kd: 0.0 @@ -77,7 +80,7 @@ pitch_angle_pid_controller: ros__parameters: measurement: /gimbal/pitch/control_angle_error control: /gimbal/pitch/control_velocity - kp: 16.0 + kp: 6.0 ki: 0.0 kd: 0.0 diff --git a/rmcs_ws/src/rmcs_core/plugins.xml b/rmcs_ws/src/rmcs_core/plugins.xml index 4d3bb141..02efadb1 100644 --- a/rmcs_ws/src/rmcs_core/plugins.xml +++ b/rmcs_ws/src/rmcs_core/plugins.xml @@ -71,6 +71,9 @@ Test plugin. + + Dynamic TF publisher for gimbal coordinate frames + Test plugin. diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/simple_gimbal_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/simple_gimbal_controller.cpp index 19bdbaa8..925db272 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/simple_gimbal_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/simple_gimbal_controller.cpp @@ -62,14 +62,15 @@ class SimpleGimbalController if (!two_axis_gimbal_solver.enabled()) return two_axis_gimbal_solver.update(TwoAxisGimbalSolver::SetToLevel()); - constexpr double joystick_sensitivity = 0.06; + constexpr double joystick_sensitivity = 0.006; constexpr double mouse_sensitivity = 0.5; double yaw_shift = joystick_sensitivity * joystick_left_->y() + mouse_sensitivity * mouse_velocity_->y(); - RCLCPP_INFO(get_logger(), "%lf",yaw_shift); + // RCLCPP_INFO(get_logger(), "%lf",yaw_shift); double pitch_shift = -joystick_sensitivity * joystick_left_->x() - mouse_sensitivity * mouse_velocity_->x(); + RCLCPP_INFO(get_logger(),"%lf",pitch_shift); return two_axis_gimbal_solver.update( TwoAxisGimbalSolver::SetControlShift(yaw_shift, pitch_shift)); diff --git a/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp b/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp index 96c836a8..ec4c6a8b 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp @@ -42,7 +42,8 @@ class Flight , transmit_buffer_(*this, 32) , event_thread_([this]() { handle_events(); }) { gimbal_yaw_motor_.configure( - device::LkMotor::Config{device::LkMotor::Type::MHF7015}.set_encoder_zero_point( + device::LkMotor::Config{device::LkMotor::Type::MHF7015} + .set_encoder_zero_point( static_cast(get_parameter("yaw_motor_zero_point").as_int()))); gimbal_pitch_motor_.configure( device::LkMotor::Config{device::LkMotor::Type::MG4010E_I10}.set_encoder_zero_point( @@ -81,7 +82,7 @@ class Flight constexpr double gimbal_center_x = 0; constexpr double gimbal_center_y = 0; - constexpr double gimbal_center_z = 0.20552; + constexpr double gimbal_center_z = -0.20552; tf_->set_transform( Eigen::Translation3d{gimbal_center_x, gimbal_center_y, gimbal_center_z}); @@ -120,8 +121,8 @@ class Flight transmit_buffer_.add_can2_transmission(0x144, gimbal_pitch_motor_.generate_command()); can_commands[0] = gimbal_bullet_feeder_.generate_command(); - can_commands[1] = gimbal_left_friction_.generate_command(); - can_commands[2] = gimbal_right_friction_.generate_command(); + can_commands[1] = gimbal_right_friction_.generate_command(); + can_commands[2] = gimbal_left_friction_.generate_command(); can_commands[3] = 0; transmit_buffer_.add_can2_transmission(0x200, std::bit_cast(can_commands)); } @@ -140,8 +141,8 @@ class Flight gimbal_right_friction_.update_status(); } - void update_imu() { - bmi088_.update_status(); + void update_imu() { + bmi088_.update_status(); Eigen::Quaterniond gimbal_imu_pose{bmi088_.q0(), bmi088_.q1(), bmi088_.q2(), bmi088_.q3()}; tf_->set_transform( gimbal_imu_pose.conjugate()); @@ -177,9 +178,9 @@ class Flight return; if (can_id == 0x201) { gimbal_bullet_feeder_.store_status(can_data); - } else if (can_id == 0x202) { - gimbal_left_friction_.store_status(can_data); } else if (can_id == 0x203) { + gimbal_left_friction_.store_status(can_data); + } else if (can_id == 0x202) { gimbal_right_friction_.store_status(can_data); } else if (can_id == 0x144) { gimbal_pitch_motor_.store_status(can_data); @@ -227,6 +228,7 @@ class Flight OutputInterface gimbal_yaw_velocity_imu_; OutputInterface gimbal_pitch_velocity_imu_; + OutputInterface tf_; From b50b39f62f41a90c3db81af12a25276d1fec5fc5 Mon Sep 17 00:00:00 2001 From: 1Feishu <1291113686@qq.com> Date: Wed, 3 Dec 2025 17:56:49 +0800 Subject: [PATCH 4/7] update librmcs submodule pointer --- rmcs_ws/src/rmcs_core/librmcs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rmcs_ws/src/rmcs_core/librmcs b/rmcs_ws/src/rmcs_core/librmcs index b82f2eaf..80444ec9 160000 --- a/rmcs_ws/src/rmcs_core/librmcs +++ b/rmcs_ws/src/rmcs_core/librmcs @@ -1 +1 @@ -Subproject commit b82f2eafd21371a23d046e5f75884fe6c6e49124 +Subproject commit 80444ec9251a83e31bc153fe06cf91ddb5eece37 From c2e3934076e8f51c5614469d983d7105acc089c3 Mon Sep 17 00:00:00 2001 From: 1Feishu <1291113686@qq.com> Date: Tue, 16 Dec 2025 19:30:07 +0800 Subject: [PATCH 5/7] add MHF7015 --- rmcs_ws/src/rmcs_bringup/config/flight.yaml | 38 +++- rmcs_ws/src/rmcs_core/plugins.xml | 3 + .../gimbal/GimbalFrequencySweep.cpp | 197 ++++++++++++++++++ .../gimbal/simple_gimbal_controller.cpp | 2 - .../gimbal/two_axis_gimbal_solver.hpp | 23 +- rmcs_ws/src/rmcs_core/src/hardware/flight.cpp | 25 ++- 6 files changed, 260 insertions(+), 28 deletions(-) create mode 100644 rmcs_ws/src/rmcs_core/src/controller/gimbal/GimbalFrequencySweep.cpp diff --git a/rmcs_ws/src/rmcs_bringup/config/flight.yaml b/rmcs_ws/src/rmcs_bringup/config/flight.yaml index 56a182e7..cf8ef4eb 100644 --- a/rmcs_ws/src/rmcs_bringup/config/flight.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/flight.yaml @@ -7,8 +7,10 @@ rmcs_executor: - rmcs_core::controller::gimbal::SimpleGimbalController -> gimbal_controller - rmcs_core::controller::pid::ErrorPidController -> yaw_angle_pid_controller - #- rmcs_core::controller::pid::PidController -> yaw_velocity_pid_controller + - rmcs_core::controller::pid::PidController -> yaw_velocity_pid_controller - rmcs_core::controller::pid::ErrorPidController -> pitch_angle_pid_controller + #- rmcs_core::controller::gimbal::GimbalFrequencySweep -> gimbal_frequency + - rmcs_core::controller::shooting::FrictionWheelController -> friction_wheel_controller - rmcs_core::controller::shooting::HeatController -> heat_controller @@ -54,8 +56,8 @@ tf_broadcaster: flight_hardware: ros__parameters: usb_pid: -1 - yaw_motor_zero_point: 5807 - pitch_motor_zero_point: 29303 + yaw_motor_zero_point: 4608 + pitch_motor_zero_point: 51629 referee_status: ros__parameters: @@ -63,27 +65,43 @@ referee_status: gimbal_controller: ros__parameters: - upper_limit: -0.349 - lower_limit: 0.349 - yaw_upper_limit: -0.5 - yaw_lower_limit: 0.5 + upper_limit: -0.149 + lower_limit: 0.149 + yaw_upper_limit: -0.349 + yaw_lower_limit: 0.349 yaw_angle_pid_controller: ros__parameters: measurement: /gimbal/yaw/control_angle_error control: /gimbal/yaw/control_velocity - kp: 0.5 + kp: 8.0 ki: 0.0 - kd: 0.0 + kd: 0.0003 + +yaw_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/yaw/velocity_imu + setpoint: /gimbal/yaw/control_velocity + control: /gimbal/yaw/control_torque + kp: 1.5 + ki: 0.0 + kd: 0.01 pitch_angle_pid_controller: ros__parameters: measurement: /gimbal/pitch/control_angle_error control: /gimbal/pitch/control_velocity - kp: 6.0 + kp: 16.0 ki: 0.0 kd: 0.0 +# gimbal_frequency: +# ros__parameters: +# F_start: 5 +# F_end: 50 +# Repeat_time: 20 + + friction_wheel_controller: ros__parameters: friction_wheels: diff --git a/rmcs_ws/src/rmcs_core/plugins.xml b/rmcs_ws/src/rmcs_core/plugins.xml index 02efadb1..3a54e23b 100644 --- a/rmcs_ws/src/rmcs_core/plugins.xml +++ b/rmcs_ws/src/rmcs_core/plugins.xml @@ -38,6 +38,9 @@ Test plugin. + + Test plugin. + Test plugin. diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/GimbalFrequencySweep.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/GimbalFrequencySweep.cpp new file mode 100644 index 00000000..701d3ddc --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/GimbalFrequencySweep.cpp @@ -0,0 +1,197 @@ +// #include +// #include +// #include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include // 用于时间格式化 +#include // 添加文件流支持 +#include // 添加格式化支持 +#include // 用于创建目录 + +namespace rmcs_core::controller::gimbal { + +using namespace rmcs_description; + +class GimbalFrequencySweep + : public rmcs_executor::Component + , public rclcpp::Node { +public: + GimbalFrequencySweep() + : Node("gimbal_frequency_sweep") { + // 创建数据存储目录 + create_data_directory(); + // 初始化CSV文件 + init_csv_file(); + + register_input("/remote/switch/right", switch_right_); + + register_input("/remote/switch/left", switch_left_); + register_input("/gimbal/yaw/velocity", yaw_velocity_); + register_input("/gimbal/yaw/max_torque", yaw_max_torque_); + + register_output("/gimbal/yaw/control_torque", yaw_control_torque_); + + // 参数配置 + } + + ~GimbalFrequencySweep() { + if (csv_file_.is_open()) { + csv_file_.close(); + RCLCPP_INFO(get_logger(), "CSV file closed: %s", filename_.c_str()); + } + } + + void update() override { + using namespace rmcs_msgs; + + auto switch_right = *switch_right_; + auto switch_left = *switch_left_; + + do { + if (switch_left == Switch::DOWN && switch_right == Switch::MIDDLE) { + + Now_time = this->now(); + if (f_now > F_end) { + break; + } + if (last_time <= 0.0002) { + last_time = Now_time.seconds(); + } + now_time = Now_time.seconds(); + + current = Top * std::sin(2 * Pi * f_now * (now_time - last_time)); + double torque_command = current * k; + *yaw_control_torque_ = torque_command; + if ((now_time - last_time) > ((1 / f_now) * Repeat_time)) { + if (f_now < 24) { + f_now += 0.5; + } else if (f_now >= 24 && f_now <= 120) { + f_now += 2; + } else { + f_now += 4; + } + last_time = Now_time.seconds(); + } + record_data(); // gimbal_yaw_motor_.control_torque() + } + if (switch_left == Switch::DOWN && switch_right == Switch::DOWN) { + reset_status(); + } + + } while (false); + } + +private: + void create_data_directory() { + // 获取当前用户的主目录 + const char* home_dir = std::getenv("HOME"); + if (!home_dir) { + RCLCPP_ERROR(get_logger(), "Failed to get home directory"); + return; + } + + // 创建数据目录 + data_dir_ = std::string(home_dir) + "/gimbal_sweep_data"; + if (mkdir(data_dir_.c_str(), 0777) == -1) { + if (errno != EEXIST) { + RCLCPP_ERROR(get_logger(), "Failed to create data directory: %s", strerror(errno)); + } else { + RCLCPP_INFO(get_logger(), "Using existing data directory: %s", data_dir_.c_str()); + } + } else { + RCLCPP_INFO(get_logger(), "Created data directory: %s", data_dir_.c_str()); + } + } + + void init_csv_file() { + // 生成带时间戳的文件名 + auto now = std::time(nullptr); + std::tm now_tm = *std::localtime(&now); + + std::ostringstream oss; + oss << data_dir_ << "/sweep_" << std::put_time(&now_tm, "%Y%m%d_%H%M%S") << ".csv"; + filename_ = oss.str(); + + csv_file_.open(filename_, std::ios::out); + if (!csv_file_.is_open()) { + RCLCPP_ERROR(get_logger(), "Failed to open CSV file: %s", filename_.c_str()); + return; + } + + // 写入CSV表头 + csv_file_ << "actual_velocity control_torque\n"; + csv_file_.flush(); + + RCLCPP_INFO(get_logger(), "Data recording started. File: %s", filename_.c_str()); + } + + void record_data() { + if (!csv_file_.is_open()) + return; + + // 获取当前时间 + auto now = this->now(); + // double elapsed = (last_time > 0) ? now.seconds() - last_time : 0.0; + + // 写入数据 + csv_file_ << *yaw_velocity_ << " " << current << "\n"; + RCLCPP_INFO(get_logger(), "%d", i++); + // 定期刷新缓冲区以确保 + static int count = 0; + if (++count % 100 == 0) { + csv_file_.flush(); + } + } + + void reset_status() { + f_now = 1.0; + *yaw_control_torque_ = 0.0; + } + + rclcpp::Time Now_time; + + double now_time; + double last_time = 0.0; + double current; + + float f_now = 5.0; + int i = 0; + + static constexpr int F_start = 5; + static constexpr int F_end = 50; + static constexpr int Repeat_time = 20; + static constexpr double Pi = 3.1415926; + static constexpr double Top = 0.999; + static constexpr double k = 0.1; + // ROS接口 + + rclcpp::TimerBase::SharedPtr control_timer_; + + // 数据接口 + InputInterface yaw_velocity_; + InputInterface yaw_max_torque_; + OutputInterface yaw_current_command_; + OutputInterface yaw_control_torque_; + + InputInterface switch_right_; + InputInterface switch_left_; + + std::string data_dir_; + std::string filename_; + std::ofstream csv_file_; +}; +} // namespace rmcs_core::controller::gimbal + +#include + +PLUGINLIB_EXPORT_CLASS( + rmcs_core::controller::gimbal::GimbalFrequencySweep, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/simple_gimbal_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/simple_gimbal_controller.cpp index 925db272..517dc0fc 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/simple_gimbal_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/simple_gimbal_controller.cpp @@ -67,10 +67,8 @@ class SimpleGimbalController double yaw_shift = joystick_sensitivity * joystick_left_->y() + mouse_sensitivity * mouse_velocity_->y(); - // RCLCPP_INFO(get_logger(), "%lf",yaw_shift); double pitch_shift = -joystick_sensitivity * joystick_left_->x() - mouse_sensitivity * mouse_velocity_->x(); - RCLCPP_INFO(get_logger(),"%lf",pitch_shift); return two_axis_gimbal_solver.update( TwoAxisGimbalSolver::SetControlShift(yaw_shift, pitch_shift)); diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/two_axis_gimbal_solver.hpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/two_axis_gimbal_solver.hpp index ae590f10..b218e27f 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/two_axis_gimbal_solver.hpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/two_axis_gimbal_solver.hpp @@ -1,5 +1,6 @@ #pragma once +#include "librmcs/utility/logging.hpp" #include #include @@ -26,11 +27,13 @@ class TwoAxisGimbalSolver { }; public: - TwoAxisGimbalSolver(rmcs_executor::Component& component, double upper_limit, double lower_limit,double yaw_upper_limit, double yaw_lower_limit) + TwoAxisGimbalSolver( + rmcs_executor::Component& component, double upper_limit, double lower_limit, + double yaw_upper_limit, double yaw_lower_limit) : upper_limit_(std::cos(upper_limit), -std::sin(upper_limit)) - , lower_limit_(std::cos(lower_limit), -std::sin(lower_limit)) - , yaw_upper_limit_(std::cos(yaw_upper_limit),-std::sin(yaw_upper_limit)) - , yaw_lower_limit_(std::cos(yaw_lower_limit),-std::sin(yaw_lower_limit)){ + , lower_limit_(std::cos(lower_limit), -std::sin(lower_limit)) + , yaw_upper_limit_(std::cos(yaw_upper_limit), -std::sin(yaw_upper_limit)) + , yaw_lower_limit_(std::cos(yaw_lower_limit), -std::sin(yaw_lower_limit)) { component.register_input("/gimbal/pitch/angle", gimbal_pitch_angle_); component.register_input("/gimbal/yaw/angle", gimbal_yaw_angle_); @@ -107,6 +110,11 @@ class TwoAxisGimbalSolver { update_yaw_axis(); PitchLink::DirectionVector control_direction = operation.update(*this); + // LOG_INFO( + // "x: %f, y: %f, z: %f", control_direction->x(), control_direction->y(), + // control_direction->z()); + + if (!control_enabled_) return {nan_, nan_}; @@ -116,6 +124,7 @@ class TwoAxisGimbalSolver { if (!control_enabled_) return {nan_, nan_}; + control_direction_ = fast_tf::cast(yaw_link_to_pitch_link(control_direction_yaw_link, pitch), *tf_); return calculate_control_errors(control_direction_yaw_link, pitch); @@ -171,7 +180,7 @@ class TwoAxisGimbalSolver { else if (z < lower_limit_.y()) *control_direction << lower_limit_.x() * projection, lower_limit_.y(); - const auto& [x_,y_,z_] = *control_direction; + const auto& [x_,y_,z_] = *control_direction; Eigen::Vector2d yaw_projection{x_,y_}; double yaw_norm = yaw_projection.norm(); @@ -189,6 +198,7 @@ class TwoAxisGimbalSolver { } + static AngleError calculate_control_errors( const YawLink::DirectionVector& control_direction, const Eigen::Vector2d& pitch) { const auto& [x, y, z] = *control_direction; @@ -199,6 +209,9 @@ class TwoAxisGimbalSolver { double x_projected = std::sqrt(x * x + y * y); result.pitch_angle_error = -std::atan2(z * c - x_projected * s, z * s + x_projected * c); + // LOG_INFO("x: %f, y: %f, z: %f, yaw err: %f, pitch err: %f",x, y, + // z,result.yaw_angle_error,result.pitch_angle_error); + return result; } diff --git a/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp b/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp index ec4c6a8b..5d741135 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp @@ -26,7 +26,9 @@ class Flight , private librmcs::client::CBoard { public: Flight() - : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} + : Node{ + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} , librmcs::client::CBoard{static_cast(get_parameter("usb_pid").as_int())} , logger_(get_logger()) , flight_command_( @@ -42,8 +44,7 @@ class Flight , transmit_buffer_(*this, 32) , event_thread_([this]() { handle_events(); }) { gimbal_yaw_motor_.configure( - device::LkMotor::Config{device::LkMotor::Type::MHF7015} - .set_encoder_zero_point( + device::LkMotor::Config{device::LkMotor::Type::MHF7015}.set_reversed().set_encoder_zero_point( static_cast(get_parameter("yaw_motor_zero_point").as_int()))); gimbal_pitch_motor_.configure( device::LkMotor::Config{device::LkMotor::Type::MG4010E_I10}.set_encoder_zero_point( @@ -54,7 +55,7 @@ class Flight .set_reduction_ratio(1.)); gimbal_right_friction_.configure( device::DjiMotor::Config{device::DjiMotor::Type::M3508} - + .set_reduction_ratio(1.)); gimbal_bullet_feeder_.configure( device::DjiMotor::Config{device::DjiMotor::Type::M2006}.enable_multi_turn_angle()); @@ -112,19 +113,22 @@ class Flight update_motors(); update_imu(); dr16_.update_status(); + } void command_update() { uint16_t can_commands[4]; - transmit_buffer_.add_can1_transmission(0x141, gimbal_yaw_motor_.generate_command()); - transmit_buffer_.add_can2_transmission(0x144, gimbal_pitch_motor_.generate_command()); + transmit_buffer_.add_can1_transmission(0x141, gimbal_yaw_motor_.generate_torque_command()); + transmit_buffer_.add_can2_transmission(0x144, gimbal_pitch_motor_.generate_command()); can_commands[0] = gimbal_bullet_feeder_.generate_command(); can_commands[1] = gimbal_right_friction_.generate_command(); can_commands[2] = gimbal_left_friction_.generate_command(); can_commands[3] = 0; transmit_buffer_.add_can2_transmission(0x200, std::bit_cast(can_commands)); + + transmit_buffer_.trigger_transmission(); } private: @@ -141,13 +145,13 @@ class Flight gimbal_right_friction_.update_status(); } - void update_imu() { - bmi088_.update_status(); + void update_imu() { + bmi088_.update_status(); Eigen::Quaterniond gimbal_imu_pose{bmi088_.q0(), bmi088_.q1(), bmi088_.q2(), bmi088_.q3()}; tf_->set_transform( gimbal_imu_pose.conjugate()); - *gimbal_yaw_velocity_imu_ = bmi088_.gz(); + *gimbal_yaw_velocity_imu_ = bmi088_.gz(); *gimbal_pitch_velocity_imu_ = bmi088_.gy(); } void gimbal_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr) { @@ -168,7 +172,7 @@ class Flight if (can_id == 0x141) { gimbal_yaw_motor_.store_status(can_data); - } + } } void can2_receive_callback( @@ -228,7 +232,6 @@ class Flight OutputInterface gimbal_yaw_velocity_imu_; OutputInterface gimbal_pitch_velocity_imu_; - OutputInterface tf_; From 721d62df019fa38cde7fe2a56c925f8af076ee39 Mon Sep 17 00:00:00 2001 From: 1Feishu <1291113686@qq.com> Date: Mon, 29 Dec 2025 17:01:42 +0800 Subject: [PATCH 6/7] new --- .../rmcs_core/src/controller/gimbal/GimbalFrequencySweep.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/GimbalFrequencySweep.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/GimbalFrequencySweep.cpp index 701d3ddc..e661b0b0 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/GimbalFrequencySweep.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/GimbalFrequencySweep.cpp @@ -163,10 +163,10 @@ class GimbalFrequencySweep double last_time = 0.0; double current; - float f_now = 5.0; + float f_now = 1.0; int i = 0; - static constexpr int F_start = 5; + static constexpr int F_start = 1; static constexpr int F_end = 50; static constexpr int Repeat_time = 20; static constexpr double Pi = 3.1415926; From f7ecebfbb4e33d86a2c50b934d211f06efb70316 Mon Sep 17 00:00:00 2001 From: 1Feishu <1291113686@qq.com> Date: Sun, 4 Jan 2026 20:06:15 +0800 Subject: [PATCH 7/7] delete *.gv *.pdf --- frames_2025-11-27_16.09.16.gv | 1 - frames_2025-11-27_16.09.16.pdf | Bin 6023 -> 0 bytes frames_2025-11-27_16.24.56.gv | 1 - frames_2025-11-27_16.24.56.pdf | Bin 6022 -> 0 bytes frames_2025-11-27_16.25.48.gv | 1 - frames_2025-11-27_16.25.48.pdf | Bin 6022 -> 0 bytes frames_2025-11-27_16.27.10.gv | 1 - frames_2025-11-27_16.27.10.pdf | Bin 6021 -> 0 bytes frames_2025-12-02_19.18.18.gv | 1 - frames_2025-12-02_19.18.18.pdf | Bin 6022 -> 0 bytes rmcs_ws/src/rmcs_bringup/config/flight.yaml | 9 - rmcs_ws/src/rmcs_core/plugins.xml | 3 - .../gimbal/GimbalFrequencySweep.cpp | 197 ------------------ 13 files changed, 214 deletions(-) delete mode 100644 frames_2025-11-27_16.09.16.gv delete mode 100644 frames_2025-11-27_16.09.16.pdf delete mode 100644 frames_2025-11-27_16.24.56.gv delete mode 100644 frames_2025-11-27_16.24.56.pdf delete mode 100644 frames_2025-11-27_16.25.48.gv delete mode 100644 frames_2025-11-27_16.25.48.pdf delete mode 100644 frames_2025-11-27_16.27.10.gv delete mode 100644 frames_2025-11-27_16.27.10.pdf delete mode 100644 frames_2025-12-02_19.18.18.gv delete mode 100644 frames_2025-12-02_19.18.18.pdf delete mode 100644 rmcs_ws/src/rmcs_core/src/controller/gimbal/GimbalFrequencySweep.cpp diff --git a/frames_2025-11-27_16.09.16.gv b/frames_2025-11-27_16.09.16.gv deleted file mode 100644 index 545c92c6..00000000 --- a/frames_2025-11-27_16.09.16.gv +++ /dev/null @@ -1 +0,0 @@ -digraph G { "No tf data received" } \ No newline at end of file diff --git a/frames_2025-11-27_16.09.16.pdf b/frames_2025-11-27_16.09.16.pdf deleted file mode 100644 index 829e93354c7f484d29c422182fb068b640f6a7f9..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 6023 zcmcJTc|6qX+sADwOSYj18EaW*F*8QiY}sYY9z%oCn6Zo{BeG@Rh3rdqAu19fOV&uo zb|fWPk|Jd1nbC62dHv3_{PWCrUNduF_w~J(&vkvTdEIZ}%W4{u5GfgY;YZ6=yYw(1 z7>Kubp_i8jf^^Y1CxSB&PBIzM1A#!01_nz&dyu}dC<0my?SQ|ArdL#?_au0rQLgm9 zS$YOKI@A!BXM@%04j!5plCP~b&>NjUUrH2LNO@=_CNV6wvzcvE7abb|BIbDEClhGtS*psL7w|L%g%xldm;OK#S)9@Owa`@~CjUO_#ecWP*+J2?2o&7Zr$B zMXPXl)`-3=2R(MvQFSmf;bxpLGE4Q(D^kB~(-$V;aQJQI(12t5Pv|Ct;(BWvTOvcg zWb<%Gsz*<-z@&N9hv5AanQaM)ZR5-}W`h}ZJ42s2TmXNhA*9Ms!O}{Y4fy?eNNfU!iK57u6e~N8_&j+%@SJSue6k%$_>b`_a#srdfKzVtxf%F2Qz@!Ecqdk-APqc@@HcNqNw6ds zDDx{KBq!4Q=VZv=lL0{{H{8%bkb%96F~Rj{>YuTc{dFfu9|$?>(7 zN*jaUMa@Nhq@tijQri5d2;|X!iU2|US{4NIw{`to1_b(h9sib7WL&H1T`< z-8$mqFJ+sbM#uK28tZYUl++Z052=EQof&8^J}z6_@u9$zK=}syc0Zp>?>4w8iaRN= zW?oa}a5JmG>Em_>NgA$Jj~XBBbS?UvGzkdl>aVbPB9Xpnu%=BZu~b&oa9hlNs`%_o z886gjv=X0QL!%^cFxC-o+w9#5z13<#QK7ll0q;BWse^@?MJdeJXUai*=`8+_DWL;? ziUaV8%lEw_NA_6q?*I)U(N4-fw-;{|0fXzS1KoJK_e1tW6rT<_^sSG}j|jw6w%pW+9n!R^~TbJvuZ+@|P>iurW%Q z&(3f*c)QxYtKg8xEAOv9Xf7|G*HR4WDKrCTjwP?35f*-XqPD1=|5Lu!xz9c(!kV?Q zAGv)I!D()~TB*nQ z!_Cbp&2^0s?2``PvJtlQL3}HKPl`9XHRyfA_f!MC7WfSMR1eIJ-fLbGo4CX7?hf->if*$Q3We!_glZbui`$ry3an3$M|GV(zURJb-V$>MWd11qV`F$gr>^J?+$Z4X{OC}3=A%aq=O1o&zoHrAowVgw;CUBxO#4dKc{x_=rxFg{ zT8~AD3NS6xR0Ka_TGRp27hW){=77tXW4hPPK45VCI4!&cm_4IK*GaMH;iuz=v=KTS zs=+LYvB$=b)L?QVeQa*0+AxNWD0Ei4e%hT|GS0jz9*}Ih`5Alt>t`%}(!y%j#Z5F5 zEZ-lMMjMv(Cw`hZJJqtgo*W-Y{8BZD6U>D_H}^GK6}h1+l`v4*^T>D_wfV3FZ?N3$ zQfGEIvobh3qy#vSl*>syA(E4RDlJoRdZ94;^CmM3){;2DN&RVP05qGQc`cL8N=u?F ziGRLoz?E|{QydYyjBZu|eUH4^WH2`WIeIDjtKNdwhKQwD&oy*{Q|Ya%D- zzQpG;Ht^)D+NSN3ZN18Z)3$<+ayxb@z$s7 zGAb%BPe81xzpO)aRXaFYjlt4M#*?A3S81zEe5ORIpfuNLJrY^6R^{NAW%v;^`fb~@ zo^Z9%>tF<2A*%Ssq(-IR5HFP;C{QVqWGodhoBfD?iFZ;_`N9Pt-JEz?@Y@NRo#v+Y ztRYGeCP>r*?2F+$Sn&I4St_JF2HZYqzVKGiR{z zYjV5$-rY$85yGoMi516fG5(nTs=cLi)n&PL4bulXbo-4jB_-vqQLd*cxnK@3=>=ma zxT~)QEG_#4+>meqoW#?=*9#R8X8qpflZUR~R<{bzKUtAfd2CM2P3V(H@(Il0?tXpy zLd5Mqe6G3HKAtZ$zZUT&abZUt6(u@x$TM2tJd-9qSj8w?ue3jtGiAd)37BXOk7r@$ zM1N>f-0*R|Z`QIk(|0aIkQm~xRyt#LK1UAqm@WSHxX6nDdYvoE*}~IVdaK=Y(Sl{rs<4* zOALLz^vy*@({ZtCXY2~wDeR-uPhMFt)Q+LO7}#q6Nb@)$=}v(023sWSlf2Ac&NrYS zi@`)Vm%Ea=ByICO02imdu1d_CL&Y#o6&-h_oVd<~vhL~^yTTTM3K{S?ea&V=Iew?b zdq`0$51o(0^q3H##SK5zr1Xw#+7bxBqtiO)dN+K|@?5Y%&M|38>HA8z=$FJwAK+MI zR$?1zZ-(TzjoWDzCoG{7RYzF%A&5!@qV{^Y#o0c&)(eR8Sb}ya4o%M&rXq+otX3Yv zDLJx)ZLb>~yX#Upq;g8!sn496n6%8I&%pPz?v3K6&?a}z!&ipOSE;73tQ7^{=O&lE zT|iGJ4_*KSx%ldCVwH$trMrl1kqdnbr__cI{Ui6|{j~C?Xua$x&c4^1#sCCt70}C% z9S@QVb_4IEvyQ%U*=QxIoi2khyX2qE3Bid4GGz*EoKtVsFj?at+qoKgHdS?>CN}-_ z!;J&s0YKj4!LmU3>Qs&Vuwu--;6vs?2JVZkVJ4QGT(3?lAT8Ke{b83hnvZX`PP&v z5mzd#U@-JKykJ8jT=KbCr6b3^{8pw966I5jkL)=Ny+eBicGSICLiL0WA3t-u>Z{q! zS~r18D(pcTtnu9@p1{NTv(+tzRzAQNGzQkJ_k{Wr=N9^P&%xX6U8X`Q9~-1mnGVH7 zV{UJo)bq~O^$CT=l@(r)Drf~xONNK_Pqfv1Yo_0v5Y7qSJM3sg_we2hL+KPgbs`W@ z$J7gjQ8_$ax;M@Hbq_zs!vn4)DEXkmNZXDx)$CZ}_Xjoh%fasYw z*{v1o@RDN1+}Mdm#15a|FF8z^>K}Ft-A^ESVGR(saVJ17Px2ZJF2PZDr?y=5Vk;^RBrn zjA1)vaXEb58^HW(_d3HZoB%q?#3=0h+i1=D6SUnS3aly6&m#g6&i02IP2++O#d32D zS0C~kY@k<+FUl81+~nG-Keac~gFUDK=Ja>H)%LIFGDu%(pqo~ldxzg$O{XUAZ831f zmv4D;1QyS*a!k_?&f-rx_=+5ubjoC|R-5{_M^*D7*2Z{!$77UMKiD2yePkj(`+|>; z@f%--FE~Q!Q|&iCw7pMAqL0%|(37pG7|`jN1IaS~mMGA2^0tm>gF#m_vCi|pI;^(+9X5koTT&c8UuxE}?=}>^e(p2weu>$bJ!E`4IMK(l zrY<}PYUSS+0a}mZy`FjFg*)~zKeB$Z#)h@t7_%#co%GJCFKbxd)_%5(#9imeozCdV zt?roTa2J37XGWw|PTi_xSwoT2Lo+Up+`9LUY|XL}gIg<$NdIz%KS$OB%|&3uL_Rt^9D0pZ=x3@2_ASVKVs)|6G0Hc9#vs(cj8;HRR5so%HEUo zOWL!tU;|c9IzJqXXV;GJOBjz%Yu64qGhjl@QSZ5~4%J{~N6fl;j$zy)OhVz*YYvEB z{8=BWO_Ks*Ozcy@(3E%_Kgt1%EH>4J+m{Oz-*fO_6hYe+^t}M|y;84h^iR*Tm#2$0 zgXn9Nw6|sR6ULX{yjKnv_;Z<4XWu@)FgA6pSfE+9!8!^Os#MHy{|`sgS1JH=wxNOu zM-IJ?^pr?`m=T|Y^qJt&G_3Vk<(}IYzRWi|&cfn$4e#}IP~FoyCRpo1!(qY9fP4G$ z+rgm7)->(Pw8({H2h;+HpveC`4IUlxejW#O6wE!5wmtOWF%rKJH><;-GI5Nr)`c#VUP zlfndl(V+$xIyHtqKV0K?7y@WKk>$nf3?ocMuUC7hlzRA1UoW3*SQh<0zx765C3(;b zqcw2mG>C(CQR70LUV$`yI_U-bXQa!AK%=-=f3g(}F{;Rh!mLk2apr-KK^jez&~fdWV- zeUvK+B3xE6xU6O@u7-9&UGXwTdte-YJ7}O>G1wcx^FR84N1dWSda)=cPoOmPx2uW@ z-WO;El$1fp0wtwk2p|*!h65oGuq@E#$VrQU!eSgQ;+(K(Ao$2~(bEA%vZ>SKo#YtiN-iN6OM*CG8q%lu2+D_ANP|Rn!g(4Es{_J8cK3LO0~bZ37Mvl z)AXNu{20&6o^V7`Ogy~MWUY>@Dkx9%5tsRIwIMSVRcDmPF9mc_Kid$wTs`N%>3|GJPenWsDU-;v!K7qiM^hi=5hMBuK`g{QZBG#?*k5D={z&a=Zox;-fAQPXkiZ)IIB>UtX-JcawS^pT4OKU}3v z?!GcB2dk>@18?voGRL`M+ifv4G6iQ=vCS>{WZ5L}-4y|OEFkrsnwCBnvG{W&M|{lv zQ^_snzHXxdEpwJTJc<)EZ?C;xZQgNrzf06=Q?B~R)>yBPt9h~%Zy8J7JIoxAqaf0l zS67&^>9}%rrt#frbA?`XdDbUjiyXhfi{@~ty7_aMKT5-3=FlZ@=|KZjoo@vo=R+z9=nnaQDI(^XVfnLQbC=Y$YVfwCHF0!z~|0Yw#E6b);C pj6H`k?OvAozNaHU*cFfzt~Ms~c$0{}<(eRBZJPc^D`3jZ^%x^VP0c^oB~<9(-Fq(B&k)hHNk(*IHWH5*TtM zn}$15+`5AVCQTw|g7%K3x5g*5jx$#p58SKS9{kMd#QsMrT&frqB&~><8PO}W*FJW3 z0eN@6L`}}B8OC+i(6^g!n^*q(GG+6;sLlvI8fW)&*Q9S`y~rYw?5) z`4th86Y2AFGQ{u606@bVE@%Ky*T%_!;Cwjs&sfU-dJ=dU06y$c$GEu@0MMV`|L6Eh z8-U(L&P9Hpq@Y0@vG}J5Y?a=IP6c@2Hnv@N&v%MgELJ->r}>IX+gps$q-D052M_j}yk z8sFM4#T%bS$M&Wg>TpI!s3-&dt158>+USFL)g{^bdaCS8Do1B5gx=_0kcE#p3e%+hR6T1!ty< zc_B`tW%#s8YDIzlv5q*aCeKdDtrk;?QjOgXSnsJ%9W2Z&ilN?KQ?}xZXYhYa3GMS! z?1PS7dFUB2vdfZl2cQRza!~TRy>KHR5L8zY;BvZaFL*B)`Ly4*cWqpLL?F7X{XxRO zh*bCXdNZ=1_Xthvka&afmT;v?hCZP8yyn?%uv_0=bw{6%`SsaeXY4~xMs9uVZER1x zjf8+*7dYqzKauP7@&+?gm+DN5uTNKidrv$OFD|;$FIf?9x6+W3`heF}j=R*b%-jY! zPw9|RGjgM)TRaDHg`+b6IDTKwKC4$7V4Gw!ttE;vc+&`X=7pQ+?u|4uB3FOah&5eb2MX*%ClzI zr=tzjSMM$WrA}qt$t-X`KRx{_+fkl+_Z{KZm741tUo4(_ex2CL-#+SY@$|X*r;(j! zr?~1povq)Ma!O>E^i}LPm6XhDB7?j0j6vyRNo%Ksh2I{l&Tr@cl%sj}vzMW;Ms>^w z9&dP1s*5)F8uMDS6!G!BNv}o!!FaUY9Q4 zOi9k)Xt-w1TTUfqo3D^@Lfm>4p#Z;V-eM)sZ;f8Sn5t!+miLZUD0+5&|Cwoi8~mp4 ze4xX!YIwh1+-;l@Qs(Q9mR#1gkT<&2>Ak~tRs20Z^6B=f?3?Jn*SIJ)afidzDe^(| z_rzol0pZ*(U`ci)-9`Zf0@VTvRW@)Gv@#$+mn9DN69@;L27Q7SFI ziIpiVZV$U%lm7FWJ+a;)Z0C2T!4 zYejq&pqfT0aDF&Z)E3?wmOHCzi@P_+^q`BQUze7a2382jnAW80q*!qC(ego92ptSo zViv_XV&XFV=3H)@Po2#p|;;rosn7&CgxTN=&`Fw;QNiUcHs&?09X*P)^Qu zkDh4A&fF+!-2r6gP{-rwgTWi?a0BA`peE<+eWe^*#VcziqkK z5iA=#_D4XaqR2OfmCAj3c&W4ifijUq11bO6jK}`D{}7hVZWD5bT+_+k3WcNfoA6lc}c6ZbRe_8MMFO3K+CSxZ%P!t7(xa>tJG zR9N~iE_wOikZ@u@j;DXG6CxnY`u(|AHo9(0%`7bEcxhtU(K%HYp-*l}$1n#wdv)y} z!*BoLW#?R7JD+D_7yc#TuwXP~<2ovw?taQyq363qt zF*5sk5Hjmt#FAWix@uEkZ9PY)%tCc0F10it)8?sB`ft9+TU%dfT{el*ZV* z$k5wE-;|$EjEzw}Wu4bfVH1^h{OU)&OVKnJ0$OYyYtRxB@Aw<6vqi8z$xiR#dIJnJ z9Y}z2yDFMU(lk9_=jO7}R*rsifDGkQ)^b(MjP3kb+*R>nN7yt#;T|mZvPP4h9KXZD z1B9rVo7RV6dQ7m;!n%)2Vp>NAO(B@wtRpD4< zmSY-dZU*PHj$3OM#4n-}R7P0#!0<9Sy!v{W>6u=+mhdHE)m`LK{4pkB0P?EGeh3tfjf%=O&jt zoq$g!_g}CJa`V;P#47rR7VW?@M9%kqJfS*#;1{tM=cAcDMdM*japt`a5yLKErGQ?l zwck(5-3hpp#yUFWwBF*Yda@YG?38mRGZ-fpz?3eqepanX-EftEY}+#AOp3}LbxhjH zN9+5-{p{Jb1H}Qbm8nYkVPy2Y;3MV%2A&Hop@!GExQC7_AWS({{Gb=rn`k#$CS6R# zS62ts_D-~-bM^1tWV(NgdE%8zxJ8itIMX8Zl`_5b@Lqk2qii!r4zX2*wfmZdWV32r z0MdEE%zRleK4$M7)z3^miiv-pCdWAw`i}TJ)<+cEbl3}5J6Rnlsn&>wsgfoM74>}sq-MqI$QCfLV9S8)} zQMEi_ROadD+BZ%5v=2VV!ThhrD|(?q<@|8!6HBHJhIjNVZXD%=#g-KNtD9aR0O^@G zILsAl@R+{jfH3fOIMMI?ZhE5J0UlWo)7)1&5OtKhT;JroApTI#fC4K@^aJUi4Oq7F ze603>Gf(FAKg{81HPj=>^o$Cy@a02K^L;cY2Z=*Eoj?r`lE3hU*K92~5JK7wmH;xNF63fce zTY1E*yN+HqxFDY&ev^B%?!@j$H+H`iklFYA?IpiDZr!xydO9Ln*Zx*eR2~QeouR9$CQ$UmfH19*#!Ku+?3+<`ck=?akoD2Rh!qi>qTY*j^OdFpaidL zl{I035Hr8laNt@b@AdQ>FI=$)IT3Y}l@_dZ2ACZo?4;-Yy5jn!txM0A5V-4{S;TwY zSrr}goUY>UU*C%`%dAll0{?|Q+viz@Z@V&zPCx16|*>mvE3Z*x?osE#tt2*O@$35t!D zmCV9kR23MR8)bAG8%>L;mA&k^^9SYwh$gtcEuz=RyufB;SlKdvdRW=dr1KId-@T}S zp=jok`Ui~ShP#20%%@*`Ej2mQp~keE$#+sJ{5?au>jlP_Sp~_{^6W3^rawa!vZg|7 zb`k7o-I9*P2)HF%6uUE`HKkM(4^AIliwg zw}@R-o@;T^F3qfadOOk5*#qe~-3-wI~P5I-=K1GWB8?pb`ovUU7nT z;?8(cZW!kJM#ns5ADj}84Ss3aHu7{a z#^B59h3%~w{DkqPH}93g1YR$3Y3

<;A3o6$muR)|*FyLlg@b9{yo(G^EUK!Zw&2 zZqKRHk(M0657pe^{FnI#`&nr0j^2ar4$23bM+K|hs5woU8E|i3 ze%l`q*(A~|6GhG+-KP?O2S)t!GgQp zToi`*3$~TGkf|~Bx#3EmgJAZCWA{CH9ifD&sI>|=0XhdQ)uD+y8bJUrm)`rTvQ;M94yUQM5 zD47Xg$*nW3V5mrH7Tp(FD9p`bQ{?d1*frmj-RjMtE`D$3P`nPE+0p*v6lIKHfNG}7 zy;~mM2jYr5$vulFQ@A56|3&{_EQfTQhWthTU#NnN9e!{EGGtIiyW6^9TnKo#A1Hui zx{PurL4+&Hx>r;U#8uHwsH+|ZXg7@gZwGahGX{GDaPCJR;ILElM=uuT;0}<6{B~7V z#(M+I0Fp9rS%9Q86b^uZK`;Oq43Y&{96D(dP*{xZ1)KvG4FDZlF1XvGN$^7!f;eoF z7f5bPlKBdm5U8MBG|(6aN5bJShb998+W9H~@#A@NL*rM2yhRdfKto8*hpF}_HzCs$ za+>~Kk00ZC*boj$ilLhanyl5KRT<@uKIAh0RU0x>QE^1M{Zc?1^|K8I|Jcmm6$gO- zWNd0U5>~_D8~{LL4DJHX9rMd@IJGgxj^OA{5>gs0BZZJ5$wvPui46Hz;oybZd zAW#U&>Cc(qAP8v?kPLzxED!)HgOCD2WTc_w-9kZNDFh4#C1nzMHmH;=^l<9KJR+qu z-Ux>wqe%dmTxj(FLdd}6vHl)v-G3l5FnL)&O3~%tT#U@fAkv2+_%~ohN~Yfs)F?sk zdK-u%l(-}Qve%@yMWkv(pp%xm>qZ0L{M`Na57u;6 z*>({Wz#DC;X~3ul@)wRkG;YD;DwS_fd$FjH~j zbJJz+3VX%{SjfwYZ+_V^NmC=|6l==6t5u(Jpg3H%Gy*PX{E>1Ft-~SE25aORdMiYJXkBak?9m>f5*D#Q)f&s<{ z4S@Wp0LB0_FaQF8{HQSccs!|8{AQ;>O`JU*04AsGVUGL`psls_=0R3OU{v4{RS+2R z90CGCmZ|8}z2doORZ?Y*XK6etfK=8-QM2|%+k8+0 kS0QO=C}#8MGXA-9cLK_d;O$1*6ih}2N-r#|rmIf>AHWbMUH||9 diff --git a/frames_2025-11-27_16.25.48.gv b/frames_2025-11-27_16.25.48.gv deleted file mode 100644 index 545c92c6..00000000 --- a/frames_2025-11-27_16.25.48.gv +++ /dev/null @@ -1 +0,0 @@ -digraph G { "No tf data received" } \ No newline at end of file diff --git a/frames_2025-11-27_16.25.48.pdf b/frames_2025-11-27_16.25.48.pdf deleted file mode 100644 index bfdd0964c7f71fbd20609ea280a196b8713b5b18..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 6022 zcmcJTc|6qX+s7r@vrk!)v1Xse%otg-MM$>n%g|snW-JpzWXqnd5ZU)7L`5Qd_9)qo zG$mP6gzWrgw48HZ=lq`M`RAFL*Y`X3bzk3m`CQjr^Sa-{m((?-Akwn*!jG3KcIaV1 zFc5F=La(3z1nHu2P6THloMbYj2Lgd0O$?TR_8{G{C<0m??SQ|6rdLv;_au0rQLgkp z_x1F3bdEw;o)1)}I(TTEO}esLM{g)9TI?sGc;}I&xa4c`?TrlUn&{BbuN|^nnu!i( zr)MIrh4KuRH{#qqg&O=RKgK!BKmA&z3^Z?L2Y+C%R~ZqGvwq<&3>jzRd`dtd!^H%m z)zGQ{&nmH(rJyHnI%*Dv#ypJUhNj8>*##Pxtb4;G9S*)NALs*!e?T`Fl-8PC*b*3W zq#9qhCwp`U3rv_reGJ~CO>c=$Xc=R!G95^(*&h51aAALwf{-pj1x6P~YeVMXpR!nc09*w*5bJwJAWWC5Dk?$m#pfYfBKe;9I zTg^lLG<*o>jR+*|fC>s^1L*}qfk_P@NbMXD0t7jrNLP}gq>dj~kR~2S_=~rr6j%xj zl>HSEk`w9kb28-b$$%i^>uzWuNZ;PYh~RoS_0L$!{dy8~5ePZ#(8PFn5`eIu-~Z?M z${2y)N6kciqNJck94*aFxE(JTK`fOu$M79|3xSRRKKdN^lV-vh+;~RYKw!j71;u3%cg@IPo!04i|H;F z)zV_L+hh@LtEvyF7=p^{vYcweZd#!4nYZSaeR;(nRmbn-nDWV)5#(np3(lOB@%tWk zug34mmy(Tzk;->>sf6n0Qx zO|MOs!c8p$i8OZmiJGpKkLw?9znJ$vZX6KuqOZ*SsbuPg{_2Gzl8Yr3b+^RrCkszc zmGDAcM#}N2RaD9X`=jk~wvApL(3{QX6lGew?eJdig?1KZ7UeJ>?@0%V#nbpVlS2Fa z6#HP>OAovvhj&?W?g9-U(M~Gfx8|=G0E25Q1Kmz_?S<@xC_U?U=v^CA7#4^rZ_7^@ z7?$qdUT;z=>^(x=@=BuKbW6BOEyEDldsh2QH^ifFue!a@*XrtYuPgQefRW1(dkfnW zZ!alur3(_=$xq}wwY6faEzp_%FoRZJ$F3(kFTyABr zG)w7}Q8RqKxmzL!dWoZ|;NYukT5{XUvrEw5a-2Zl^SgH1w)SR-2n}dBsD2 zTsF$~#*T;71FCsNUyX7=Z!^_Oe9>4=Tlr#ytBk3zU2aWZJZDn6${5ahn>mKDN9}o& z`?HaHs>}E0LDJk=cQXq;&rVGZWjiZS?Y<}6yi{{_46wt)IQHS{UAW z&dpip*yA)@V;>I zM)IA4jd}|!-io8r4h4!CCnW4vk&1}(R?W5w{C4PhjJZbEDFvSx#p35@_n(^=v?6Z! z%?3Fwt4H)3#NEP~D9L`^(UH%Z+-E<&dWxcaR1V0sa92`{$v#%kSL=kl)q3^Q?i?Z> zZf17GOxF;>G2!qn17S-a#J9}8pme=UlinwMS1sV$9G`x#+P<0L2d(qs<99jSU83@1 zz9-({5D?Do0+nV*(QOn$p)ehgP*pugVGA=-k@Xo()QgieyY4GyO)+;tW{*=p6{pbP z8(EpcaVr!emBzTi;5yW(*xx?5K2I*qK>0O1F^+NHW>@jJ^8xGZsH#Zn@*)CYBCX z=!;6B4!i#cp6E9{*|f8k6c_0CrD6alm<4Y&^D$fzxvnc6-(TMS*ocVQcvOVfU+QwH zF};^w9vmG~1nf`D;ygMol9_rUC0&p>mzVK*gP8?u;n&Z3bYZX`G@X-vC7sPuTe2jP zf3~9Em2)Cp0uj4}Zd3(*kG#>KKRWw4dNKN|-rTiy5exC|D;nPG4=279-!Hh=#y)Iv z0hHFG6}VH9K=YD>3Er-fob7MfFyetFy(9vAn$d zBA?4>|I@GP8@8u2ChO$tc0DdK9eW}l;GM8`@#e|9u*Q~`T?#byPgyE|q-8<(TtATlvJyu>l?j8;I-|HEx zprW#~1;iWrO4`L%E(9m3GgvsudNS1aC~uZXOchDz7H1i*MIwtU2?%`!b?EY)Z9^8 zo=Tg5#U<~6>yj?)$MN(Z^g;!MS--#V&PLa6X;_Bm94|{OKQ^Q8CbZy@M2k7t*{f}v zi@5d1`-*Gzli578D-mB3=C(CZQDWl*1cUHm^N)r^`1!+^b7G4J{T5xnZ zj?vktA!MI5Sd&$BU!GM|+I}3FTZ~>x*qkEFbv<43j`ee_uXXb+8I|9&eb=yVlFHb- z$k5wE-&jyUydA5~ZI{BD1~vV>bNUs-tL$y>8kA95jGE0OoQLPsMTm7&+jy! zj})`?(E0S59up!ozwWD+nA)B}T?AqG=&;JX+6AAnI1{X&c}zx1=7I7}`bF{Lhd375 z<=A@a8zDI@V|Lnw@r$SgwPBV$2%;Q;sJ}D(lEAm-S{p^^+wqW|y4PnISmwK&Eto^)nicn#QaAquVy2r<2w8sA5x3 zK3d-w?q|<_GEfo-Uzw~@c&!vOEBJ_cfPv>+bC|IOC)cavib!*g6@S=y%|@Dy<_R}5 ziPhCXjlB~s=v>3J8%+0aGLH|rMOX(rjxjC5hE(ZgUhmZAyh3{fwl%J?gz5<$JbCVBd(d8w?&iG}hSJG<=0qT% zj%nlxqcTsu(7j>Sr+e@@4jynhUfCNJChw2a9A7ebGQMkIef<~!e!H|JK-2sj0YuNd z!C|FXgU9sU2@Hp9M-ctb?xrWoAK;bh;M)5t2V&0h7wa1R<|Q5&7*Sxwi2h*1Gl9#t zUXL~M0eO0-I0BCjw<74p*zKy_*k_g@m!vX-T^eXT+u4oZsW0&wzbo(9*Loc+(70o! z24mQ|GrttR=EctZYUe7$O`HHa%GfaM`@3kZT3YI^5JlEI(9gpH5zh7pnhj%ukHoVw z4OSlU>aU}hjm{|)MBL!otUa+i+>PBY17`NUcz40SmP3VOO36$3gswJ%lT-xLK}O4`y9tJ8nc=vVHK7x|JC z;8$LEp4EG(d6Mbvq_~&)LmgJzzIN+@%}r^5_m`^GjC*x?L#^Ip?&q0}I6}s@f)l(g zs%pZ6pqBnE5umjw-mB@?JKeDdIgzy!Ro1MvMwlHT?1b0-+LF4ZtqaeWkhrUWEMi)B zR%QDvz+K|QA8C=6nKdg?C3OW(k4(7$Sv4OV*&5{{1~!-Hk^ZF&fzGWXcJZ~PW7QTj zQqum_<2_kx4mE(RF^;yZs*i=7*o9V29t5ZRM`~FjGKS~0{tVCCO&@4Q(YZw<_l&5x z5;I$;59{J1=2aJ2M-defuU6+MUcq#4%Z1CdHi}>77RSS6^%16zg7D5QLGh9D z(rI|-!$K1)lZ$m{mON$PC z6~kOwm(M6+yc-n7e5&(nnc3-f4W`{pzLU}s9~jcz&oREtD!enL!2XhM>N89+Yci~6 z7s-xR8oIsFF~!)@0?7V0u{ca-6sDn~z zJ)prUDugI=vPc*4O^oS&ZbVWnvP39ZzqCCu5@Ev@&F+e93GNVA3_I1J)v#tApG-G{Ogzpgpj$z{|z!&8}#DZ zfGiEG06$&kgojCWQK6D1^Bv(z#K~RL@jP zyLrv$Ktg%vPS4`WWUfe?f6@OJ%OM@7p?{J87pfp*haa4P3>nnXo(>)uHv-<{2MQpW zE}~pX5aE)l{v~xI33ap!>hd)sv-mizg<;T z@jgIHpp+~^4k#r9Lja)=FdPVhfaQSJhfdlA6c*!f4(EhL1Hp%ubDj=p68w;ZA`hG7 z1(MrRWWGWs1ZpTZEi}f-nQ%DFp~;AVcD)Ql{&=3;(E8ONZ;^x=&`^@|VXFPfO~^Eb zoTh)*QFBIl{8B&{^|K9u{MgLj6$e88 zWNaEZ5>~_DoPZ!x4DKAx6Z6Y(IJGI}3c=ZvB%};PRvIZwl8+qZ|F1Vvmb|Y2{SB8x zNJHcxFffAT3`c;a!Ehv;v@!$?EDe!C$U@|Ra5)lVltYk5l#wM_A+iu88CZcyJCTz{ zLSayn)1NaTz);d4U|A$NSYRMb7AXye%F4jVyM=)v(nvTQM#?1eY%pm#*x}TNc|=NS zya^6NMw37YxzHH?g^)qWWBonU`u{*=5c0Bql%k8jxfq#|L1hj@@NdA1luW-Ns7Zps z)eta87;z6Ui`M0GPIS2|aHCk2%i$81_~NuG<7H0GMC3?dlls-puAx1o?CdG&V{~&T zxac^YHIJq_Bo&=A7kK(sGjh6J9z#jdZAs+zzZsO>G8-cfKYju>_H{Nk>GG0LzNV;8 zCBRuXIu+PpA)jWQ*!O72zu|jp`vq@1e^hh}k_dR{H?jSIIya=`NH6l4gH z!8_~^&f8$C1vIrTXjd40GJd@KB@BC;hS}P2fCuv?KMXdepfG?l7!$5{H{CR?oJtNX z`SR|i^?RF_kpV|Q*Ku$Er8xbfmE__ii~RlH@CzaS`C~LQME|HbKiQ$I+3ev_o;(-uy${yy(?*Ka3*=-(VMFvF&9Z?5E zpf8|cFm(ASou+p@Cr!L?AJtSQC7Z62it6<7h`Y2xRJkhZtnn-j#{`sAbx~BTeKGc* mR3HzPsHrKY^XM}ExpPkf%7ftJLE02tRu)DtEUclgN&g?Vx+D$& diff --git a/frames_2025-11-27_16.27.10.gv b/frames_2025-11-27_16.27.10.gv deleted file mode 100644 index 545c92c6..00000000 --- a/frames_2025-11-27_16.27.10.gv +++ /dev/null @@ -1 +0,0 @@ -digraph G { "No tf data received" } \ No newline at end of file diff --git a/frames_2025-11-27_16.27.10.pdf b/frames_2025-11-27_16.27.10.pdf deleted file mode 100644 index 83314931a71914c6661dc6910dee0c9c140f8c79..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 6021 zcmcJTc|6qX+s7p-OLj#igDBhVGsZ4kWXl$^4h=?Q#xjA=&~EB$Ey;0000fV{t@`6X}ja6ETVyE5c0-t&9wJqQocd?(2Sg(1lO6O(hGzJlNtb!!esyi0J1`pt|UcC9Y3xhWdfe~7jJ7(uqYTH z_A4SJC(`HVWT@Yh0YG}U9WVfpmZhC8(f)AipRpAG^(06W06FYX#yU9@0q~#S|L6F^ zbip6Pro%o{Qc$CgnEq1)^5{QBfFOS@3j+Dex_&ML0{y*?f6FPdESS#>~3KU7kk>0Bdj^9JU=(d+D@uf05BRXjGb#9LtCkP9!bcvS6++3+nOnS)WD9Kr*S8QS_ zGec>URxxn9zD+m_dX23t_w?=L?~Zw+efLE_(x*$lxl#7H*g|FqrQ=$Xs4zQf*L#-s zq*$1__l}e3W2!l652a%Ej(Vz(a@3}{Ei9Z-l?iCeF#x9y#jl;==l^h`Jhzc&K1=oD7dJhAmGa2X zXWfzhNe=3qYm94k<}3~JpJj{|`>%&|oisjWP;-ZEd&ruW)i32$2?}-R9SVn4isGO2CSucOlmNia5 z7#bciRM$bWjaq$6Lz>h2aWAvZ%iL~NrgabARq%0{<<{y@*f-Sqq;f@Q+)SO59+(*A-SFb;~B*|>@QpCzt z^|^qjG+b3b5y^ub7qmil1ZPhvTH#Zs86LK>b!ixeZdh%jf{8f90XIr zQk`K*)IksaAdGuXjn(a}#Yg*kel6+7^JO4j8@lVP2;5c&#&i|8J<}aWZ#>N>Xf3wd zRT$h)E%px&$Om-AWpErF5lBxym6XaiKAV&FWrL9kcf+%bEjl=rg`V|J$P-yTh1o<$elOOjOaC_-_oYoVz8)e+F8c%EB0-t=dS&vR+5}*-FK@9xoG#F3D(xB10@tXFoP-2`W?Hzw|HBB?c5O}L zgPtQGte>C}!9hTtC8sAr{gsM##lfB}@l1k#d4YAUp<7s7U!4ExkCZO~CJtsQ6K8PI ztCBl=uB~yrA^aztv0m8DlHG-ir3D!kRpa~VG<(%=MMWiV9$8D0wZraXle33T zoGmr;Sy*)Qxh-PHdXhlTPCJ*OUXDm>ts<}@v&(|hYRyg@h7kcJA0Ll zvmtlix!trce?F6Acr)Z{?CiD@I!tin;M`!g?L?ArcL}|CrR?5B`k3k2QPz>#;Akc` z4$NeY%(|QXV}rWQiH?gYe4YVbijO7?E~QJNpR+{Y9Ts@wL#uXOE{%WufyPSfbU5G8 zax}fIdriPTv%fO)rF}`}OF+vJRQ4mxV(jJwakllvqFbb=b#QT zKcntg1gE2{p(u6jLsm`>OLh5(_XjdT9P(<8vguLHvjwfCZ+7^Ne5F$mQJN~X+LAmr za}QC1#!hOV`)IKN7v|PI6ylPb(x~$xtWM1)>9<-D(>E^qYo#B9iNYSs-l1I(dh`U( zB(@w`P3;|!)i7+KnisQxj#U_7+JhjAk;w8}!A9phBQ6tuObb?m-fagY2dVVe%aqn>e#Oqz7uFDu^5Y+TqloBp&{d(n(?498rQ{e60L z(bW$0Vs!rvD<3C!g*Q&tGw9I{GEG3DWA>C{-+@=?UbKg5<`}h$1;zPK8sk`2UUO;8 z;&bc$`0O3ud&$g$y>{#Mo{Fan;EZ-z=hFl5LcR>Cyz3X0YL)d?d4{&l0?#KZ>`_G~ zpMJW&&)>zG`MkTp7qK!{CetBV@k?v9L(HR zTA3?G&~Z6!D6LiQyPhWq2%a=0qn_nS#H@1Hs>$w!&e-YMPW6k3MoYV~z$4GKk|_0i zBEqqExAZExrYkxw1Vt6(NC30z0pp^sFVP5}>oKx!=paciyz0)vK=z+C9#_tCwV}SsY0mk%N+>XN@`Zsdd>+y*4CxK366V#buRiY zo4YDyXTM2WE!E3IpjJ-Rz=|0JW#F~=6foX zk*>Y+luK(Jv#fhrDmTQNbF=c)?m!!EzX*`t+4AA4S0$%b@^Tf;xXkoN!p=(aQP15? zI`-(oO=ouBya{IZaoX-F!bvN4fqlJZvDB4PeXqu_Qf}nx5SROKgzU@tFAD+_1BLvydM*CHF~1PY~I;q+P%36WOw^owwiXoDyRRo+pyymMqRdm;Vu7I zw;N>@!G2IoeQ{8*9VmaRf}6!UoE2Wx7ahr zQ`$00n`YP@g+Kj~5^9`Yu_9Vfm230VfRjC=;*&K?t$0ZH=JFiMtB}su_Vu7eOl9G4 z`Hg8&pjY`wd&Zho1$)LYTVqDqWF7}@{9g-Q-JvD{MH z&|q=l6ynX3Jbe@Wv^E3%2_dE8w@vrnVLyYZ1FBw!cIcbrSq}8co8?aQ$$J?#UuEY` z3HR-dU@WY9NH46n>lem&?#)t>;rS*dhTU}T)4-5Vbg7P)>ECALB}_=MzNMM?0+-Gh z3#!;fv0`NUqc)l+=o?yXs>*QHX-Pq(65E+S`h=Jgn$(WcfGlGL^ynYiE*Jtoj0Wj8 zQ_8IS)YyatjKfant0TV&GCar*iH}4TTu9U^Y>Ep-nz4kl+M^o$n}wu<&ef{_ zAsW%=-6%KovOOarU$XX$2}kjut#GJ3eRYIoA#dJ8D<^sZj74_G8`h3qrHX2=;He z;qABmZh_5l>g90(iDUamd69mh|2z#I9`b%32Vrm&LmZj$$k7*;b_&O;P;u$_fqol z-7Z+wt}~}W?9_A0H~m)`0wG)q`3D2Dg5pkyd1GpUD3$APX_9RX(Zx7nt$#ZxqwTS{+ki_y`T&QWfAl)PRAKoDWm&Pi{h{ zDdaT$yBG{;M`*rlMercKW4&I{IfD0{O9-zbg)a z{K?pq@Fc8;#oGWt23Y)MyfgNf;c#jL>`kJrGf7AoL=1=$Bgsb&^8eQxB}QJ?|NcgZ zBY_Zc2po(gIU|r@AQ*u{kXD9-gMknjQVb#vK!}qVqd1a0B20{Ag@{2=WMBm*?L-`i zg2JIBr$1*xf}x~Az+xzJu)qMg7zzl6ioxLI-NL~TAPRwilQM}s8yqMOKb-n7k4Py^ z(8puRXc7P+7aEmP^=LSEL7Ql$Bti;)=_6m}Sbe*;#eWcm$3^<%s2 z8o^9~OCDh_5+vAgs#~yzsTeAG!JwF_>mE$y4phz1IFpphrS?$WupYFmH3Ft`2lk}? z0gMK6T3rNU`qofC3uP`DFp+tNmE+Dt*WeNEn2)FlXF21cN@oAZLyavBFRO=I`1;#D zj>m>WXIg;}SF}FcQTv*C;Pv45Th`DQ(vy*4gU=+{PnM5ebrL@<4{Y`a`hNNhn~Cq8 z0-Z{4m009X5EHy|6?w-^*Z7&EEL&24EPrrjqSY)iPRinY&r| zT04cLnl+cE(vVkTlTY~v-g+6^N9}0%Cy1})(3EWaL8Z=}e(}b;l~d9^pxvTMeQoa- z(;uRIKAJ@-*748d`~Rgj{eqR`+9W3S`>){_LHzT#XsCnvQE+~uLoxCH8U=Du(8YRS z0MH)=zyM$j0YCxJ9|cB-Kp<6$-{=&iink^JAmofaOp)L5v$C+*Jje+33->#s2!=pg zpkOd``6!LDTMWnX82(PGiF8U9bs0JNsgoi1PF$eMmQ!SoVX8UCDHq)$ diff --git a/frames_2025-12-02_19.18.18.gv b/frames_2025-12-02_19.18.18.gv deleted file mode 100644 index 545c92c6..00000000 --- a/frames_2025-12-02_19.18.18.gv +++ /dev/null @@ -1 +0,0 @@ -digraph G { "No tf data received" } \ No newline at end of file diff --git a/frames_2025-12-02_19.18.18.pdf b/frames_2025-12-02_19.18.18.pdf deleted file mode 100644 index 4f66c84365103b97b7c874b39fca597bd518fb96..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 6022 zcmcJTc|6qX+sB3M*%cvUE$b|1#>kp2vSkY)h6W>MEW^l}Eqj)P$evw@L?V0Hx3V2+ z3R#jOWal@d<(%_6=l49%KhMm(zTdg8`}*F?=ep*a*Zmg0tfnCemXe_pu30YMrGo-M z0IaPOoxD5%sDr{d;2Z%kqREgB0000r(9SrNJMr#}#G%wscG&AEIz>e~51cy+=|boI zKv!Q|n*z-IVxS_`&Rz5T-Ro<0bcW~775R!QBt5YZlNc7;+03x2jt&j|+9AWGk!WXj zW-h`rlzXtO5##0|)Zj-x6Xz)R>}#bGz`T_W^ntBjc~m&g>ZO}7c!HJl84ivJ7Zr$B zMX7LjREqYO0H3;QtJ)bFb2CgBnkM^Y=c`|~>IsvuJN&kCsLw(81F}i4xZd2tnn0f; z**M&u?A{eDFliPw6TDB8-V&eCGR{IDE80JK9AuS7+O9Y3x>4J-!t7jJt>kR%8o z^D81mC*tSlWQgCB0f5FgT~Pp_zO9oH&gE$8pRttv^(0Ua06yx_K)ZY30MMV`|L6Eh z8-d?+& zJ*P@wrWSz&YMX;Z4Ht`=`o}vj7x5>I143T*mYP44NZr(5yL3!qskprEwwUcy!I|k| zUWn6Z88)?&Qc2)otUb=U(W?V;tJ$2aRCBK#)^qw(J2MlrQkXY>%1(Ug4ED{G&;dW$ z0f^@EBd^GjJ?5Ny00VHegEId1;>~O(Tfta$khY15C zQe8V6O^O9Q$EaF{#OqDBg)3Dv3;{jowa#{d-Fx?|+IxL0Z_M_%I6vZG;4*Z+?c5!2 zDb6CC`CpTNnpvdP5QsW#K>>(d$N(H&2~ii@uHNmj&PU#(A0dC2P~$5m=vW@)Q9 zPwtRWJ#w?TOFRd1nY}Xq)XmiIZUvLW_a)ymWXip|TKTceMsW;b=vAJqx-jW9#7%cX zCd%4>*In`v<)VU*dKpJgGv#W0;doA4*;0gyw5hO7Zgp=w=iPMWag5^*Qw&45>We0~ z=cDzMSMDzYrA}wv%PjCXKRx|A+fkl!?>+9;C?yjE}7Jf%lmEXqyDM#zxG* zZf|&SimMLSI@5ZSHEWB?M@94H*HJ@c614s<1gv? zCnx1^)?c;cEvJyO%U8%aDQ>fdP=H^wY_^u?w?QqU&DFDb}JCI`8m3)d0^0KK&lm12e-9nis_;?y%X{BVM(9lR$zcUH{~lQzfru#>${pPHHqRtU(L)}rkoTXgr)_CZ()9S&Ba zm&Dj(<3=jcnUQ#_+sRf8p(6?%6)vB4=a!7puZaiTHQD^^eB8uu762*!9 z^W}XmoRjI|@YrQkqYChQq<@3{*!<_{rRcA^3!WPySH-%ntK&BwPkt$Skbl38ZN%gf zFs)lNaQzx#=X3JL($h^IL#CcI! z_?*W2o_$r@v_6wDRVQ1w=dQdS2gAgx=;5-@~)jD zlU2Hs<Z54}87fR(8Wf`tVA_~{a?R*{>d<2eu+xDo% zU90yz7y*@vD!w(YROvOqN~H!0l!+u7Nd?Si)bKCyPAV#&KM$at6E6;aH$l16*wFT1 z@E8ysBx;Uz6;WZ$8%)x9p=nTkxbKKNon%r~Y~N^xk4hX)3_kUq{5inV^_u4NY3H~# zx!rxQ&P0I-;nkpoQfh0oAG)`EZ|Q7BaaMI5;UJTCzrJ5mQttY(^%Ny1^Z`0GcZ`O+ z;#$DcGCts@gcI8dEZqm)Pyu09Hi}0KirHN(7=hR$u^fJAue8LMD`=ffkEa2 z2{0}enQ!#Gv6-IOx#bSxBiR=nC3HV;%tgWb{7Y&4MLcUXLg z5VdgE{y0pB4iQ@1@KH@nZO@=81hct!SZ3bngw0(&8?2vsTv}54k9$pfQyA+B+(eZ_;2%-!slm{_N z_RL}1>-xvXk|}PdD@Vj`JhWcvk6!$pq8K7 zAKcB|4ZN4iGCJh6(d?^ssu;@TlyfFC1S1y6m@cq!R=rWfc#VH-=UV8QWYvAj*wj-` zHV%aQ*s`Av6bHgqrz+)#6=UWFpD+#3b6;o1rmv zwl=7~f3gLYYnbNG_}~`P#B0|Gt6=+a#wF-$6*}qR{kmjF*(UZJLW?R(*HtUYCbilG zOp&mH{@~~E+zp9v$yTv4dya=W&5Sb=B~uJFwj2gtq1^&I>YmJ@xgIzhGq44XzBS7|q27eKgUqM*OrDoI{${;8ho9qM0axOc@W?PZKa9r2vblrtJp-$o$2nkkN{RzC%rD@8bWEG< zmI~EabZ=5%ICv+5;CFs6JyGrut5^rqI#50ob(GVqYxG?de`H`p<}6C^0~wwTT(S15 zQGdvhr^~}0NHNk1rxRtfsd8nTTLE8|%nWvFpz&yDGk&MG%xnCvtm8m)I9Q-@*Gv^k zzn!$W9KPsq02O6y820^Lv}P?0RcD9-OA_Ssh(Ls+?V(1)xZo49tW1N| zC%pO_s1>6N^7#?|TwAp#_eQ#$4@v=z21Ky;w$IZg6A~(z>!L z+UGgk#6SFz7HN@Ly((E;m+$bzl#3&)`hz`dqin>$*2*HnuY^9(v31lYzP4n%>gt@N zlwZ|EchAAq!jh3&MpRs} znYF`*4Y5)4N+HqK8Sig9ahcXe@k`%kf1Io~$~Yqkd$lbnHdsHzy01D^hXd?NL_1WkBMc0?Z~jowfyN}6+g3%OB{S@(SbuT zOeJ*>8N`kEf})ssUVSY!JJYVtxR=RyN-E+5eY)EPhW@OAq-lA!e%k5JP=&0iu+QY#tcP{OJ-8QDb86V_E+>J#P3nlB9v?oTwudzn6xgc7CJH!;icp5Z2R*%W?eQmx) z=%nykkCS$7V%ay?jgihCNN@H>ElA?rvE)9!9j(TKK%6bI)XawFTd-8`fXB+-v($^1 zW@SP8EFQEz7-o-`JKp_J9_{9rJD~J{2{C)!Rvm5ffwK0Pb+b%^JBv^Wg_Ey2z`Ai~ z@Z_7uxxO*6&)Ej2#P9GU?VJ$>CORecmrsoA#jw6UgO zJ&nS)mJEK}`10Ei%HaZkEOTn_+s5U^CXW>eG|JXlMu9_>3g{obu{Rk~VKZYL%#E<; z&}~mmisXkH^4UqB4lYV@w*0Exb^H96`Fi_V=$&1Ihh6RD54Da9R=HDhm^0C1-t~Vw z7!cVaP^}O|&L2OZ5P%0o{_`|=bjbU89F&G4z`qZKf1Zke9rFNS5_kB&K?W;>9!wj6 zxnYgNSBEL#@!h(pPzjU8j&MceT0UhPw*;pckmA#e-X-2qRu zR^RDUKn|)!jqAZ{jG}E`pzU-hj^R>byBk9xE8AmZ_R{ z%hUT%TxmC{d+Ag%SLC&S(f=3AAs(k8f06$esvu#9ADn;$8Prf7cJ63b9M=5@3Lu*F zkS;`sa9KtFvYL^&8p;WI#nTAoj<)~ppn-HjJKqGH`_TtD>Jx^{p07yfAyQ-*Q zy#W>gNg22-KvEhC2SC6e7yt|g$pWm7oV0LAXSCe~jDs@@06MZ<@UTM>;fE{)anvL& zkkpnW@f8vwP(`|GqRWFmzrGO6dXB!Ouv6;Us4gmkj z*wisZtcJ!o0Dz`w%ms`G`j_EoYE$%eoTCR(NNKQ)6hek5A1TQHUvGpAXtm0m}klvP8xx3nz^zEkm?|Wxxm$umTZxA}fV} zKp{k@KWBo2AjCmHG6+(zKme!=LJ9SWVq>n=IZ@`L}Our$hNrJ(R zRuFp_VOPArClxg~cO@f(o$m?2jzP_z+m{Ai6gHG3#JfiLT2OO`h30LXhetm@CCiX> zvOu@8c9dh-wi64-@!_?a*P2XKqf+GYFJSlDs39~wtFqY{ixx~#@O0#ijqZrJ3)raY zF;&v#OqyP)cX&)j0mY+7pQU2mBOiP6#0{89`rG|s=o4%HtY7V^(2IpI_wFpUgL7)~ z_xCzlDJ;VGL!O+UrRkztDKFK)^fA5S@T;=CRris-enSFp_IAVf#tx%8%WY8H8H$X- z$l`mCmV#62^|m;8?Du5}#0t|ovAU`UMAr?z46~EajPzz&^1%OE@dz2%;188Ssvvrsi85A9KObrBv zyo7*2kQEAA4SYN&b-Zvd<#Z-FtB#_w%It}Vdo)6nxyouR@yrd!1r$|ukd!RFF}5F- l!H*TGsK{pXXfyt~a}ONS9p~*%+!Rbk21+L^tgf#?_a7qEC&T~% diff --git a/rmcs_ws/src/rmcs_bringup/config/flight.yaml b/rmcs_ws/src/rmcs_bringup/config/flight.yaml index cf8ef4eb..f7d7ac40 100644 --- a/rmcs_ws/src/rmcs_bringup/config/flight.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/flight.yaml @@ -9,8 +9,6 @@ rmcs_executor: - rmcs_core::controller::pid::ErrorPidController -> yaw_angle_pid_controller - rmcs_core::controller::pid::PidController -> yaw_velocity_pid_controller - rmcs_core::controller::pid::ErrorPidController -> pitch_angle_pid_controller - #- rmcs_core::controller::gimbal::GimbalFrequencySweep -> gimbal_frequency - - rmcs_core::controller::shooting::FrictionWheelController -> friction_wheel_controller - rmcs_core::controller::shooting::HeatController -> heat_controller @@ -95,13 +93,6 @@ pitch_angle_pid_controller: ki: 0.0 kd: 0.0 -# gimbal_frequency: -# ros__parameters: -# F_start: 5 -# F_end: 50 -# Repeat_time: 20 - - friction_wheel_controller: ros__parameters: friction_wheels: diff --git a/rmcs_ws/src/rmcs_core/plugins.xml b/rmcs_ws/src/rmcs_core/plugins.xml index 3a54e23b..02efadb1 100644 --- a/rmcs_ws/src/rmcs_core/plugins.xml +++ b/rmcs_ws/src/rmcs_core/plugins.xml @@ -38,9 +38,6 @@ Test plugin. - - Test plugin. - Test plugin. diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/GimbalFrequencySweep.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/GimbalFrequencySweep.cpp deleted file mode 100644 index e661b0b0..00000000 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/GimbalFrequencySweep.cpp +++ /dev/null @@ -1,197 +0,0 @@ -// #include -// #include -// #include - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include // 用于时间格式化 -#include // 添加文件流支持 -#include // 添加格式化支持 -#include // 用于创建目录 - -namespace rmcs_core::controller::gimbal { - -using namespace rmcs_description; - -class GimbalFrequencySweep - : public rmcs_executor::Component - , public rclcpp::Node { -public: - GimbalFrequencySweep() - : Node("gimbal_frequency_sweep") { - // 创建数据存储目录 - create_data_directory(); - // 初始化CSV文件 - init_csv_file(); - - register_input("/remote/switch/right", switch_right_); - - register_input("/remote/switch/left", switch_left_); - register_input("/gimbal/yaw/velocity", yaw_velocity_); - register_input("/gimbal/yaw/max_torque", yaw_max_torque_); - - register_output("/gimbal/yaw/control_torque", yaw_control_torque_); - - // 参数配置 - } - - ~GimbalFrequencySweep() { - if (csv_file_.is_open()) { - csv_file_.close(); - RCLCPP_INFO(get_logger(), "CSV file closed: %s", filename_.c_str()); - } - } - - void update() override { - using namespace rmcs_msgs; - - auto switch_right = *switch_right_; - auto switch_left = *switch_left_; - - do { - if (switch_left == Switch::DOWN && switch_right == Switch::MIDDLE) { - - Now_time = this->now(); - if (f_now > F_end) { - break; - } - if (last_time <= 0.0002) { - last_time = Now_time.seconds(); - } - now_time = Now_time.seconds(); - - current = Top * std::sin(2 * Pi * f_now * (now_time - last_time)); - double torque_command = current * k; - *yaw_control_torque_ = torque_command; - if ((now_time - last_time) > ((1 / f_now) * Repeat_time)) { - if (f_now < 24) { - f_now += 0.5; - } else if (f_now >= 24 && f_now <= 120) { - f_now += 2; - } else { - f_now += 4; - } - last_time = Now_time.seconds(); - } - record_data(); // gimbal_yaw_motor_.control_torque() - } - if (switch_left == Switch::DOWN && switch_right == Switch::DOWN) { - reset_status(); - } - - } while (false); - } - -private: - void create_data_directory() { - // 获取当前用户的主目录 - const char* home_dir = std::getenv("HOME"); - if (!home_dir) { - RCLCPP_ERROR(get_logger(), "Failed to get home directory"); - return; - } - - // 创建数据目录 - data_dir_ = std::string(home_dir) + "/gimbal_sweep_data"; - if (mkdir(data_dir_.c_str(), 0777) == -1) { - if (errno != EEXIST) { - RCLCPP_ERROR(get_logger(), "Failed to create data directory: %s", strerror(errno)); - } else { - RCLCPP_INFO(get_logger(), "Using existing data directory: %s", data_dir_.c_str()); - } - } else { - RCLCPP_INFO(get_logger(), "Created data directory: %s", data_dir_.c_str()); - } - } - - void init_csv_file() { - // 生成带时间戳的文件名 - auto now = std::time(nullptr); - std::tm now_tm = *std::localtime(&now); - - std::ostringstream oss; - oss << data_dir_ << "/sweep_" << std::put_time(&now_tm, "%Y%m%d_%H%M%S") << ".csv"; - filename_ = oss.str(); - - csv_file_.open(filename_, std::ios::out); - if (!csv_file_.is_open()) { - RCLCPP_ERROR(get_logger(), "Failed to open CSV file: %s", filename_.c_str()); - return; - } - - // 写入CSV表头 - csv_file_ << "actual_velocity control_torque\n"; - csv_file_.flush(); - - RCLCPP_INFO(get_logger(), "Data recording started. File: %s", filename_.c_str()); - } - - void record_data() { - if (!csv_file_.is_open()) - return; - - // 获取当前时间 - auto now = this->now(); - // double elapsed = (last_time > 0) ? now.seconds() - last_time : 0.0; - - // 写入数据 - csv_file_ << *yaw_velocity_ << " " << current << "\n"; - RCLCPP_INFO(get_logger(), "%d", i++); - // 定期刷新缓冲区以确保 - static int count = 0; - if (++count % 100 == 0) { - csv_file_.flush(); - } - } - - void reset_status() { - f_now = 1.0; - *yaw_control_torque_ = 0.0; - } - - rclcpp::Time Now_time; - - double now_time; - double last_time = 0.0; - double current; - - float f_now = 1.0; - int i = 0; - - static constexpr int F_start = 1; - static constexpr int F_end = 50; - static constexpr int Repeat_time = 20; - static constexpr double Pi = 3.1415926; - static constexpr double Top = 0.999; - static constexpr double k = 0.1; - // ROS接口 - - rclcpp::TimerBase::SharedPtr control_timer_; - - // 数据接口 - InputInterface yaw_velocity_; - InputInterface yaw_max_torque_; - OutputInterface yaw_current_command_; - OutputInterface yaw_control_torque_; - - InputInterface switch_right_; - InputInterface switch_left_; - - std::string data_dir_; - std::string filename_; - std::ofstream csv_file_; -}; -} // namespace rmcs_core::controller::gimbal - -#include - -PLUGINLIB_EXPORT_CLASS( - rmcs_core::controller::gimbal::GimbalFrequencySweep, rmcs_executor::Component) \ No newline at end of file