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/4] 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 000000000..183835e74 --- /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 ac7d1368b..4d3bb141b 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 f20bd98fe..65417862b 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 d27704d52..517dc0fcf 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 5d937aae0..ae590f103 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 000000000..af218545e --- /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 bb555662320d8251271852d03ad9ffa5be2826be Mon Sep 17 00:00:00 2001 From: Embers-of-the-Fire Date: Sat, 14 Feb 2026 19:16:30 +0800 Subject: [PATCH 2/4] Merge downstream to branch `gimbal` --- .clangd | 4 + .github/workflows/update-image.yml | 49 ------------ .gitmodules | 6 ++ .script/complete/_play-autoaim | 7 ++ .script/complete/_set-remote | 22 ++++++ .script/foxglove | 5 ++ .script/play-autoaim | 74 +++++++++++++++++++ .script/sync-remote | 2 +- .script/template/env_setup.bash | 2 +- .script/template/env_setup.zsh | 8 +- DEVELOPMENT.md | 8 ++ Dockerfile | 42 ++++++++--- README.md | 2 + docker-compose.yml | 13 +--- ip.conf | 1 + rmcs_ws/src/rmcs_bringup/config/flight.yaml | 62 ++++++++++------ rmcs_ws/src/rmcs_core/package.xml | 1 + rmcs_ws/src/rmcs_core/plugins.xml | 3 + .../gimbal/two_axis_gimbal_solver.hpp | 23 ++++-- rmcs_ws/src/rmcs_core/src/hardware/flight.cpp | 31 ++++---- .../include/rmcs_executor/component.hpp | 32 ++++---- 21 files changed, 268 insertions(+), 129 deletions(-) create mode 100644 .clangd delete mode 100644 .github/workflows/update-image.yml create mode 100644 .script/complete/_play-autoaim create mode 100644 .script/complete/_set-remote create mode 100755 .script/foxglove create mode 100755 .script/play-autoaim create mode 100644 DEVELOPMENT.md create mode 100644 ip.conf diff --git a/.clangd b/.clangd new file mode 100644 index 000000000..5ba2064ee --- /dev/null +++ b/.clangd @@ -0,0 +1,4 @@ +CompileFlags: + Add: + - --target=x86_64-linux-gnu + \ No newline at end of file diff --git a/.github/workflows/update-image.yml b/.github/workflows/update-image.yml deleted file mode 100644 index 71c5538fe..000000000 --- a/.github/workflows/update-image.yml +++ /dev/null @@ -1,49 +0,0 @@ -name: Build and Push RMCS Images - -on: - workflow_dispatch: - push: - paths: - - 'Dockerfile' - branches: - - main - -jobs: - build-and-push: - runs-on: ubuntu-latest - - steps: - - name: Checkout repository - uses: actions/checkout@v4 - - - name: Set up Docker Buildx - uses: docker/setup-buildx-action@v3 - - - name: Log in to Docker Hub - uses: docker/login-action@v3 - with: - username: ${{ secrets.DOCKERHUB_USERNAME }} - password: ${{ secrets.DOCKERHUB_TOKEN }} - - - name: Set up SSH keys - run: | - echo "${{ secrets.CONTAINER_ID_RSA }}" > .ssh/id_rsa - echo "${{ secrets.CONTAINER_ID_RSA_PUB }}" > .ssh/id_rsa.pub - chmod 600 .ssh/id_rsa - chmod 644 .ssh/id_rsa.pub - - - name: Build and push rmcs-develop:latest - uses: docker/build-push-action@v6 - with: - context: . - push: true - target: rmcs-develop - tags: qzhhhi/rmcs-develop:latest - - - name: Build and push rmcs-runtime:latest - uses: docker/build-push-action@v6 - with: - context: . - push: true - target: rmcs-runtime - tags: qzhhhi/rmcs-runtime:latest diff --git a/.gitmodules b/.gitmodules index d4d71be28..6706448a9 100644 --- a/.gitmodules +++ b/.gitmodules @@ -10,3 +10,9 @@ [submodule "rmcs_ws/src/serial"] path = rmcs_ws/src/serial url = https://github.com/Alliance-Algorithm/ros2-serial.git +[submodule "rmcs_ws/src/rmcs_slam"] + path = rmcs_ws/src/rmcs_slam + url = https://github.com/Alliance-Algorithm/rmcs_slam +[submodule "rmcs_ws/src/livox_ros_driver2"] + path = rmcs_ws/src/livox_ros_driver2 + url = https://github.com/Alliance-Algorithm/livox_ros_driver2.git diff --git a/.script/complete/_play-autoaim b/.script/complete/_play-autoaim new file mode 100644 index 000000000..025a0974c --- /dev/null +++ b/.script/complete/_play-autoaim @@ -0,0 +1,7 @@ +#compdef play-autoaim + +_arguments \ + '--user[指定远程用户名]:用户名:' \ + '--remote[从设备拉取 SDP 到容器]' \ + '--no-copy[跳过容器到宿主机的拷贝]'\ + '--ip[监视端的 IP 地址]' diff --git a/.script/complete/_set-remote b/.script/complete/_set-remote new file mode 100644 index 000000000..e5599c3f4 --- /dev/null +++ b/.script/complete/_set-remote @@ -0,0 +1,22 @@ +#compdef set-remote + +_remote_hosts() { + local hosts=( + "169.254.233.233" + "alliance-sentry.local" + "alliance-infantry.local" + "alliance-hero.local" + ) + + # 从 ~/.ssh/config 提取最近使用的 HostName + if [[ -f ~/.ssh/config ]]; then + local extracted + extracted=(${(f)"$(grep -A1 'Host remote' ~/.ssh/config | grep HostName | awk '{print $2}')"}) + hosts+=(${extracted}) + fi + + compadd -- $hosts +} + +_arguments \ + '1:远程主机地址:_remote_hosts' diff --git a/.script/foxglove b/.script/foxglove new file mode 100755 index 000000000..ba845e52e --- /dev/null +++ b/.script/foxglove @@ -0,0 +1,5 @@ +#! /bin/bash + +source ~/env_setup.bash + +ros2 launch foxglove_bridge foxglove_bridge_launch.xml port:=8765 diff --git a/.script/play-autoaim b/.script/play-autoaim new file mode 100755 index 000000000..b9a33f0f5 --- /dev/null +++ b/.script/play-autoaim @@ -0,0 +1,74 @@ +#!/bin/sh + +set -e + +MONITOR_USER="" +MONITOR_PLAYER="vlc" +MONITOR_HOST="localhost" + +SDP_PATH="/tmp/auto_aim.sdp" + +USE_REMOTE=false +SKIP_COPY=false + +# 参数解析 +while [ "$#" -gt 0 ]; do + case "$1" in + --user) + shift + [ -z "$1" ] && echo "❌ --user 参数缺失" && exit 1 + MONITOR_USER="$1" + ;; + --remote) + USE_REMOTE=true + ;; + --no-copy) + SKIP_COPY=true + ;; + --ip) + shift + [ -z "$1" ] && echo "❌ --ip 参数缺失" && exit 1 + MONITOR_HOST="$1" + ;; + *) + echo "未知参数: $1" + echo "用法: $0 --user <用户名> [--remote] [--no-copy]" + exit 1 + ;; + esac + shift +done + +if [ -z "$MONITOR_USER" ]; then + echo "❌ 缺少 --user 参数" + echo "用法: play-autoaim --user <用户名> [--remote] [--no-copy]" + exit 1 +fi + +if [ "$USE_REMOTE" = true ]; then + scp remote:${SDP_PATH} ${SDP_PATH} + if [ $? -ne 0 ]; then + echo " 从 remote 拷贝失败。是否继续？(y/n)" + read -r answer + case "$answer" in + [Yy]*) echo " 继续执行后续操作…" ;; + *) + echo " 已取消。" + exit 1 + ;; + esac + fi +fi + +if [ "$SKIP_COPY" = false ]; then + scp "${SDP_PATH}" "${MONITOR_USER}@${MONITOR_HOST}:${SDP_PATH}" + if [ $? -ne 0 ]; then + echo " scp 拷贝失败" + exit 1 + fi + echo " 文件已拷贝到宿主机：${SDP_PATH}" +else + echo " 跳过拷贝，直接打开远程 SDP" +fi + +ssh -f "${MONITOR_USER}@${MONITOR_HOST}" "DISPLAY=:0 ${MONITOR_PLAYER} '${SDP_PATH}' --network-caching=50" diff --git a/.script/sync-remote b/.script/sync-remote index f9c915fc8..7d62782b9 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 = os.getenv("RMCS_PATH") + "/rmcs_ws/install" DST_DIR = "ssh://remote//rmcs_install" SOCKET_PATH = "/tmp/sync-remote" diff --git a/.script/template/env_setup.bash b/.script/template/env_setup.bash index f8ab1dd27..341647d3b 100644 --- a/.script/template/env_setup.bash +++ b/.script/template/env_setup.bash @@ -1,6 +1,6 @@ #!/bin/bash -export ROS_LOCALHOST_ONLY=1 +export ROS_AUTOMATIC_DISCOVERY_RANGE=LOCALHOST export RCUTILS_COLORIZED_OUTPUT=1 export RMCS_PATH="/workspaces/RMCS" diff --git a/.script/template/env_setup.zsh b/.script/template/env_setup.zsh index 335250c90..fab840df0 100644 --- a/.script/template/env_setup.zsh +++ b/.script/template/env_setup.zsh @@ -1,7 +1,7 @@ -#!/bin/bash +#!/bin/zsh +export ROS_AUTOMATIC_DISCOVERY_RANGE=LOCALHOST export RCUTILS_COLORIZED_OUTPUT=1 -export ROS_LOCALHOST_ONLY=1 export RMCS_PATH="/workspaces/RMCS" source /opt/ros/jazzy/setup.zsh @@ -16,3 +16,7 @@ eval "$(register-python-argcomplete ros2)" eval "$(register-python-argcomplete colcon)" export RMCS_ROBOT_TYPE="" + +fpath=(${RMCS_PATH}/.script/complete $fpath) +autoload -Uz compinit +compinit diff --git a/DEVELOPMENT.md b/DEVELOPMENT.md new file mode 100644 index 000000000..c1158a378 --- /dev/null +++ b/DEVELOPMENT.md @@ -0,0 +1,8 @@ +# Development of UAV + +This branch contains runtime code for the UAV. + +## External Dependencies + +Flight controller takes advantage of the SLAM runtime [`rmcs_slam`](https://Alliance-Algorithm/rmcs_slam). +Remember to sync the submodule. diff --git a/Dockerfile b/Dockerfile index c4a67af1a..22ba59bda 100644 --- a/Dockerfile +++ b/Dockerfile @@ -18,6 +18,7 @@ RUN apt-get update && apt-get install -y --no-install-recommends \ vim wget curl unzip \ zsh screen tmux \ usbutils net-tools iputils-ping \ + gstreamer1.0-tools gstreamer1.0-plugins-base gstreamer1.0-plugins-good \ ripgrep htop fzf \ libusb-1.0-0-dev \ libeigen3-dev \ @@ -29,12 +30,18 @@ RUN apt-get update && apt-get install -y --no-install-recommends \ libceres-dev \ ros-$ROS_DISTRO-rviz2 \ ros-$ROS_DISTRO-foxglove-bridge \ + ros-$ROS_DISTRO-pcl-conversions \ + ros-$ROS_DISTRO-pcl-ros \ + libpcl-ros-dev \ + libpcl-dev \ + libgomp-dev \ + libomp-dev \ + libomp-22-dev \ dotnet-sdk-8.0 \ ros-$ROS_DISTRO-pcl-ros ros-$ROS_DISTRO-pcl-conversions ros-$ROS_DISTRO-pcl-msgs && \ apt-get autoremove -y && apt-get clean && \ rm -rf /var/lib/apt/lists/* /tmp/* - # Install openvino runtime RUN wget https://apt.repos.intel.com/intel-gpg-keys/GPG-PUB-KEY-INTEL-SW-PRODUCTS.PUB && \ apt-key add ./GPG-PUB-KEY-INTEL-SW-PRODUCTS.PUB && \ @@ -81,19 +88,25 @@ RUN apt-get update && apt-get install -y --no-install-recommends \ openssh-client \ lsb-release software-properties-common gnupg sudo \ python3-colorama python3-dpkt && \ - wget -O ./llvm-snapshot.gpg.key https://apt.llvm.org/llvm-snapshot.gpg.key && \ - apt-key add ./llvm-snapshot.gpg.key && \ - rm ./llvm-snapshot.gpg.key && \ - echo "deb https://apt.llvm.org/noble/ llvm-toolchain-noble main" > /etc/apt/sources.list.d/llvm-apt.list && \ - apt-get update && \ - version=`apt-cache search clangd- | grep clangd- | awk -F' ' '{print $1}' | sort -V | tail -1 | cut -d- -f2` && \ - apt-get install -y --no-install-recommends clangd-$version && \ update-alternatives --install /usr/bin/gcc gcc /usr/bin/gcc-14 50 && \ update-alternatives --install /usr/bin/g++ g++ /usr/bin/g++-14 50 && \ - update-alternatives --install /usr/bin/clangd clangd /usr/bin/clangd-$version 50 && \ apt-get autoremove -y && apt-get clean && \ rm -rf /var/lib/apt/lists/* /tmp/* +# Install latest stable llvm-toolchain +RUN mkdir -p /etc/apt/keyrings && \ + wget -O - https://apt.llvm.org/llvm-snapshot.gpg.key | gpg -o /etc/apt/keyrings/llvm-snapshot.gpg --dearmor && \ + echo "deb [signed-by=/etc/apt/keyrings/llvm-snapshot.gpg] https://mirrors.tuna.tsinghua.edu.cn/llvm-apt/noble/ llvm-toolchain-noble-22 main" \ + | tee /etc/apt/sources.list.d/llvm.list && \ + apt-get update && \ + apt-get install -y --no-install-recommends clang-22 clangd-22 clang-format-22 lldb-22 && \ + update-alternatives --install /usr/bin/clang clang /usr/bin/clang-22 100 && \ + update-alternatives --install /usr/bin/clang++ clang++ /usr/bin/clang++-22 100 && \ + update-alternatives --install /usr/bin/clangd clangd /usr/bin/clangd-22 100 && \ + update-alternatives --install /usr/bin/clang-format clang-format /usr/bin/clang-format-22 100 && \ + update-alternatives --install /usr/bin/lldb lldb /usr/bin/lldb-22 100 && \ + apt-get autoremove -y && apt-get clean && rm -rf /var/lib/apt/lists/* /tmp/* + # Generate/load ssh key and setup unison RUN --mount=type=bind,target=/tmp/.ssh,source=.ssh,readonly=false \ cd /home/ubuntu && mkdir -p .ssh && \ @@ -110,6 +123,11 @@ RUN curl -LO https://github.com/neovim/neovim/releases/latest/download/nvim-linu tar -C /opt -xzf nvim-linux-x86_64.tar.gz && \ rm nvim-linux-x86_64.tar.gz +# Install latest stable cmake for user ubuntu +RUN wget https://github.com/kitware/cmake/releases/download/v4.2.3/cmake-4.2.3-linux-x86_64.sh -O install.sh && \ + mkdir -p /opt/cmake/ && bash install.sh --skip-license --prefix=/opt/cmake/ --exclude-subdir && \ + rm install.sh + # Change user RUN chsh -s /bin/zsh ubuntu && \ echo "ubuntu ALL=(ALL:ALL) NOPASSWD:ALL" >> /etc/sudoers @@ -123,12 +141,14 @@ RUN sh -c "$(wget https://raw.githubusercontent.com/ohmyzsh/ohmyzsh/master/tools sed -i 's/ZSH_THEME=\"[a-z0-9\-]*\"/ZSH_THEME="af-magic"/g' ~/.zshrc && \ echo 'source ~/env_setup.zsh' >> ~/.zshrc && \ echo 'export PATH="${PATH}:/opt/nvim-linux-x86_64/bin"' >> ~/.zshrc && \ + # echo 'export PATH="${PATH}:/opt/cmake/bin"' >> ~/.zshrc && \ echo 'export PATH="${PATH}:${RMCS_PATH}/.script"' >> ~/.zshrc # Copy environment setup scripts COPY --chown=1000:1000 .script/template/env_setup.bash env_setup.bash COPY --chown=1000:1000 .script/template/env_setup.zsh env_setup.zsh + # Runtime container, will automatically launch the main program FROM rmcs-base AS rmcs-runtime @@ -150,6 +170,10 @@ RUN sh -c "$(wget https://raw.githubusercontent.com/ohmyzsh/ohmyzsh/master/tools echo 'export PATH=${PATH}:/rmcs_install/lib/rmcs_cli' >> ~/.zshrc && \ chsh -s /bin/zsh root +# Remove git proxy config to avoid issues when interacting with docker +RUN git config --global --unset http.proxy || true && \ + git config --global --unset https.proxy || true + RUN mkdir -p /rmcs_install/ COPY --chown=root:root .script/set-robot /usr/local/bin/set-robot diff --git a/README.md b/README.md index 1bb950ab6..dc365f69b 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,8 @@ # RMCS RoboMaster Control System based on ROS2. +快速开始: [quick-start](https://github.com/Alliance-Algorithm/RMCS/wiki/Quick-Start) + ## Development ### Pre-requirements: diff --git a/docker-compose.yml b/docker-compose.yml index d59afc5bf..43ec6c74d 100644 --- a/docker-compose.yml +++ b/docker-compose.yml @@ -1,17 +1,11 @@ services: rmcs-develop: image: qzhhhi/rmcs-develop:latest - user: "${CONTAINER_USER}" - privileged: true - command: > - bash -c " - sudo chown -R ${CONTAINER_USER}:${CONTAINER_USER} ${CONTAINER_HOME}/.config - exec bash - " + user: "1000:1000" volumes: - /dev:/dev:bind - /tmp/.X11-unix:/tmp/.X11-unix:bind - - /run/user/1000/wayland-0:/run/user/1000/wayland-0 + - /run/user/1000/wayland-0:/run/user/1000/wayland-0:bind - ${HOST_NVIM_DIR}:${CONTAINER_HOME}/.config/nvim/:bind - .:/workspaces/RMCS:bind environment: @@ -19,5 +13,4 @@ services: - WAYLAND_DISPLAY=${WAYLAND_DISPLAY} network_mode: host tty: true - stdin_open: true - + stdin_open: true diff --git a/ip.conf b/ip.conf new file mode 100644 index 000000000..24dd82fad --- /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 183835e74..f7d7ac40a 100644 --- a/rmcs_ws/src/rmcs_bringup/config/flight.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/flight.yaml @@ -9,7 +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::pid::PidController -> pitch_velocity_pid_controller - rmcs_core::controller::shooting::FrictionWheelController -> friction_wheel_controller - rmcs_core::controller::shooting::HeatController -> heat_controller @@ -18,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,54 +26,70 @@ rmcs_executor: # - rmcs_auto_aim::AutoAimInitializer -> auto_aim_initializer # - 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 + - /gimbal/yaw/control_torque + - /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: 0 - pitch_motor_zero_point: 0 + yaw_motor_zero_point: 4608 + pitch_motor_zero_point: 51629 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.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: 3.0 + 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: 0.5 + kp: 1.5 ki: 0.0 - kd: 0.0 + kd: 0.01 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 +169,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/package.xml b/rmcs_ws/src/rmcs_core/package.xml index 410745520..3b401799c 100644 --- a/rmcs_ws/src/rmcs_core/package.xml +++ b/rmcs_ws/src/rmcs_core/package.xml @@ -19,6 +19,7 @@ rmcs_msgs rmcs_executor rmcs_description + rmcs_slam ament_lint_auto ament_lint_common diff --git a/rmcs_ws/src/rmcs_core/plugins.xml b/rmcs_ws/src/rmcs_core/plugins.xml index 4d3bb141b..02efadb10 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/two_axis_gimbal_solver.hpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/two_axis_gimbal_solver.hpp index ae590f103..b218e27f0 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 af218545e..5d741135e 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,7 +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( @@ -53,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()); @@ -81,7 +83,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}); @@ -111,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_can1_transmission(0x142, 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_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)); + + transmit_buffer_.trigger_transmission(); } private: @@ -146,7 +151,7 @@ class Flight 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) { @@ -167,8 +172,6 @@ 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); } } @@ -180,9 +183,11 @@ class Flight 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 == 0x202) { + gimbal_right_friction_.store_status(can_data); + } else if (can_id == 0x144) { + gimbal_pitch_motor_.store_status(can_data); } } diff --git a/rmcs_ws/src/rmcs_executor/include/rmcs_executor/component.hpp b/rmcs_ws/src/rmcs_executor/include/rmcs_executor/component.hpp index cba9b55fa..9155e7a9a 100644 --- a/rmcs_ws/src/rmcs_executor/include/rmcs_executor/component.hpp +++ b/rmcs_ws/src/rmcs_executor/include/rmcs_executor/component.hpp @@ -16,12 +16,12 @@ class Component { public: friend class Executor; - Component(const Component&) = delete; + Component(const Component&) = delete; Component& operator=(const Component&) = delete; - Component(Component&&) = delete; - Component& operator=(Component&&) = delete; + Component(Component&&) = delete; + Component& operator=(Component&&) = delete; - virtual ~Component(){}; + virtual ~Component() = default; virtual void before_pairing(const std::map& output_map) { (void)output_map; @@ -37,10 +37,10 @@ class Component { InputInterface() = default; - InputInterface(const InputInterface&) = delete; + InputInterface(const InputInterface&) = delete; InputInterface& operator=(const InputInterface&) = delete; - InputInterface(InputInterface&&) = delete; - InputInterface& operator=(InputInterface&&) = delete; + InputInterface(InputInterface&&) = delete; + InputInterface& operator=(InputInterface&&) = delete; ~InputInterface() { if (delete_data_when_deconstruct) { @@ -61,7 +61,7 @@ class Component { throw std::runtime_error("The interface has already been bound to somewhere"); data_pointer_ = new T(std::forward(args)...); - activated = true; + activated = true; delete_data_when_deconstruct = true; } @@ -71,7 +71,7 @@ class Component { throw std::runtime_error("The interface has already been bound to somewhere"); data_pointer_ = const_cast(&destination); - activated = true; + activated = true; } const T* operator->() const { return data_pointer_; } @@ -84,7 +84,7 @@ class Component { } T* data_pointer_ = nullptr; - bool activated = false; + bool activated = false; bool delete_data_when_deconstruct = false; }; @@ -96,10 +96,10 @@ class Component { OutputInterface() = default; - OutputInterface(const OutputInterface&) = delete; + OutputInterface(const OutputInterface&) = delete; OutputInterface& operator=(const OutputInterface&) = delete; - OutputInterface(OutputInterface&&) = delete; - OutputInterface& operator=(OutputInterface&&) = delete; + OutputInterface(OutputInterface&&) = delete; + OutputInterface& operator=(OutputInterface&&) = delete; ~OutputInterface() { if (active()) @@ -136,6 +136,7 @@ class Component { } template + requires std::constructible_from void register_output(const std::string& name, OutputInterface& interface, Args&&... args) { if (interface.active()) throw std::runtime_error("The interface has been activated"); @@ -144,6 +145,7 @@ class Component { } template + requires std::constructible_from std::shared_ptr create_partner_component(const std::string& name, Args&&... args) { initializing_component_name = name.c_str(); @@ -182,8 +184,8 @@ class Component { std::vector> partner_component_list_; - size_t dependency_count_ = 0; + std::size_t dependency_count_ = 0; std::unordered_set wanted_by_ = {}; }; -} // namespace rmcs_executor \ No newline at end of file +} // namespace rmcs_executor From f99b7ed47844667e88d78cb4dab33682e2b580cd Mon Sep 17 00:00:00 2001 From: Embers-of-the-Fire Date: Sat, 14 Feb 2026 19:21:27 +0800 Subject: [PATCH 3/4] fix: fix logging import --- .../src/controller/gimbal/two_axis_gimbal_solver.hpp | 12 +----------- 1 file changed, 1 insertion(+), 11 deletions(-) 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 b218e27f0..a16aa2238 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 @@ -8,8 +8,6 @@ #include #include -#include -#include #include #include #include @@ -110,10 +108,6 @@ 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_}; @@ -124,7 +118,6 @@ 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); @@ -209,9 +202,6 @@ 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; } @@ -230,4 +220,4 @@ class TwoAxisGimbalSolver { OdomImu::DirectionVector control_direction_; }; -} // namespace rmcs_core::controller::gimbal \ No newline at end of file +} // namespace rmcs_core::controller::gimbal From e89679b6dd95b6fbbc2adfbc67b03f4b0b436750 Mon Sep 17 00:00:00 2001 From: Embers-of-the-Fire Date: Sat, 14 Feb 2026 19:30:00 +0800 Subject: [PATCH 4/4] fix: fix gimbal xz transform --- .../gimbal/two_axis_gimbal_solver.hpp | 24 ++++++++----------- 1 file changed, 10 insertions(+), 14 deletions(-) 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 a16aa2238..a8f3e7d82 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 @@ -173,21 +173,17 @@ class TwoAxisGimbalSolver { 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; - } + const auto& [yaw_x, yaw_y, yaw_z] = *control_direction; + + Eigen::Vector2d xz_projection{yaw_x, yaw_z}; + double xz_norm = xz_projection.norm(); + if (xz_norm > 0) + xz_projection /= xz_norm; - 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_; + if (yaw_y > yaw_upper_limit_.y()) + *control_direction << yaw_upper_limit_.x() * xz_projection.x(), yaw_upper_limit_.x(), yaw_upper_limit_.x() * xz_projection.y(); + else if (yaw_y < yaw_lower_limit_.y()) + *control_direction << yaw_lower_limit_.x() * xz_projection.x(), yaw_lower_limit_.x(), yaw_lower_limit_.x() * xz_projection.y(); }