From 737ea8274031c0b020b33436b15af5e5940637c5 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Mon, 11 May 2026 00:04:15 +0800 Subject: [PATCH 01/30] feat: add odin_ros_driver submodule --- .gitmodules | 3 +++ rmcs_ws/src/odin_ros_driver | 1 + 2 files changed, 4 insertions(+) create mode 160000 rmcs_ws/src/odin_ros_driver diff --git a/.gitmodules b/.gitmodules index 61e5f08f..20531eff 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,3 +1,6 @@ [submodule "rmcs_ws/src/fast_tf"] path = rmcs_ws/src/fast_tf url = https://github.com/qzhhhi/FastTF.git +[submodule "rmcs_ws/src/odin_ros_driver"] + path = rmcs_ws/src/odin_ros_driver + url = git@github.com:Alliance-Algorithm/odin_ros_driver.git diff --git a/rmcs_ws/src/odin_ros_driver b/rmcs_ws/src/odin_ros_driver new file mode 160000 index 00000000..dbcb0286 --- /dev/null +++ b/rmcs_ws/src/odin_ros_driver @@ -0,0 +1 @@ +Subproject commit dbcb0286b510142e966ac090a37ce5ca238f9e75 From 20575e85947c91d9de67b25ea1220edfb1a5c821 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Mon, 11 May 2026 00:11:19 +0800 Subject: [PATCH 02/30] feat: add hikcamera submodule --- .gitmodules | 3 +++ rmcs_ws/src/hikcamera | 1 + 2 files changed, 4 insertions(+) create mode 160000 rmcs_ws/src/hikcamera diff --git a/.gitmodules b/.gitmodules index 20531eff..4b443c64 100644 --- a/.gitmodules +++ b/.gitmodules @@ -4,3 +4,6 @@ [submodule "rmcs_ws/src/odin_ros_driver"] path = rmcs_ws/src/odin_ros_driver url = git@github.com:Alliance-Algorithm/odin_ros_driver.git +[submodule "rmcs_ws/src/hikcamera"] + path = rmcs_ws/src/hikcamera + url = git@github.com:Alliance-Algorithm/ros2-hikcamera.git diff --git a/rmcs_ws/src/hikcamera b/rmcs_ws/src/hikcamera new file mode 160000 index 00000000..f0077f03 --- /dev/null +++ b/rmcs_ws/src/hikcamera @@ -0,0 +1 @@ +Subproject commit f0077f034800bcd0dde4fffeff270b733772a57e From f0761a2024cb6046b5c27eb9ce3f9048447f6402 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Mon, 11 May 2026 00:57:18 +0800 Subject: [PATCH 03/30] feat: add rmcs_auto_aim_v2 submodule --- .gitmodules | 3 +++ rmcs_ws/src/rmcs_auto_aim_v2 | 1 + 2 files changed, 4 insertions(+) create mode 160000 rmcs_ws/src/rmcs_auto_aim_v2 diff --git a/.gitmodules b/.gitmodules index 4b443c64..acf743ec 100644 --- a/.gitmodules +++ b/.gitmodules @@ -7,3 +7,6 @@ [submodule "rmcs_ws/src/hikcamera"] path = rmcs_ws/src/hikcamera url = git@github.com:Alliance-Algorithm/ros2-hikcamera.git +[submodule "rmcs_ws/src/rmcs_auto_aim_v2"] + path = rmcs_ws/src/rmcs_auto_aim_v2 + url = git@github.com:Alliance-Algorithm/rmcs_auto_aim_v2.git diff --git a/rmcs_ws/src/rmcs_auto_aim_v2 b/rmcs_ws/src/rmcs_auto_aim_v2 new file mode 160000 index 00000000..d665f7bc --- /dev/null +++ b/rmcs_ws/src/rmcs_auto_aim_v2 @@ -0,0 +1 @@ +Subproject commit d665f7bc35e2f60bbd7b0564ef65c0947f9fa973 From 4df9945dfc0e0d9d9fccbaa60c007de3abd7e9f6 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Mon, 11 May 2026 02:53:20 +0800 Subject: [PATCH 04/30] feat: restructure Aerial gimbal controller and integrate auto-aim module --- rmcs_ws/src/rmcs_bringup/config/flight.yaml | 173 ++++++++++++ rmcs_ws/src/rmcs_core/CMakeLists.txt | 2 +- rmcs_ws/src/rmcs_core/plugins.xml | 4 +- .../gimbal/simple_gimbal_controller.cpp | 16 +- .../bullet_feeder_controller_17mm.cpp | 12 +- .../src/hardware/device/lk_motor.hpp | 15 +- rmcs_ws/src/rmcs_core/src/hardware/flight.cpp | 260 ++++++++++++++++++ .../rmcs_core/src/hardware/flight_mavros.cpp | 224 +++++++++++++++ 8 files changed, 691 insertions(+), 15 deletions(-) create mode 100644 rmcs_ws/src/rmcs_bringup/config/flight.yaml create mode 100644 rmcs_ws/src/rmcs_core/src/hardware/flight.cpp create mode 100644 rmcs_ws/src/rmcs_core/src/hardware/flight_mavros.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..8c4057cd --- /dev/null +++ b/rmcs_ws/src/rmcs_bringup/config/flight.yaml @@ -0,0 +1,173 @@ +rmcs_executor: + ros__parameters: + update_rate: 1000.0 + components: + - rmcs_core::hardware::Flight -> flight_hardware + # - rmcs_core::hardware::FlightMavros -> flight_mavros_hardware + + - 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::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::referee::command::Interaction -> referee_interaction + - rmcs_core::referee::Command -> referee_command + - rmcs_core::referee::Status -> referee_status + + - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster + - rmcs_core::broadcaster::TfBroadcaster -> tf_broadcaster + + # - rmcs::AutoAimComponent -> auto_aim_component + +mavros: + ros__parameters: + enabled: true + fcu_url: serial:///dev/ttyACM0:921600 + gcs_url: "" + target_system_id: 1 + target_component_id: 1 + fcu_protocol: v2.0 + respawn: true + respawn_delay: 1.0 + +odin_ros_driver: + ros__parameters: + enabled: true + config_file: "/rmcs_install/share/odin_ros_driver/config/control_command.yaml" + node_name: host_sdk_sample + respawn: true + respawn_delay: 1.0 + +value_broadcaster: + ros__parameters: + forward_list: + - /gimbal/pitch/angle + - /gimbal/pitch/velocity + - /gimbal/pitch/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/velocity_imu + + +tf_broadcaster: + ros__parameters: + tf: /tf + +flight_hardware: + ros__parameters: + board_serial: "d4-9d44" + yaw_motor_zero_point: 11720 + pitch_motor_zero_point: 18578 + +flight_mavros_hardware: + ros__parameters: + target_frame_id: odom + source_frame_id: odin1_base_link + output_rate: 30.0 + sensor_roll_offset_rad: 3.141592653589793 + sensor_pitch_offset_rad: 1.5707963267948966 + sensor_yaw_offset_rad: 0.0 + mavros_pose_topic: /mavros/vision_pose/pose + mavros_pose_frame_id: odom + +referee_status: + ros__parameters: + path: /dev/tty0 + +gimbal_controller: + ros__parameters: + upper_limit: -0.39518 # -0.39518 rad ≈ -22.6° + lower_limit: 0.7 # 0.7 rad ≈ 40.1° + # TODO: yaw limite + +yaw_angle_pid_controller: + ros__parameters: + measurement: /gimbal/yaw/control_angle_error + control: /gimbal/yaw/control_velocity + kp: 15.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: 8.0 + ki: 0.0 + kd: 0.0 + +pitch_angle_pid_controller: + ros__parameters: + measurement: /gimbal/pitch/control_angle_error + control: /gimbal/pitch/control_velocity + kp: 10.0 + 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: 1 + 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 + +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 diff --git a/rmcs_ws/src/rmcs_core/CMakeLists.txt b/rmcs_ws/src/rmcs_core/CMakeLists.txt index d9504de2..38d75ca7 100644 --- a/rmcs_ws/src/rmcs_core/CMakeLists.txt +++ b/rmcs_ws/src/rmcs_core/CMakeLists.txt @@ -8,7 +8,7 @@ set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-packed-bitfield-compat") if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-O2 -Wall -Wextra -Wpedantic) + add_compile_options(-O2 -Wall -Wextra -Wpedantic) endif() find_package(ament_cmake_auto REQUIRED) diff --git a/rmcs_ws/src/rmcs_core/plugins.xml b/rmcs_ws/src/rmcs_core/plugins.xml index 82ffac1c..58f30161 100644 --- a/rmcs_ws/src/rmcs_core/plugins.xml +++ b/rmcs_ws/src/rmcs_core/plugins.xml @@ -1,5 +1,7 @@ - + + + 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..e18b0dd9 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 @@ -30,7 +30,8 @@ class SimpleGimbalController register_input("/remote/mouse/velocity", mouse_velocity_); register_input("/remote/mouse", mouse_); - register_input("/gimbal/auto_aim/control_direction", auto_aim_control_direction_, false); + register_input("/auto_aim/should_control", auto_aim_should_control_, false); + register_input("/auto_aim/control_direction", auto_aim_control_direction_, false); register_output("/gimbal/yaw/control_angle_error", yaw_angle_error_, nan_); register_output("/gimbal/pitch/control_angle_error", pitch_angle_error_, nan_); @@ -45,18 +46,20 @@ class SimpleGimbalController TwoAxisGimbalSolver::AngleError calculate_angle_error() { auto switch_right = *switch_right_; auto switch_left = *switch_left_; - auto mouse = *mouse_; - using namespace rmcs_msgs; if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) return two_axis_gimbal_solver.update(TwoAxisGimbalSolver::SetDisabled()); - if (auto_aim_control_direction_.ready() && (mouse.right || switch_right == Switch::UP) - && !auto_aim_control_direction_->isZero()) + const auto auto_aim_active = + switch_right == Switch::UP && auto_aim_should_control_.ready() + && *auto_aim_should_control_ && auto_aim_control_direction_.ready() + && auto_aim_control_direction_->allFinite() && !auto_aim_control_direction_->isZero(); + if (auto_aim_active) { return two_axis_gimbal_solver.update( TwoAxisGimbalSolver::SetControlDirection( OdomImu::DirectionVector(*auto_aim_control_direction_))); + } if (!two_axis_gimbal_solver.enabled()) return two_axis_gimbal_solver.update(TwoAxisGimbalSolver::SetToLevel()); @@ -82,6 +85,7 @@ class SimpleGimbalController InputInterface mouse_velocity_; InputInterface mouse_; + InputInterface auto_aim_should_control_; InputInterface auto_aim_control_direction_; TwoAxisGimbalSolver two_axis_gimbal_solver; @@ -94,4 +98,4 @@ class SimpleGimbalController #include PLUGINLIB_EXPORT_CLASS( - rmcs_core::controller::gimbal::SimpleGimbalController, rmcs_executor::Component) \ No newline at end of file + rmcs_core::controller::gimbal::SimpleGimbalController, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/shooting/bullet_feeder_controller_17mm.cpp b/rmcs_ws/src/rmcs_core/src/controller/shooting/bullet_feeder_controller_17mm.cpp index 0c97c98a..e90e62cc 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/shooting/bullet_feeder_controller_17mm.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/shooting/bullet_feeder_controller_17mm.cpp @@ -53,7 +53,7 @@ class BulletFeederController17mm register_input("/remote/mouse", mouse_); register_input("/remote/keyboard", keyboard_); - register_input("/gimbal/auto_aim/fire_control", fire_control_, false); + register_input("/auto_aim/should_shoot", auto_aim_should_shoot_, false); register_input("/gimbal/bullet_feeder/velocity", bullet_feeder_velocity_); register_output( @@ -63,8 +63,8 @@ class BulletFeederController17mm } void before_updating() override { - if (!fire_control_.ready()) - fire_control_.bind_directly(false); + if (!auto_aim_should_shoot_.ready()) + auto_aim_should_shoot_.bind_directly(false); } void update() override { @@ -103,7 +103,7 @@ class BulletFeederController17mm if (*friction_ready_) { if (shoot_mode == ShootMode::AUTOMATIC) { bool triggered = mouse.left || switch_left == Switch::DOWN - || (switch_right == Switch::UP && *fire_control_); + || (switch_right == Switch::UP && *auto_aim_should_shoot_); bullet_allowance = triggered ? *control_bullet_allowance_limited_by_heat_ : 0; } else { @@ -209,7 +209,7 @@ class BulletFeederController17mm InputInterface mouse_; InputInterface keyboard_; - InputInterface fire_control_; + InputInterface auto_aim_should_shoot_; rmcs_msgs::Switch last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; rmcs_msgs::Switch last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; @@ -232,4 +232,4 @@ class BulletFeederController17mm #include PLUGINLIB_EXPORT_CLASS( - rmcs_core::controller::shooting::BulletFeederController17mm, rmcs_executor::Component) \ No newline at end of file + rmcs_core::controller::shooting::BulletFeederController17mm, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/lk_motor.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/lk_motor.hpp index 1bc0ef8a..60348bb6 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/lk_motor.hpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/lk_motor.hpp @@ -23,7 +23,14 @@ namespace rmcs_core::hardware::device { class LkMotor { public: - enum class Type : uint8_t { kMG5010Ei10, kMG4010Ei10, kMG6012Ei8, kMG4005Ei10, kMG5010Ei36 }; + enum class Type : uint8_t { + kMG5010Ei10, + kMG4010Ei10, + kMG6012Ei8, + kMG4005Ei10, + kMG5010Ei36, + kMHF7015, + }; struct Config { explicit Config(Type type) @@ -110,6 +117,12 @@ class LkMotor { reduction_ratio = 36.0; max_torque_ = 25.0; break; + case Type::kMHF7015: + raw_angle_modulus_ = 1 << 16; + torque_constant = 0.51; + reduction_ratio = 1.0; + max_torque_ = 2.42; + break; default: std::unreachable(); } 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..ba0b5439 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp @@ -0,0 +1,260 @@ +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "hardware/device/bmi088.hpp" +#include "hardware/device/can_packet.hpp" +#include "hardware/device/dji_motor.hpp" +#include "hardware/device/dr16.hpp" +#include "hardware/device/lk_motor.hpp" + +namespace rmcs_core::hardware { + +class Flight + : public rmcs_executor::Component + , public rclcpp::Node + , private librmcs::agent::CBoard { +public: + Flight() + : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} + , librmcs::agent::CBoard{get_parameter("board_serial").as_string()} + , logger_(get_logger()) + , command_component_( + create_partner_component(get_component_name() + "_command", *this)) + , gimbal_yaw_motor_(*this, *command_component_, "/gimbal/yaw") + , gimbal_pitch_motor_(*this, *command_component_, "/gimbal/pitch") + , gimbal_left_friction_(*this, *command_component_, "/gimbal/left_friction") + , gimbal_right_friction_(*this, *command_component_, "/gimbal/right_friction") + , gimbal_bullet_feeder_(*this, *command_component_, "/gimbal/bullet_feeder") + , dr16_(*this) + , bmi088_(1000.0, 0.2, 0.00) { + + gimbal_yaw_motor_.configure( + device::LkMotor::Config{device::LkMotor::Type::kMHF7015} + .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::kMG4010Ei10}.set_encoder_zero_point( + static_cast(get_parameter("pitch_motor_zero_point").as_int()))); + gimbal_left_friction_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reversed() + .set_reduction_ratio(1.0)); + gimbal_right_friction_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(1.0)); + gimbal_bullet_feeder_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM2006} + .set_reversed() + .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; // NOLINT(google-build-using-namespace) + 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.0; + constexpr double gimbal_center_y = 0.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.1572, 0.00675, 0.0528}); + + 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_n( + [&buffer](std::byte byte) noexcept { *buffer++ = byte; }, size); + }; + referee_serial_->write = [this](const std::byte* buffer, size_t size) { + start_transmit().uart1_transmit( + {.uart_data = std::span{buffer, size}}); + return size; + }; + } + + Flight(const Flight&) = delete; + Flight& operator=(const Flight&) = delete; + Flight(Flight&&) = delete; + Flight& operator=(Flight&&) = delete; + + ~Flight() override = default; + + void update() override { + update_motors(); + update_imu(); + dr16_.update_status(); + } + + void command_update() { + auto builder = start_transmit(); + builder + .can2_transmit( + {.can_id = 0x141, + .can_data = gimbal_yaw_motor_.generate_torque_command().as_bytes()}) + .can2_transmit( + {.can_id = 0x142, .can_data = gimbal_pitch_motor_.generate_command().as_bytes()}) + .can1_transmit( + {.can_id = 0x200, + .can_data = + device::CanPacket8{ + gimbal_bullet_feeder_.generate_command(), + device::CanPacket8::PaddingQuarter{}, + gimbal_left_friction_.generate_command(), + gimbal_right_friction_.generate_command()} + .as_bytes()}); + } + +private: + void update_motors() { + using namespace rmcs_description; // NOLINT(google-build-using-namespace) + 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()); + } + + class FlightCommand : public rmcs_executor::Component { + public: + explicit FlightCommand(Flight& flight) + : flight_(flight) {} + + void update() override { flight_.command_update(); } + + private: + Flight& flight_; + }; + +protected: + void can1_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission || data.can_data.size() < 8) + [[unlikely]] + return; + + if (data.can_id == 0x201) { + gimbal_bullet_feeder_.store_status(data.can_data); + } else if (data.can_id == 0x203) { + gimbal_left_friction_.store_status(data.can_data); + } else if (data.can_id == 0x204) { + gimbal_right_friction_.store_status(data.can_data); + } + } + + void can2_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission || data.can_data.size() < 8) + [[unlikely]] + return; + + if (data.can_id == 0x142) + gimbal_pitch_motor_.store_status(data.can_data); + else if (data.can_id == 0x141) { + gimbal_yaw_motor_.store_status(data.can_data); + } + } + + void uart1_receive_callback(const librmcs::data::UartDataView& data) override { + const std::byte* ptr = data.uart_data.data(); + referee_ring_buffer_receive_.emplace_back_n( + [&ptr](std::byte* storage) noexcept { new (storage) std::byte{*ptr++}; }, + data.uart_data.size()); + } + + void dbus_receive_callback(const librmcs::data::UartDataView& data) override { + dr16_.store_status(data.uart_data.data(), data.uart_data.size()); + } + + void accelerometer_receive_callback(const librmcs::data::AccelerometerDataView& data) override { + bmi088_.store_accelerometer_status(data.x, data.y, data.z); + } + + void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { + bmi088_.store_gyroscope_status(data.x, data.y, data.z); + } + +private: + rclcpp::Logger logger_; + std::shared_ptr command_component_; + 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_; + OutputInterface referee_serial_; + + rmcs_utility::RingBuffer referee_ring_buffer_receive_{256}; +}; + +} // namespace rmcs_core::hardware + +#include + +PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::Flight, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/hardware/flight_mavros.cpp b/rmcs_ws/src/rmcs_core/src/hardware/flight_mavros.cpp new file mode 100644 index 00000000..54362804 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/flight_mavros.cpp @@ -0,0 +1,224 @@ +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace rmcs_core::hardware { + +class FlightMavros + : public rmcs_executor::Component + , public rclcpp::Node { +public: + FlightMavros() + : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} + , logger_(get_logger()) + , tf_buffer_(get_clock()) + , tf_listener_(tf_buffer_) + , last_pose_received_time_{0, 0, get_clock()->get_clock_type()} { + load_parameters(); + register_input("/predefined/update_rate", update_rate_); + + mavros_pose_publisher_ = create_publisher( + mavros_pose_topic_, rclcpp::QoS{10}.reliable()); + + RCLCPP_INFO( + logger_, + "FlightMavros bridging TF '%s' -> '%s' to '%s' with sensor RPY [%.1f, %.1f, %.1f] " + "deg.", + source_frame_id_.c_str(), target_frame_id_.c_str(), + mavros_pose_publisher_->get_topic_name(), radians_to_degrees(sensor_roll_offset_rad_), + radians_to_degrees(sensor_pitch_offset_rad_), + radians_to_degrees(sensor_yaw_offset_rad_)); + } + + ~FlightMavros() override = default; + + void update() override { + update_pose_from_tf(); + const double update_rate = *update_rate_; + if (!std::isfinite(update_rate) || update_rate <= 0.0) + return; + + publish_credit_ += output_rate_hz_ / update_rate; + while (publish_credit_ >= 1.0) { + publish_credit_ -= 1.0; + publish_pose(); + } + } + +private: + static double radians_to_degrees(double radians) { + return radians * 180.0 / std::numbers::pi_v; + } + + static Eigen::Quaterniond quaternion_from_rpy(double roll, double pitch, double yaw) { + const Eigen::AngleAxisd roll_rotation{roll, Eigen::Vector3d::UnitX()}; + const Eigen::AngleAxisd pitch_rotation{pitch, Eigen::Vector3d::UnitY()}; + const Eigen::AngleAxisd yaw_rotation{yaw, Eigen::Vector3d::UnitZ()}; + return Eigen::Quaterniond{roll_rotation * pitch_rotation * yaw_rotation}; + } + + void load_parameters() { + mavros_pose_topic_ = get_parameter("mavros_pose_topic").as_string(); + mavros_pose_frame_id_ = get_parameter("mavros_pose_frame_id").as_string(); + target_frame_id_ = get_parameter("target_frame_id").as_string(); + source_frame_id_ = get_parameter("source_frame_id").as_string(); + output_rate_hz_ = get_parameter("output_rate").as_double(); + sensor_roll_offset_rad_ = get_parameter("sensor_roll_offset_rad").as_double(); + sensor_pitch_offset_rad_ = get_parameter("sensor_pitch_offset_rad").as_double(); + sensor_yaw_offset_rad_ = get_parameter("sensor_yaw_offset_rad").as_double(); + + output_rate_hz_ = std::max(0.0, output_rate_hz_); + } + + Eigen::Quaterniond sensor_to_body_rotation() const { + auto sensor_to_body = quaternion_from_rpy( + sensor_roll_offset_rad_, sensor_pitch_offset_rad_, sensor_yaw_offset_rad_); + sensor_to_body.normalize(); + return sensor_to_body; + } + + static bool finite_transform(const geometry_msgs::msg::TransformStamped& transform) { + const auto& t = transform.transform.translation; + const auto& r = transform.transform.rotation; + return std::isfinite(t.x) && std::isfinite(t.y) && std::isfinite(t.z) && std::isfinite(r.x) + && std::isfinite(r.y) && std::isfinite(r.z) && std::isfinite(r.w); + } + + Eigen::Vector3d rotate_world_position(const geometry_msgs::msg::Vector3& position) const { + return Eigen::Vector3d{position.x, position.y, position.z}; + } + + Eigen::Quaterniond + rotate_body_orientation(const geometry_msgs::msg::Quaternion& orientation) const { + const Eigen::Quaterniond quat_sensor{ + orientation.w, orientation.x, orientation.y, orientation.z}; + + Eigen::Quaterniond quat_body = quat_sensor * sensor_to_body_rotation(); + quat_body.normalize(); + return quat_body; + } + + void update_pose_from_tf() { + geometry_msgs::msg::TransformStamped transform; + try { + transform = + tf_buffer_.lookupTransform(target_frame_id_, source_frame_id_, tf2::TimePointZero); + } catch (const tf2::TransformException& ex) { + RCLCPP_WARN_THROTTLE( + logger_, *get_clock(), 1000, "TF lookup failed for '%s' -> '%s': %s", + source_frame_id_.c_str(), target_frame_id_.c_str(), ex.what()); + return; + } + + if (!finite_transform(transform)) + return; + + const auto tf_stamp_ns = rclcpp::Time{transform.header.stamp}.nanoseconds(); + + std::lock_guard lock(pose_mutex_); + if (last_tf_stamp_ns_ != 0 && tf_stamp_ns <= last_tf_stamp_ns_) + return; + + const Eigen::Vector3d rotated_position = + rotate_world_position(transform.transform.translation); + const Eigen::Quaterniond rotated_orientation = + rotate_body_orientation(transform.transform.rotation); + + latest_pose_.position.x = rotated_position.x(); + latest_pose_.position.y = rotated_position.y(); + latest_pose_.position.z = rotated_position.z(); + latest_pose_.orientation.x = rotated_orientation.x(); + latest_pose_.orientation.y = rotated_orientation.y(); + latest_pose_.orientation.z = rotated_orientation.z(); + latest_pose_.orientation.w = rotated_orientation.w(); + last_tf_stamp_ns_ = tf_stamp_ns; + last_pose_received_time_ = get_clock()->now(); + pose_ready_ = true; + + RCLCPP_INFO_THROTTLE( + logger_, *get_clock(), 1000, "Received TF '%s' -> '%s'.", source_frame_id_.c_str(), + target_frame_id_.c_str()); + } + + void publish_pose() { + geometry_msgs::msg::Pose pose; + rclcpp::Time pose_received_time{0, 0, get_clock()->get_clock_type()}; + { + std::lock_guard lock(pose_mutex_); + if (!pose_ready_) + return; + pose_received_time = last_pose_received_time_; + pose = latest_pose_; + } + + const rclcpp::Duration max_pose_age = rclcpp::Duration::from_seconds(1.0); + rclcpp::Time publish_stamp = get_clock()->now(); + if ((publish_stamp - pose_received_time) >= max_pose_age) + return; + + { + std::lock_guard lock(pose_mutex_); + if (last_published_stamp_ns_ != 0 + && publish_stamp.nanoseconds() <= last_published_stamp_ns_) { + publish_stamp = + rclcpp::Time{last_published_stamp_ns_ + 1, get_clock()->get_clock_type()}; + } + last_published_stamp_ns_ = publish_stamp.nanoseconds(); + } + + geometry_msgs::msg::PoseStamped pose_msg{}; + pose_msg.header.stamp = publish_stamp; + pose_msg.header.frame_id = mavros_pose_frame_id_; + pose_msg.pose = pose; + mavros_pose_publisher_->publish(pose_msg); + + RCLCPP_INFO_THROTTLE( + logger_, *get_clock(), 1000, "Published MAVROS vision pose at %.1f Hz to '%s'.", + output_rate_hz_, mavros_pose_publisher_->get_topic_name()); + } + + rclcpp::Logger logger_; + std::string mavros_pose_topic_; + std::string mavros_pose_frame_id_; + std::string target_frame_id_; + std::string source_frame_id_; + double output_rate_hz_; + double sensor_roll_offset_rad_; + double sensor_pitch_offset_rad_; + double sensor_yaw_offset_rad_; + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; + + rclcpp::Publisher::SharedPtr mavros_pose_publisher_; + + InputInterface update_rate_; + + std::mutex pose_mutex_; + geometry_msgs::msg::Pose latest_pose_{}; + std::int64_t last_tf_stamp_ns_{0}; + rclcpp::Time last_pose_received_time_; + std::int64_t last_published_stamp_ns_{0}; + double publish_credit_{0.0}; + bool pose_ready_{false}; +}; + +} // namespace rmcs_core::hardware + +#include +PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::FlightMavros, rmcs_executor::Component) From 6ef94bbc42394ba61d5e874598cd0c3bca56e487 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Mon, 11 May 2026 14:19:33 +0800 Subject: [PATCH 05/30] feat: launch odin_ros_driver in startup script --- .../src/rmcs_bringup/launch/rmcs.launch.py | 26 ++++++++++++++----- 1 file changed, 20 insertions(+), 6 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/launch/rmcs.launch.py b/rmcs_ws/src/rmcs_bringup/launch/rmcs.launch.py index 976cb9e7..537aadd8 100644 --- a/rmcs_ws/src/rmcs_bringup/launch/rmcs.launch.py +++ b/rmcs_ws/src/rmcs_bringup/launch/rmcs.launch.py @@ -6,7 +6,8 @@ LaunchDescription, LaunchDescriptionEntity, ) -from launch.actions import LogInfo +from launch.actions import IncludeLaunchDescription, LogInfo +from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node @@ -28,10 +29,7 @@ def visit( robot_name = robot_config entities.append( - LogInfo( - msg=f"Starting RMCS on robot '{robot_config}'{'(automatic)' if is_automatic else ''} -> {robot_name}.yaml" - ) - ) + LogInfo(msg=f"Starting RMCS on robot -> {robot_name}.yaml")) entities.append( Node( @@ -46,13 +44,29 @@ def visit( ], respawn=True, respawn_delay=1.0, - output="log", # stdout and stderr are logged to launch log file and stderr to the screen. + output="log", ) ) if is_automatic: pass + entities.append( + IncludeLaunchDescription( + PythonLaunchDescriptionSource([ + FindPackageShare("odin_ros_driver"), "/launch/odin1_ros2.launch.py" + ]) + ) + ) + + entities.append( + IncludeLaunchDescription( + PythonLaunchDescriptionSource([ + FindPackageShare('rmcs_auto_aim_v2'), '/launch.py' + ]) + ) + ) + return entities From e76872d653517edec5c77d6bb673615d9b72822b Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Mon, 11 May 2026 14:35:12 +0800 Subject: [PATCH 06/30] chore: update rmcs_auto_aim_v2 submodule --- rmcs_ws/src/rmcs_auto_aim_v2 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rmcs_ws/src/rmcs_auto_aim_v2 b/rmcs_ws/src/rmcs_auto_aim_v2 index d665f7bc..8c16a237 160000 --- a/rmcs_ws/src/rmcs_auto_aim_v2 +++ b/rmcs_ws/src/rmcs_auto_aim_v2 @@ -1 +1 @@ -Subproject commit d665f7bc35e2f60bbd7b0564ef65c0947f9fa973 +Subproject commit 8c16a23745ed4c880f0823f90d37d5fd2362c015 From 62774950771426b0411f8cefed944c9cf940236b Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Wed, 13 May 2026 01:40:01 +0800 Subject: [PATCH 07/30] refactor: restructure flight_mavros module --- rmcs_ws/src/rmcs_bringup/config/flight.yaml | 9 +- rmcs_ws/src/rmcs_core/package.xml | 3 +- .../rmcs_core/src/hardware/flight_mavros.cpp | 212 +++++------------- 3 files changed, 64 insertions(+), 160 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/flight.yaml b/rmcs_ws/src/rmcs_bringup/config/flight.yaml index 8c4057cd..5d2ee6e8 100644 --- a/rmcs_ws/src/rmcs_bringup/config/flight.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/flight.yaml @@ -3,7 +3,7 @@ rmcs_executor: update_rate: 1000.0 components: - rmcs_core::hardware::Flight -> flight_hardware - # - rmcs_core::hardware::FlightMavros -> flight_mavros_hardware + - rmcs_core::hardware::FlightMavros -> flight_mavros_hardware - rmcs_core::controller::gimbal::SimpleGimbalController -> gimbal_controller - rmcs_core::controller::pid::ErrorPidController -> yaw_angle_pid_controller @@ -24,7 +24,7 @@ rmcs_executor: - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster - rmcs_core::broadcaster::TfBroadcaster -> tf_broadcaster - # - rmcs::AutoAimComponent -> auto_aim_component + - rmcs::AutoAimComponent -> auto_aim_component mavros: ros__parameters: @@ -75,14 +75,11 @@ flight_hardware: flight_mavros_hardware: ros__parameters: - target_frame_id: odom - source_frame_id: odin1_base_link - output_rate: 30.0 + odometry_topic: /odin1/odometry sensor_roll_offset_rad: 3.141592653589793 sensor_pitch_offset_rad: 1.5707963267948966 sensor_yaw_offset_rad: 0.0 mavros_pose_topic: /mavros/vision_pose/pose - mavros_pose_frame_id: odom referee_status: ros__parameters: diff --git a/rmcs_ws/src/rmcs_core/package.xml b/rmcs_ws/src/rmcs_core/package.xml index 4312d334..9eb6b089 100644 --- a/rmcs_ws/src/rmcs_core/package.xml +++ b/rmcs_ws/src/rmcs_core/package.xml @@ -10,11 +10,10 @@ ament_cmake rclcpp + nav_msgs std_msgs std_srvs pluginlib - tf2 - tf2_ros serial rmcs_utility rmcs_msgs diff --git a/rmcs_ws/src/rmcs_core/src/hardware/flight_mavros.cpp b/rmcs_ws/src/rmcs_core/src/hardware/flight_mavros.cpp index 54362804..6a1dee4c 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/flight_mavros.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/flight_mavros.cpp @@ -1,20 +1,16 @@ -#include +#include #include #include -#include #include #include #include #include -#include -#include +#include #include #include -#include -#include -#include +#include #include @@ -26,45 +22,51 @@ class FlightMavros public: FlightMavros() : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} - , logger_(get_logger()) - , tf_buffer_(get_clock()) - , tf_listener_(tf_buffer_) - , last_pose_received_time_{0, 0, get_clock()->get_clock_type()} { - load_parameters(); - register_input("/predefined/update_rate", update_rate_); + , logger_(get_logger()) { + odometry_topic_ = get_parameter("odometry_topic").as_string(); + const auto mavros_pose_topic = get_parameter("mavros_pose_topic").as_string(); + const auto sensor_roll_offset_rad = get_parameter("sensor_roll_offset_rad").as_double(); + const auto sensor_pitch_offset_rad = get_parameter("sensor_pitch_offset_rad").as_double(); + const auto sensor_yaw_offset_rad = get_parameter("sensor_yaw_offset_rad").as_double(); + + sensor_to_body_rotation_ = quaternion_from_rpy( + sensor_roll_offset_rad, sensor_pitch_offset_rad, sensor_yaw_offset_rad); + sensor_to_body_rotation_.normalize(); mavros_pose_publisher_ = create_publisher( - mavros_pose_topic_, rclcpp::QoS{10}.reliable()); + mavros_pose_topic, rclcpp::QoS{10}.reliable()); + odometry_subscription_ = create_subscription( + odometry_topic_, rclcpp::QoS{10}.reliable(), + [this](nav_msgs::msg::Odometry::ConstSharedPtr msg) { odometry_callback(msg); }); + last_odometry_received_time_ns_.store( + get_clock()->now().nanoseconds(), std::memory_order_relaxed); RCLCPP_INFO( logger_, - "FlightMavros bridging TF '%s' -> '%s' to '%s' with sensor RPY [%.1f, %.1f, %.1f] " + "FlightMavros bridging odometry '%s' to '%s' with sensor RPY [%.1f, %.1f, %.1f] " "deg.", - source_frame_id_.c_str(), target_frame_id_.c_str(), - mavros_pose_publisher_->get_topic_name(), radians_to_degrees(sensor_roll_offset_rad_), - radians_to_degrees(sensor_pitch_offset_rad_), - radians_to_degrees(sensor_yaw_offset_rad_)); + odometry_topic_.c_str(), mavros_pose_publisher_->get_topic_name(), + radians_to_degrees(sensor_roll_offset_rad), radians_to_degrees(sensor_pitch_offset_rad), + radians_to_degrees(sensor_yaw_offset_rad)); } ~FlightMavros() override = default; void update() override { - update_pose_from_tf(); - const double update_rate = *update_rate_; - if (!std::isfinite(update_rate) || update_rate <= 0.0) - return; - - publish_credit_ += output_rate_hz_ / update_rate; - while (publish_credit_ >= 1.0) { - publish_credit_ -= 1.0; - publish_pose(); + const auto now_ns = get_clock()->now().nanoseconds(); + const auto last_odometry_received_time_ns = + last_odometry_received_time_ns_.load(std::memory_order_relaxed); + if (now_ns - last_odometry_received_time_ns >= kOdometryTimeoutNs) { + RCLCPP_WARN_THROTTLE( + logger_, *get_clock(), 1000, "No odometry received on '%s' for more than %.1f s.", + odometry_topic_.c_str(), static_cast(kOdometryTimeoutNs) / 1'000'000'000.0); } } private: - static double radians_to_degrees(double radians) { - return radians * 180.0 / std::numbers::pi_v; - } + static constexpr std::int64_t kOdometryTimeoutNs{1'000'000'000}; // 1s + + static double radians_to_degrees(double radians) { return radians * 180.0 / std::numbers::pi; } static Eigen::Quaterniond quaternion_from_rpy(double roll, double pitch, double yaw) { const Eigen::AngleAxisd roll_rotation{roll, Eigen::Vector3d::UnitX()}; @@ -73,149 +75,55 @@ class FlightMavros return Eigen::Quaterniond{roll_rotation * pitch_rotation * yaw_rotation}; } - void load_parameters() { - mavros_pose_topic_ = get_parameter("mavros_pose_topic").as_string(); - mavros_pose_frame_id_ = get_parameter("mavros_pose_frame_id").as_string(); - target_frame_id_ = get_parameter("target_frame_id").as_string(); - source_frame_id_ = get_parameter("source_frame_id").as_string(); - output_rate_hz_ = get_parameter("output_rate").as_double(); - sensor_roll_offset_rad_ = get_parameter("sensor_roll_offset_rad").as_double(); - sensor_pitch_offset_rad_ = get_parameter("sensor_pitch_offset_rad").as_double(); - sensor_yaw_offset_rad_ = get_parameter("sensor_yaw_offset_rad").as_double(); - - output_rate_hz_ = std::max(0.0, output_rate_hz_); - } - - Eigen::Quaterniond sensor_to_body_rotation() const { - auto sensor_to_body = quaternion_from_rpy( - sensor_roll_offset_rad_, sensor_pitch_offset_rad_, sensor_yaw_offset_rad_); - sensor_to_body.normalize(); - return sensor_to_body; - } - - static bool finite_transform(const geometry_msgs::msg::TransformStamped& transform) { - const auto& t = transform.transform.translation; - const auto& r = transform.transform.rotation; - return std::isfinite(t.x) && std::isfinite(t.y) && std::isfinite(t.z) && std::isfinite(r.x) - && std::isfinite(r.y) && std::isfinite(r.z) && std::isfinite(r.w); - } - - Eigen::Vector3d rotate_world_position(const geometry_msgs::msg::Vector3& position) const { - return Eigen::Vector3d{position.x, position.y, position.z}; - } - Eigen::Quaterniond rotate_body_orientation(const geometry_msgs::msg::Quaternion& orientation) const { const Eigen::Quaterniond quat_sensor{ orientation.w, orientation.x, orientation.y, orientation.z}; - Eigen::Quaterniond quat_body = quat_sensor * sensor_to_body_rotation(); + Eigen::Quaterniond quat_body = quat_sensor * sensor_to_body_rotation_; quat_body.normalize(); return quat_body; } - void update_pose_from_tf() { - geometry_msgs::msg::TransformStamped transform; - try { - transform = - tf_buffer_.lookupTransform(target_frame_id_, source_frame_id_, tf2::TimePointZero); - } catch (const tf2::TransformException& ex) { + void odometry_callback(const nav_msgs::msg::Odometry::ConstSharedPtr& msg) { + last_odometry_received_time_ns_.store( + get_clock()->now().nanoseconds(), std::memory_order_relaxed); + + RCLCPP_INFO_ONCE( + logger_, "Received odometry from '%s' with frame '%s' and child frame '%s'.", + odometry_topic_.c_str(), msg->header.frame_id.c_str(), msg->child_frame_id.c_str()); + + if (msg->header.frame_id != "odom" || msg->child_frame_id != "odin1_base_link") { RCLCPP_WARN_THROTTLE( - logger_, *get_clock(), 1000, "TF lookup failed for '%s' -> '%s': %s", - source_frame_id_.c_str(), target_frame_id_.c_str(), ex.what()); - return; + logger_, *get_clock(), 1000, + "Unexpected odometry frames: header='%s', child='%s'. Expected 'odom' and " + "'odin1_base_link'.", + msg->header.frame_id.c_str(), msg->child_frame_id.c_str()); } - if (!finite_transform(transform)) - return; - - const auto tf_stamp_ns = rclcpp::Time{transform.header.stamp}.nanoseconds(); + geometry_msgs::msg::PoseStamped pose_msg{}; + pose_msg.header.stamp = msg->header.stamp; + pose_msg.header.frame_id = msg->header.frame_id; - std::lock_guard lock(pose_mutex_); - if (last_tf_stamp_ns_ != 0 && tf_stamp_ns <= last_tf_stamp_ns_) - return; + pose_msg.pose.position = msg->pose.pose.position; - const Eigen::Vector3d rotated_position = - rotate_world_position(transform.transform.translation); const Eigen::Quaterniond rotated_orientation = - rotate_body_orientation(transform.transform.rotation); - - latest_pose_.position.x = rotated_position.x(); - latest_pose_.position.y = rotated_position.y(); - latest_pose_.position.z = rotated_position.z(); - latest_pose_.orientation.x = rotated_orientation.x(); - latest_pose_.orientation.y = rotated_orientation.y(); - latest_pose_.orientation.z = rotated_orientation.z(); - latest_pose_.orientation.w = rotated_orientation.w(); - last_tf_stamp_ns_ = tf_stamp_ns; - last_pose_received_time_ = get_clock()->now(); - pose_ready_ = true; - - RCLCPP_INFO_THROTTLE( - logger_, *get_clock(), 1000, "Received TF '%s' -> '%s'.", source_frame_id_.c_str(), - target_frame_id_.c_str()); - } - - void publish_pose() { - geometry_msgs::msg::Pose pose; - rclcpp::Time pose_received_time{0, 0, get_clock()->get_clock_type()}; - { - std::lock_guard lock(pose_mutex_); - if (!pose_ready_) - return; - pose_received_time = last_pose_received_time_; - pose = latest_pose_; - } + rotate_body_orientation(msg->pose.pose.orientation); + pose_msg.pose.orientation.x = rotated_orientation.x(); + pose_msg.pose.orientation.y = rotated_orientation.y(); + pose_msg.pose.orientation.z = rotated_orientation.z(); + pose_msg.pose.orientation.w = rotated_orientation.w(); - const rclcpp::Duration max_pose_age = rclcpp::Duration::from_seconds(1.0); - rclcpp::Time publish_stamp = get_clock()->now(); - if ((publish_stamp - pose_received_time) >= max_pose_age) - return; - - { - std::lock_guard lock(pose_mutex_); - if (last_published_stamp_ns_ != 0 - && publish_stamp.nanoseconds() <= last_published_stamp_ns_) { - publish_stamp = - rclcpp::Time{last_published_stamp_ns_ + 1, get_clock()->get_clock_type()}; - } - last_published_stamp_ns_ = publish_stamp.nanoseconds(); - } - - geometry_msgs::msg::PoseStamped pose_msg{}; - pose_msg.header.stamp = publish_stamp; - pose_msg.header.frame_id = mavros_pose_frame_id_; - pose_msg.pose = pose; mavros_pose_publisher_->publish(pose_msg); - - RCLCPP_INFO_THROTTLE( - logger_, *get_clock(), 1000, "Published MAVROS vision pose at %.1f Hz to '%s'.", - output_rate_hz_, mavros_pose_publisher_->get_topic_name()); } rclcpp::Logger logger_; - std::string mavros_pose_topic_; - std::string mavros_pose_frame_id_; - std::string target_frame_id_; - std::string source_frame_id_; - double output_rate_hz_; - double sensor_roll_offset_rad_; - double sensor_pitch_offset_rad_; - double sensor_yaw_offset_rad_; - tf2_ros::Buffer tf_buffer_; - tf2_ros::TransformListener tf_listener_; + std::string odometry_topic_; + Eigen::Quaterniond sensor_to_body_rotation_{Eigen::Quaterniond::Identity()}; + std::atomic last_odometry_received_time_ns_{0}; rclcpp::Publisher::SharedPtr mavros_pose_publisher_; - - InputInterface update_rate_; - - std::mutex pose_mutex_; - geometry_msgs::msg::Pose latest_pose_{}; - std::int64_t last_tf_stamp_ns_{0}; - rclcpp::Time last_pose_received_time_; - std::int64_t last_published_stamp_ns_{0}; - double publish_credit_{0.0}; - bool pose_ready_{false}; + rclcpp::Subscription::SharedPtr odometry_subscription_; }; } // namespace rmcs_core::hardware From 0129fa1667c29ddb1f0249ae90cd91a9389f8916 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Wed, 13 May 2026 01:40:01 +0800 Subject: [PATCH 08/30] refactor: restructure flight_mavros module --- rmcs_ws/src/rmcs_core/src/hardware/flight_mavros.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/rmcs_ws/src/rmcs_core/src/hardware/flight_mavros.cpp b/rmcs_ws/src/rmcs_core/src/hardware/flight_mavros.cpp index 6a1dee4c..8536c831 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/flight_mavros.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/flight_mavros.cpp @@ -37,7 +37,7 @@ class FlightMavros mavros_pose_topic, rclcpp::QoS{10}.reliable()); odometry_subscription_ = create_subscription( odometry_topic_, rclcpp::QoS{10}.reliable(), - [this](nav_msgs::msg::Odometry::ConstSharedPtr msg) { odometry_callback(msg); }); + [this](const nav_msgs::msg::Odometry::ConstSharedPtr& msg) { odometry_callback(msg); }); last_odometry_received_time_ns_.store( get_clock()->now().nanoseconds(), std::memory_order_relaxed); @@ -64,7 +64,7 @@ class FlightMavros } private: - static constexpr std::int64_t kOdometryTimeoutNs{1'000'000'000}; // 1s + static constexpr std::int64_t kOdometryTimeoutNs{1'000'000'000}; static double radians_to_degrees(double radians) { return radians * 180.0 / std::numbers::pi; } @@ -104,7 +104,6 @@ class FlightMavros geometry_msgs::msg::PoseStamped pose_msg{}; pose_msg.header.stamp = msg->header.stamp; pose_msg.header.frame_id = msg->header.frame_id; - pose_msg.pose.position = msg->pose.pose.position; const Eigen::Quaterniond rotated_orientation = From 2c79e27199ac5fe59a5873701e8d1dca9d9f58db Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Wed, 13 May 2026 23:50:26 +0800 Subject: [PATCH 09/30] refactor: update gimbal coordinate transformation --- rmcs_ws/src/rmcs_core/src/hardware/flight.cpp | 17 ++++++++++------- 1 file changed, 10 insertions(+), 7 deletions(-) diff --git a/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp b/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp index ba0b5439..d0d15155 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp @@ -73,8 +73,8 @@ class Flight [](double x, double y, double z) { return std::make_tuple(-x, z, y); }); using namespace rmcs_description; // NOLINT(google-build-using-namespace) - constexpr double rotor_distance_x = 0.83637; - constexpr double rotor_distance_y = 0.83637; + constexpr double rotor_distance_x = 1.128; + constexpr double rotor_distance_y = 1.128; tf_->set_transform( Eigen::Translation3d{rotor_distance_x / 2, rotor_distance_y / 2, 0}); @@ -85,12 +85,15 @@ class Flight tf_->set_transform( Eigen::Translation3d{-rotor_distance_x / 2, -rotor_distance_y / 2, 0}); - constexpr double gimbal_center_x = 0.0; - constexpr double gimbal_center_y = 0.0; - constexpr double gimbal_center_z = -0.20552; + // GimbalCenterLink 原点等效于 YawLink 和 PitchLink 的交点 + constexpr double gimbal_center_z = -0.27812; tf_->set_transform( - Eigen::Translation3d{gimbal_center_x, gimbal_center_y, gimbal_center_z}); - tf_->set_transform(Eigen::Translation3d{0.1572, 0.00675, 0.0528}); + Eigen::Translation3d{0.0, 0.0, gimbal_center_z}); + constexpr auto camera_postion_x = 0.10238; + constexpr auto camera_postion_z = 0.05286; + + tf_->set_transform( + Eigen::Translation3d{camera_postion_x, 0.0, camera_postion_z}); gimbal_calibrate_subscription_ = create_subscription( "/gimbal/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { From 9c22bbd81d224bc4d58d9b35fdeefa3285f1e7cb Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Thu, 14 May 2026 00:42:43 +0800 Subject: [PATCH 10/30] feat(launch): integrate MAVROS node in launch script for FCU communication --- rmcs_ws/src/rmcs_bringup/config/flight.yaml | 1 - .../src/rmcs_bringup/launch/rmcs.launch.py | 24 +++++++++++++++++++ rmcs_ws/src/rmcs_bringup/package.xml | 3 ++- .../gimbal/simple_gimbal_controller.cpp | 12 ++++++---- 4 files changed, 33 insertions(+), 7 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/flight.yaml b/rmcs_ws/src/rmcs_bringup/config/flight.yaml index 5d2ee6e8..da23e880 100644 --- a/rmcs_ws/src/rmcs_bringup/config/flight.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/flight.yaml @@ -62,7 +62,6 @@ value_broadcaster: - /gimbal/yaw/control_angle_error - /gimbal/yaw/velocity_imu - tf_broadcaster: ros__parameters: tf: /tf diff --git a/rmcs_ws/src/rmcs_bringup/launch/rmcs.launch.py b/rmcs_ws/src/rmcs_bringup/launch/rmcs.launch.py index 537aadd8..bd076a3c 100644 --- a/rmcs_ws/src/rmcs_bringup/launch/rmcs.launch.py +++ b/rmcs_ws/src/rmcs_bringup/launch/rmcs.launch.py @@ -59,6 +59,30 @@ def visit( ) ) + mavros_share = FindPackageShare("mavros").perform(context) + entities.append( + Node( + package="mavros", + executable="mavros_node", + namespace="mavros", + parameters=[ + os.path.join(mavros_share, "launch", "px4_pluginlists.yaml"), + os.path.join(mavros_share, "launch", "px4_config.yaml"), + { + "fcu_url": "serial:///dev/ttyACM0:921600", + "gcs_url": "", + "tgt_system": 1, + "tgt_component": 1, + "fcu_protocol": "v2.0", + }, + ], + respawn=True, + respawn_delay=1.0, + output="screen", + emulate_tty=True, + ) + ) + entities.append( IncludeLaunchDescription( PythonLaunchDescriptionSource([ diff --git a/rmcs_ws/src/rmcs_bringup/package.xml b/rmcs_ws/src/rmcs_bringup/package.xml index c64c661f..43835a9b 100644 --- a/rmcs_ws/src/rmcs_bringup/package.xml +++ b/rmcs_ws/src/rmcs_bringup/package.xml @@ -11,6 +11,7 @@ ament_cmake joint_state_broadcaster + mavros robot_state_publisher rviz2 xacro @@ -21,4 +22,4 @@ ament_cmake - \ 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 e18b0dd9..44e26673 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 @@ -46,16 +46,18 @@ class SimpleGimbalController TwoAxisGimbalSolver::AngleError calculate_angle_error() { auto switch_right = *switch_right_; auto switch_left = *switch_left_; + using namespace rmcs_msgs; if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) return two_axis_gimbal_solver.update(TwoAxisGimbalSolver::SetDisabled()); - const auto auto_aim_active = - switch_right == Switch::UP && auto_aim_should_control_.ready() - && *auto_aim_should_control_ && auto_aim_control_direction_.ready() - && auto_aim_control_direction_->allFinite() && !auto_aim_control_direction_->isZero(); - if (auto_aim_active) { + const auto manual_active = switch_right == Switch::UP; + const auto should_control = auto_aim_should_control_.ready() && *auto_aim_should_control_; + const auto valid_control = + auto_aim_control_direction_.ready() && auto_aim_control_direction_->allFinite(); + + if (manual_active && should_control && valid_control) { return two_axis_gimbal_solver.update( TwoAxisGimbalSolver::SetControlDirection( OdomImu::DirectionVector(*auto_aim_control_direction_))); From 95df49f3e90851df4032e3352379a32507e19c0f Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Thu, 14 May 2026 21:00:18 +0800 Subject: [PATCH 11/30] refactor: restructure flight_mavros module and optimize odin launch scripts - Refactor flight_mavros to support absolute pose tracking and feedback. - Optimize odin1 launch script by integrating hardware drivers and auto-aim. - Remove default Rviz startup to reduce system overhead in headless mode. - Synchronize coordinate transformations for cross-platform deployment. --- rmcs_ws/src/odin_ros_driver | 2 +- rmcs_ws/src/rmcs_bringup/config/flight.yaml | 13 +- .../src/rmcs_bringup/launch/rmcs.launch.py | 9 +- rmcs_ws/src/rmcs_bringup/package.xml | 1 + .../rmcs_core/src/hardware/flight_mavros.cpp | 131 ------------------ 5 files changed, 9 insertions(+), 147 deletions(-) delete mode 100644 rmcs_ws/src/rmcs_core/src/hardware/flight_mavros.cpp diff --git a/rmcs_ws/src/odin_ros_driver b/rmcs_ws/src/odin_ros_driver index dbcb0286..165a7907 160000 --- a/rmcs_ws/src/odin_ros_driver +++ b/rmcs_ws/src/odin_ros_driver @@ -1 +1 @@ -Subproject commit dbcb0286b510142e966ac090a37ce5ca238f9e75 +Subproject commit 165a79072fc292b73f388765e23a23aa99561f55 diff --git a/rmcs_ws/src/rmcs_bringup/config/flight.yaml b/rmcs_ws/src/rmcs_bringup/config/flight.yaml index da23e880..1f21f85e 100644 --- a/rmcs_ws/src/rmcs_bringup/config/flight.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/flight.yaml @@ -3,7 +3,6 @@ rmcs_executor: update_rate: 1000.0 components: - rmcs_core::hardware::Flight -> flight_hardware - - rmcs_core::hardware::FlightMavros -> flight_mavros_hardware - rmcs_core::controller::gimbal::SimpleGimbalController -> gimbal_controller - rmcs_core::controller::pid::ErrorPidController -> yaw_angle_pid_controller @@ -72,14 +71,6 @@ flight_hardware: yaw_motor_zero_point: 11720 pitch_motor_zero_point: 18578 -flight_mavros_hardware: - ros__parameters: - odometry_topic: /odin1/odometry - sensor_roll_offset_rad: 3.141592653589793 - sensor_pitch_offset_rad: 1.5707963267948966 - sensor_yaw_offset_rad: 0.0 - mavros_pose_topic: /mavros/vision_pose/pose - referee_status: ros__parameters: path: /dev/tty0 @@ -121,8 +112,8 @@ friction_wheel_controller: - /gimbal/left_friction - /gimbal/right_friction friction_velocities: - - 740.0 - - 740.0 + - 610.0 + - 610.0 friction_soft_start_stop_time: 1.0 heat_controller: diff --git a/rmcs_ws/src/rmcs_bringup/launch/rmcs.launch.py b/rmcs_ws/src/rmcs_bringup/launch/rmcs.launch.py index bd076a3c..b56e910d 100644 --- a/rmcs_ws/src/rmcs_bringup/launch/rmcs.launch.py +++ b/rmcs_ws/src/rmcs_bringup/launch/rmcs.launch.py @@ -52,10 +52,11 @@ def visit( pass entities.append( - IncludeLaunchDescription( - PythonLaunchDescriptionSource([ - FindPackageShare("odin_ros_driver"), "/launch/odin1_ros2.launch.py" - ]) + Node( + package="odin_ros_driver", + executable="tmux-launch.sh", + output="screen", + emulate_tty=True, ) ) diff --git a/rmcs_ws/src/rmcs_bringup/package.xml b/rmcs_ws/src/rmcs_bringup/package.xml index 43835a9b..8642966e 100644 --- a/rmcs_ws/src/rmcs_bringup/package.xml +++ b/rmcs_ws/src/rmcs_bringup/package.xml @@ -12,6 +12,7 @@ joint_state_broadcaster mavros + odin_ros_driver robot_state_publisher rviz2 xacro diff --git a/rmcs_ws/src/rmcs_core/src/hardware/flight_mavros.cpp b/rmcs_ws/src/rmcs_core/src/hardware/flight_mavros.cpp deleted file mode 100644 index 8536c831..00000000 --- a/rmcs_ws/src/rmcs_core/src/hardware/flight_mavros.cpp +++ /dev/null @@ -1,131 +0,0 @@ -#include -#include -#include -#include -#include - -#include - -#include -#include -#include -#include -#include - -#include - -namespace rmcs_core::hardware { - -class FlightMavros - : public rmcs_executor::Component - , public rclcpp::Node { -public: - FlightMavros() - : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} - , logger_(get_logger()) { - odometry_topic_ = get_parameter("odometry_topic").as_string(); - const auto mavros_pose_topic = get_parameter("mavros_pose_topic").as_string(); - const auto sensor_roll_offset_rad = get_parameter("sensor_roll_offset_rad").as_double(); - const auto sensor_pitch_offset_rad = get_parameter("sensor_pitch_offset_rad").as_double(); - const auto sensor_yaw_offset_rad = get_parameter("sensor_yaw_offset_rad").as_double(); - - sensor_to_body_rotation_ = quaternion_from_rpy( - sensor_roll_offset_rad, sensor_pitch_offset_rad, sensor_yaw_offset_rad); - sensor_to_body_rotation_.normalize(); - - mavros_pose_publisher_ = create_publisher( - mavros_pose_topic, rclcpp::QoS{10}.reliable()); - odometry_subscription_ = create_subscription( - odometry_topic_, rclcpp::QoS{10}.reliable(), - [this](const nav_msgs::msg::Odometry::ConstSharedPtr& msg) { odometry_callback(msg); }); - last_odometry_received_time_ns_.store( - get_clock()->now().nanoseconds(), std::memory_order_relaxed); - - RCLCPP_INFO( - logger_, - "FlightMavros bridging odometry '%s' to '%s' with sensor RPY [%.1f, %.1f, %.1f] " - "deg.", - odometry_topic_.c_str(), mavros_pose_publisher_->get_topic_name(), - radians_to_degrees(sensor_roll_offset_rad), radians_to_degrees(sensor_pitch_offset_rad), - radians_to_degrees(sensor_yaw_offset_rad)); - } - - ~FlightMavros() override = default; - - void update() override { - const auto now_ns = get_clock()->now().nanoseconds(); - const auto last_odometry_received_time_ns = - last_odometry_received_time_ns_.load(std::memory_order_relaxed); - if (now_ns - last_odometry_received_time_ns >= kOdometryTimeoutNs) { - RCLCPP_WARN_THROTTLE( - logger_, *get_clock(), 1000, "No odometry received on '%s' for more than %.1f s.", - odometry_topic_.c_str(), static_cast(kOdometryTimeoutNs) / 1'000'000'000.0); - } - } - -private: - static constexpr std::int64_t kOdometryTimeoutNs{1'000'000'000}; - - static double radians_to_degrees(double radians) { return radians * 180.0 / std::numbers::pi; } - - static Eigen::Quaterniond quaternion_from_rpy(double roll, double pitch, double yaw) { - const Eigen::AngleAxisd roll_rotation{roll, Eigen::Vector3d::UnitX()}; - const Eigen::AngleAxisd pitch_rotation{pitch, Eigen::Vector3d::UnitY()}; - const Eigen::AngleAxisd yaw_rotation{yaw, Eigen::Vector3d::UnitZ()}; - return Eigen::Quaterniond{roll_rotation * pitch_rotation * yaw_rotation}; - } - - Eigen::Quaterniond - rotate_body_orientation(const geometry_msgs::msg::Quaternion& orientation) const { - const Eigen::Quaterniond quat_sensor{ - orientation.w, orientation.x, orientation.y, orientation.z}; - - Eigen::Quaterniond quat_body = quat_sensor * sensor_to_body_rotation_; - quat_body.normalize(); - return quat_body; - } - - void odometry_callback(const nav_msgs::msg::Odometry::ConstSharedPtr& msg) { - last_odometry_received_time_ns_.store( - get_clock()->now().nanoseconds(), std::memory_order_relaxed); - - RCLCPP_INFO_ONCE( - logger_, "Received odometry from '%s' with frame '%s' and child frame '%s'.", - odometry_topic_.c_str(), msg->header.frame_id.c_str(), msg->child_frame_id.c_str()); - - if (msg->header.frame_id != "odom" || msg->child_frame_id != "odin1_base_link") { - RCLCPP_WARN_THROTTLE( - logger_, *get_clock(), 1000, - "Unexpected odometry frames: header='%s', child='%s'. Expected 'odom' and " - "'odin1_base_link'.", - msg->header.frame_id.c_str(), msg->child_frame_id.c_str()); - } - - geometry_msgs::msg::PoseStamped pose_msg{}; - pose_msg.header.stamp = msg->header.stamp; - pose_msg.header.frame_id = msg->header.frame_id; - pose_msg.pose.position = msg->pose.pose.position; - - const Eigen::Quaterniond rotated_orientation = - rotate_body_orientation(msg->pose.pose.orientation); - pose_msg.pose.orientation.x = rotated_orientation.x(); - pose_msg.pose.orientation.y = rotated_orientation.y(); - pose_msg.pose.orientation.z = rotated_orientation.z(); - pose_msg.pose.orientation.w = rotated_orientation.w(); - - mavros_pose_publisher_->publish(pose_msg); - } - - rclcpp::Logger logger_; - std::string odometry_topic_; - Eigen::Quaterniond sensor_to_body_rotation_{Eigen::Quaterniond::Identity()}; - std::atomic last_odometry_received_time_ns_{0}; - - rclcpp::Publisher::SharedPtr mavros_pose_publisher_; - rclcpp::Subscription::SharedPtr odometry_subscription_; -}; - -} // namespace rmcs_core::hardware - -#include -PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::FlightMavros, rmcs_executor::Component) From b27493560855165537e6186da0b2cb2c8d6786f0 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Sat, 16 May 2026 16:14:42 +0800 Subject: [PATCH 12/30] refactor: optimize odin1 launch script, fix referee serial port, update heat management, and conduct aerial tracking tests --- rmcs_ws/src/odin_ros_driver | 2 +- rmcs_ws/src/rmcs_bringup/config/flight.yaml | 14 +++++------ .../src/rmcs_bringup/launch/rmcs.launch.py | 25 ------------------- .../gimbal/simple_gimbal_controller.cpp | 2 +- .../bullet_feeder_controller_17mm.cpp | 3 ++- .../controller/shooting/heat_controller.cpp | 2 +- rmcs_ws/src/rmcs_core/src/hardware/flight.cpp | 4 +-- rmcs_ws/src/rmcs_core/src/referee/status.cpp | 4 +-- 8 files changed, 15 insertions(+), 41 deletions(-) diff --git a/rmcs_ws/src/odin_ros_driver b/rmcs_ws/src/odin_ros_driver index 165a7907..81cc4a60 160000 --- a/rmcs_ws/src/odin_ros_driver +++ b/rmcs_ws/src/odin_ros_driver @@ -1 +1 @@ -Subproject commit 165a79072fc292b73f388765e23a23aa99561f55 +Subproject commit 81cc4a604ce0f70d97ffee0ca766e89f43e238e9 diff --git a/rmcs_ws/src/rmcs_bringup/config/flight.yaml b/rmcs_ws/src/rmcs_bringup/config/flight.yaml index 1f21f85e..9a60b920 100644 --- a/rmcs_ws/src/rmcs_bringup/config/flight.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/flight.yaml @@ -104,7 +104,7 @@ pitch_angle_pid_controller: control: /gimbal/pitch/control_velocity kp: 10.0 ki: 0.0 - kd: 0.0 + kd: 0.0 friction_wheel_controller: ros__parameters: @@ -112,19 +112,19 @@ friction_wheel_controller: - /gimbal/left_friction - /gimbal/right_friction friction_velocities: - - 610.0 - - 610.0 + - 770.0 + - 770.0 friction_soft_start_stop_time: 1.0 heat_controller: ros__parameters: - heat_per_shot: 1 - reserved_heat: 0 + heat_per_shot: 10000 + reserved_heat: 10000 bullet_feeder_controller: ros__parameters: bullets_per_feeder_turn: 8.0 - shot_frequency: 20.0 + shot_frequency: 30.0 safe_shot_frequency: 10.0 eject_frequency: 10.0 eject_time: 0.05 @@ -155,6 +155,6 @@ bullet_feeder_velocity_pid_controller: measurement: /gimbal/bullet_feeder/velocity setpoint: /gimbal/bullet_feeder/control_velocity control: /gimbal/bullet_feeder/control_torque - kp: 0.583 + kp: 1.5 ki: 0.0 kd: 0.0 diff --git a/rmcs_ws/src/rmcs_bringup/launch/rmcs.launch.py b/rmcs_ws/src/rmcs_bringup/launch/rmcs.launch.py index b56e910d..adcafe2a 100644 --- a/rmcs_ws/src/rmcs_bringup/launch/rmcs.launch.py +++ b/rmcs_ws/src/rmcs_bringup/launch/rmcs.launch.py @@ -59,31 +59,6 @@ def visit( emulate_tty=True, ) ) - - mavros_share = FindPackageShare("mavros").perform(context) - entities.append( - Node( - package="mavros", - executable="mavros_node", - namespace="mavros", - parameters=[ - os.path.join(mavros_share, "launch", "px4_pluginlists.yaml"), - os.path.join(mavros_share, "launch", "px4_config.yaml"), - { - "fcu_url": "serial:///dev/ttyACM0:921600", - "gcs_url": "", - "tgt_system": 1, - "tgt_component": 1, - "fcu_protocol": "v2.0", - }, - ], - respawn=True, - respawn_delay=1.0, - output="screen", - emulate_tty=True, - ) - ) - entities.append( IncludeLaunchDescription( PythonLaunchDescriptionSource([ 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 44e26673..790f17c0 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 @@ -72,7 +72,7 @@ class SimpleGimbalController double yaw_shift = joystick_sensitivity * joystick_left_->y() + mouse_sensitivity * mouse_velocity_->y(); double pitch_shift = - -joystick_sensitivity * joystick_left_->x() - mouse_sensitivity * mouse_velocity_->x(); + -joystick_sensitivity * joystick_left_->x() + mouse_sensitivity * mouse_velocity_->x(); return two_axis_gimbal_solver.update( TwoAxisGimbalSolver::SetControlShift(yaw_shift, pitch_shift)); diff --git a/rmcs_ws/src/rmcs_core/src/controller/shooting/bullet_feeder_controller_17mm.cpp b/rmcs_ws/src/rmcs_core/src/controller/shooting/bullet_feeder_controller_17mm.cpp index e90e62cc..4b148cd7 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/shooting/bullet_feeder_controller_17mm.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/shooting/bullet_feeder_controller_17mm.cpp @@ -103,7 +103,8 @@ class BulletFeederController17mm if (*friction_ready_) { if (shoot_mode == ShootMode::AUTOMATIC) { bool triggered = mouse.left || switch_left == Switch::DOWN - || (switch_right == Switch::UP && *auto_aim_should_shoot_); + || (mouse.right && switch_right == Switch::UP + && *auto_aim_should_shoot_); bullet_allowance = triggered ? *control_bullet_allowance_limited_by_heat_ : 0; } else { diff --git a/rmcs_ws/src/rmcs_core/src/controller/shooting/heat_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/shooting/heat_controller.cpp index 3de0960a..c6901fbc 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/shooting/heat_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/shooting/heat_controller.cpp @@ -53,4 +53,4 @@ class HeatController #include -PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::shooting::HeatController, rmcs_executor::Component) \ No newline at end of file +PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::shooting::HeatController, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp b/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp index d0d15155..bfbedabb 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp @@ -106,7 +106,7 @@ class Flight [&buffer](std::byte byte) noexcept { *buffer++ = byte; }, size); }; referee_serial_->write = [this](const std::byte* buffer, size_t size) { - start_transmit().uart1_transmit( + start_transmit().uart2_transmit( {.uart_data = std::span{buffer, size}}); return size; }; @@ -215,7 +215,7 @@ class Flight } } - void uart1_receive_callback(const librmcs::data::UartDataView& data) override { + void uart2_receive_callback(const librmcs::data::UartDataView& data) override { const std::byte* ptr = data.uart_data.data(); referee_ring_buffer_receive_.emplace_back_n( [&ptr](std::byte* storage) noexcept { new (storage) std::byte{*ptr++}; }, diff --git a/rmcs_ws/src/rmcs_core/src/referee/status.cpp b/rmcs_ws/src/rmcs_core/src/referee/status.cpp index c080a355..6cb4e026 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/status.cpp +++ b/rmcs_ws/src/rmcs_core/src/referee/status.cpp @@ -21,9 +21,7 @@ class Status , public rclcpp::Node { public: Status() - : Node{ - get_component_name(), - rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} + : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} , logger_(get_logger()) { register_input("/referee/serial", serial_); From b015a98054cc57b97207f2955faab542aac62450 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Tue, 19 May 2026 14:46:20 +0800 Subject: [PATCH 13/30] config: update bullet speed --- rmcs_ws/src/rmcs_bringup/config/flight.yaml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/flight.yaml b/rmcs_ws/src/rmcs_bringup/config/flight.yaml index 9a60b920..dd83cbb6 100644 --- a/rmcs_ws/src/rmcs_bringup/config/flight.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/flight.yaml @@ -112,14 +112,14 @@ friction_wheel_controller: - /gimbal/left_friction - /gimbal/right_friction friction_velocities: - - 770.0 - - 770.0 + - 610.0 + - 610.0 friction_soft_start_stop_time: 1.0 heat_controller: ros__parameters: heat_per_shot: 10000 - reserved_heat: 10000 + reserved_heat: 15000 bullet_feeder_controller: ros__parameters: From 525d97920431fa434638c26366b961ece6557cd4 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Wed, 20 May 2026 04:45:50 +0800 Subject: [PATCH 14/30] feat: update serial baund, communicate with fcu successfully --- .script/template/entrypoint | 15 ++++++++++++++- rmcs_ws/src/rmcs_bringup/config/flight.yaml | 15 ++------------- 2 files changed, 16 insertions(+), 14 deletions(-) diff --git a/.script/template/entrypoint b/.script/template/entrypoint index e8660e7a..49290c5c 100755 --- a/.script/template/entrypoint +++ b/.script/template/entrypoint @@ -1,5 +1,18 @@ #!/usr/bin/bash +USBFS_MEMORY_MB="${USBFS_MEMORY_MB:-2000}" +USBFS_MEMORY_MB_PATH="/sys/module/usbcore/parameters/usbfs_memory_mb" + +if [ -w "$USBFS_MEMORY_MB_PATH" ]; then + if ! printf '%s\n' "$USBFS_MEMORY_MB" > "$USBFS_MEMORY_MB_PATH"; then + echo "Warning: failed to set $USBFS_MEMORY_MB_PATH to $USBFS_MEMORY_MB" >&2 + fi +elif [ -e "$USBFS_MEMORY_MB_PATH" ]; then + echo "Warning: $USBFS_MEMORY_MB_PATH is not writable, skipping usbfs_memory_mb setup" >&2 +else + echo "Warning: $USBFS_MEMORY_MB_PATH not found, skipping usbfs_memory_mb setup" >&2 +fi + # Remove all files in /tmp rm -rf /tmp/* @@ -14,4 +27,4 @@ if [ -f "/etc/avahi/enabled" ]; then service avahi-daemon start fi -sleep infinity \ No newline at end of file +sleep infinity diff --git a/rmcs_ws/src/rmcs_bringup/config/flight.yaml b/rmcs_ws/src/rmcs_bringup/config/flight.yaml index dd83cbb6..31a2bdd2 100644 --- a/rmcs_ws/src/rmcs_bringup/config/flight.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/flight.yaml @@ -25,17 +25,6 @@ rmcs_executor: - rmcs::AutoAimComponent -> auto_aim_component -mavros: - ros__parameters: - enabled: true - fcu_url: serial:///dev/ttyACM0:921600 - gcs_url: "" - target_system_id: 1 - target_component_id: 1 - fcu_protocol: v2.0 - respawn: true - respawn_delay: 1.0 - odin_ros_driver: ros__parameters: enabled: true @@ -112,8 +101,8 @@ friction_wheel_controller: - /gimbal/left_friction - /gimbal/right_friction friction_velocities: - - 610.0 - - 610.0 + - 605.0 + - 605.0 friction_soft_start_stop_time: 1.0 heat_controller: From 097c64e45e589bde257cd2d17cd5ade65e20bf84 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Wed, 20 May 2026 04:49:25 +0800 Subject: [PATCH 15/30] feat: update odin ros driver --- rmcs_ws/src/odin_ros_driver | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rmcs_ws/src/odin_ros_driver b/rmcs_ws/src/odin_ros_driver index 81cc4a60..5cb37012 160000 --- a/rmcs_ws/src/odin_ros_driver +++ b/rmcs_ws/src/odin_ros_driver @@ -1 +1 @@ -Subproject commit 81cc4a604ce0f70d97ffee0ca766e89f43e238e9 +Subproject commit 5cb37012a12686bb757923244eb0612285012c04 From b5216ace0b863cff33409dd8f8ead54e8bc77ecb Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Thu, 21 May 2026 05:37:54 +0800 Subject: [PATCH 16/30] feat: update ui and gimbal control --- rmcs_ws/src/rmcs_bringup/config/flight.yaml | 11 +-- rmcs_ws/src/rmcs_core/plugins.xml | 1 + .../gimbal/simple_gimbal_controller.cpp | 4 +- .../rmcs_core/src/referee/app/ui/flight.cpp | 71 +++++++++++++++++++ .../src/referee/app/ui/widget/status_ring.hpp | 30 ++++++-- 5 files changed, 107 insertions(+), 10 deletions(-) create mode 100644 rmcs_ws/src/rmcs_core/src/referee/app/ui/flight.cpp diff --git a/rmcs_ws/src/rmcs_bringup/config/flight.yaml b/rmcs_ws/src/rmcs_bringup/config/flight.yaml index 31a2bdd2..a42e0dcb 100644 --- a/rmcs_ws/src/rmcs_bringup/config/flight.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/flight.yaml @@ -19,6 +19,8 @@ rmcs_executor: - rmcs_core::referee::command::Interaction -> referee_interaction - rmcs_core::referee::Command -> referee_command - rmcs_core::referee::Status -> referee_status + - rmcs_core::referee::command::interaction::Ui -> referee_ui + - rmcs_core::referee::app::ui::Flight -> referee_ui_flight - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster - rmcs_core::broadcaster::TfBroadcaster -> tf_broadcaster @@ -49,6 +51,7 @@ value_broadcaster: - /gimbal/yaw/control_torque - /gimbal/yaw/control_angle_error - /gimbal/yaw/velocity_imu + - /gimbal/bullet_feeder/velocity tf_broadcaster: ros__parameters: @@ -101,8 +104,8 @@ friction_wheel_controller: - /gimbal/left_friction - /gimbal/right_friction friction_velocities: - - 605.0 - - 605.0 + - 600.0 + - 600.0 friction_soft_start_stop_time: 1.0 heat_controller: @@ -113,7 +116,7 @@ heat_controller: bullet_feeder_controller: ros__parameters: bullets_per_feeder_turn: 8.0 - shot_frequency: 30.0 + shot_frequency: 25.0 safe_shot_frequency: 10.0 eject_frequency: 10.0 eject_time: 0.05 @@ -144,6 +147,6 @@ bullet_feeder_velocity_pid_controller: measurement: /gimbal/bullet_feeder/velocity setpoint: /gimbal/bullet_feeder/control_velocity control: /gimbal/bullet_feeder/control_torque - kp: 1.5 + kp: 0.583 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 58f30161..51fb30aa 100644 --- a/rmcs_ws/src/rmcs_core/plugins.xml +++ b/rmcs_ws/src/rmcs_core/plugins.xml @@ -50,6 +50,7 @@ + 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 790f17c0..7873188b 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 @@ -52,12 +52,12 @@ class SimpleGimbalController || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) return two_axis_gimbal_solver.update(TwoAxisGimbalSolver::SetDisabled()); - const auto manual_active = switch_right == Switch::UP; + const auto auto_aim_requested = mouse_->right || switch_right == Switch::UP; const auto should_control = auto_aim_should_control_.ready() && *auto_aim_should_control_; const auto valid_control = auto_aim_control_direction_.ready() && auto_aim_control_direction_->allFinite(); - if (manual_active && should_control && valid_control) { + if (auto_aim_requested && should_control && valid_control) { return two_axis_gimbal_solver.update( TwoAxisGimbalSolver::SetControlDirection( OdomImu::DirectionVector(*auto_aim_control_direction_))); diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/flight.cpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/flight.cpp new file mode 100644 index 00000000..9ef80e79 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/flight.cpp @@ -0,0 +1,71 @@ +#include +#include + +#include +#include +#include + +#include "referee/app/ui/shape/shape.hpp" +#include "referee/app/ui/widget/crosshair_circle.hpp" +#include "referee/app/ui/widget/status_ring.hpp" + +namespace rmcs_core::referee::app::ui { +using namespace std::chrono_literals; + +class Flight + : public rmcs_executor::Component + , public rclcpp::Node { +public: + Flight() + : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} + , crosshair_circle_(Shape::Color::WHITE, x_center - 2, y_center - 30, 8, 2) + , status_ring_( + 26.5, 26.5, 600, 300, StatusRing::DynamicArcsVisibility{false, false}) + , horizontal_center_guidelines_( + {Shape::Color::WHITE, 2, x_center - 360, y_center, x_center - 110, y_center}, + {Shape::Color::WHITE, 2, x_center + 110, y_center, x_center + 360, y_center}) + , vertical_center_guidelines_( + {Shape::Color::WHITE, 2, x_center, 800, x_center, y_center + 110}, + {Shape::Color::WHITE, 2, x_center, y_center - 110, x_center, 200}) { + + register_input("/referee/shooter/bullet_allowance", robot_bullet_allowance_); + + register_input("/gimbal/left_friction/control_velocity", left_friction_control_velocity_); + register_input("/gimbal/left_friction/velocity", left_friction_velocity_); + register_input("/gimbal/right_friction/velocity", right_friction_velocity_); + + register_input("/remote/mouse", mouse_); + } + + void update() override { + status_ring_.update_bullet_allowance(*robot_bullet_allowance_); + status_ring_.update_friction_wheel_speed( + std::min(*left_friction_velocity_, *right_friction_velocity_), + *left_friction_control_velocity_ > 0); + status_ring_.update_auto_aim_enable(mouse_->right == 1); + } + +private: + static constexpr uint16_t screen_width = 1920, screen_height = 1080; + static constexpr uint16_t x_center = screen_width / 2, y_center = screen_height / 2; + + InputInterface robot_bullet_allowance_; + + InputInterface left_friction_control_velocity_; + InputInterface left_friction_velocity_; + InputInterface right_friction_velocity_; + + InputInterface mouse_; + + CrossHairCircle crosshair_circle_; + StatusRing status_ring_; + + Line horizontal_center_guidelines_[2]; + Line vertical_center_guidelines_[2]; +}; + +} // namespace rmcs_core::referee::app::ui + +#include + +PLUGINLIB_EXPORT_CLASS(rmcs_core::referee::app::ui::Flight, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/status_ring.hpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/status_ring.hpp index 8af3d993..32d8c614 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/status_ring.hpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/status_ring.hpp @@ -16,8 +16,20 @@ namespace rmcs_core::referee::app::ui { class StatusRing { public: + struct DynamicArcsVisibility { + bool supercap; + bool battery; + }; + + StatusRing(double supercap_limit, double battery_limit, double friction_limit, int16_t bullet_limit) + : StatusRing( + supercap_limit, battery_limit, friction_limit, bullet_limit, + DynamicArcsVisibility{true, true}) {} + StatusRing( - double supercap_limit, double battery_limit, double friction_limit, int16_t bullet_limit) { + double supercap_limit, double battery_limit, double friction_limit, int16_t bullet_limit, + DynamicArcsVisibility dynamic_arcs_visibility) + : dynamic_arcs_visibility_(dynamic_arcs_visibility) { supercap_status_.set_x(x_center); supercap_status_.set_y(y_center); supercap_status_.set_r(visible_radius - width_ring); @@ -108,12 +120,14 @@ class StatusRing { arc_right_down_.set_visible(true); set_limits(supercap_limit, battery_limit, friction_limit, bullet_limit); + supercap_status_.set_visible(dynamic_arcs_visibility_.supercap); + battery_status_.set_visible(dynamic_arcs_visibility_.battery); } void set_visible(bool value) { // Dynamic - supercap_status_.set_visible(value); - battery_status_.set_visible(value); + supercap_status_.set_visible(value && dynamic_arcs_visibility_.supercap); + battery_status_.set_visible(value && dynamic_arcs_visibility_.battery); friction_wheel_speed_.set_visible(value); bullet_status_.set_visible(value); @@ -246,6 +260,9 @@ class StatusRing { } void update_supercap(double value, bool enable) { + if (!dynamic_arcs_visibility_.supercap) + return; + auto angle = 275 + calculate_angle(value, 10.5, supercap_limit_) + 1; supercap_status_.set_angle_end(static_cast(angle)); @@ -259,6 +276,9 @@ class StatusRing { } void update_battery_power(double value) { + if (!dynamic_arcs_visibility_.battery) + return; + auto angle = 265 - calculate_angle(value, 20, 25.7) - 1; battery_status_.set_angle_start(static_cast(angle)); @@ -353,6 +373,8 @@ class StatusRing { constexpr static uint16_t visible_radius = 400; constexpr static uint16_t visible_angle = 40; + DynamicArcsVisibility dynamic_arcs_visibility_; + double supercap_limit_; double battery_limit_; double friction_limit_; @@ -378,4 +400,4 @@ class StatusRing { Arc bullet_scales_[4]; }; -} // namespace rmcs_core::referee::app::ui \ No newline at end of file +} // namespace rmcs_core::referee::app::ui From af533f1299a5de2d87711073be11c99fb3e923db Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Fri, 22 May 2026 11:02:50 +0800 Subject: [PATCH 17/30] update auto aim shoot logic --- .../src/controller/shooting/bullet_feeder_controller_17mm.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/rmcs_ws/src/rmcs_core/src/controller/shooting/bullet_feeder_controller_17mm.cpp b/rmcs_ws/src/rmcs_core/src/controller/shooting/bullet_feeder_controller_17mm.cpp index 4b148cd7..2b7b0d29 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/shooting/bullet_feeder_controller_17mm.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/shooting/bullet_feeder_controller_17mm.cpp @@ -103,8 +103,7 @@ class BulletFeederController17mm if (*friction_ready_) { if (shoot_mode == ShootMode::AUTOMATIC) { bool triggered = mouse.left || switch_left == Switch::DOWN - || (mouse.right && switch_right == Switch::UP - && *auto_aim_should_shoot_); + || (mouse.right && *auto_aim_should_shoot_); bullet_allowance = triggered ? *control_bullet_allowance_limited_by_heat_ : 0; } else { From b05cb5e6b9d0dd2eb4d2c3a3f9c82248e4232145 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Sat, 23 May 2026 16:12:49 +0800 Subject: [PATCH 18/30] feat: add knob-controlled low-frequency firing and remove redundant UI code --- rmcs_ws/src/rmcs_bringup/config/flight.yaml | 4 +- .../bullet_feeder_controller_17mm.cpp | 31 ++++++++-- .../rmcs_core/src/referee/app/ui/flight.cpp | 62 ++++++++++++++++++- .../src/referee/app/ui/shape/shape.hpp | 8 ++- .../src/referee/command/interaction/ui.cpp | 38 +++++++++--- 5 files changed, 123 insertions(+), 20 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/flight.yaml b/rmcs_ws/src/rmcs_bringup/config/flight.yaml index a42e0dcb..db7bea5b 100644 --- a/rmcs_ws/src/rmcs_bringup/config/flight.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/flight.yaml @@ -104,8 +104,8 @@ friction_wheel_controller: - /gimbal/left_friction - /gimbal/right_friction friction_velocities: - - 600.0 - - 600.0 + - 620.0 + - 620.0 friction_soft_start_stop_time: 1.0 heat_controller: diff --git a/rmcs_ws/src/rmcs_core/src/controller/shooting/bullet_feeder_controller_17mm.cpp b/rmcs_ws/src/rmcs_core/src/controller/shooting/bullet_feeder_controller_17mm.cpp index 2b7b0d29..950fcdc7 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/shooting/bullet_feeder_controller_17mm.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/shooting/bullet_feeder_controller_17mm.cpp @@ -28,6 +28,9 @@ class BulletFeederController17mm bullet_feeder_working_velocity = bullet_feeder_angle_per_bullet * shot_frequency; double safe_shot_frequency = get_parameter("safe_shot_frequency").as_double(); bullet_feeder_safe_shot_velocity = bullet_feeder_angle_per_bullet * safe_shot_frequency; + constexpr double rotary_knob_shot_frequency = 8.0; + bullet_feeder_rotary_knob_velocity_ = + bullet_feeder_angle_per_bullet * rotary_knob_shot_frequency; double eject_frequency = get_parameter("eject_frequency").as_double(); bullet_feeder_eject_velocity_ = -bullet_feeder_angle_per_bullet * eject_frequency; @@ -52,6 +55,7 @@ class BulletFeederController17mm register_input("/remote/switch/left", switch_left_); register_input("/remote/mouse", mouse_); register_input("/remote/keyboard", keyboard_); + register_input("/remote/rotary_knob_switch", rotary_knob_switch_); register_input("/auto_aim/should_shoot", auto_aim_should_shoot_, false); @@ -74,6 +78,7 @@ class BulletFeederController17mm const auto switch_left = *switch_left_; const auto mouse = *mouse_; const auto keyboard = *keyboard_; + const auto rotary_knob_switch = *rotary_knob_switch_; using namespace rmcs_msgs; if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) @@ -102,19 +107,29 @@ class BulletFeederController17mm if (*friction_ready_) { if (shoot_mode == ShootMode::AUTOMATIC) { - bool triggered = mouse.left || switch_left == Switch::DOWN - || (mouse.right && *auto_aim_should_shoot_); + bool manual_triggered = mouse.left || switch_left == Switch::DOWN; + bool auto_aim_triggered = mouse.right && *auto_aim_should_shoot_; + bool rotary_knob_triggered = rotary_knob_switch == Switch::DOWN; + bool triggered = + manual_triggered || auto_aim_triggered || rotary_knob_triggered; + bool rotary_knob_low_speed = + rotary_knob_triggered && !manual_triggered && !auto_aim_triggered; bullet_allowance = triggered ? *control_bullet_allowance_limited_by_heat_ : 0; + update_bullet_feeder_velocity(bullet_allowance, rotary_knob_low_speed); } else { bool triggered = single_shot_stop_counter_ > 0; bullet_allowance = triggered && (*control_bullet_allowance_limited_by_heat_ > 0); + update_bullet_feeder_velocity(bullet_allowance, false); } + } else { + update_bullet_feeder_velocity(0, false); } } - update_bullet_feeder_velocity(bullet_allowance); + if (!*friction_ready_ || switch_right == Switch::DOWN) + update_bullet_feeder_velocity(0, false); } last_switch_right_ = switch_right; @@ -130,7 +145,7 @@ class BulletFeederController17mm *bullet_feeder_control_velocity_ = nan_; } - void update_bullet_feeder_velocity(int64_t bullet_allowance) { + void update_bullet_feeder_velocity(int64_t bullet_allowance, bool rotary_knob_low_speed) { if (bullet_allowance <= 0) { bullet_feeder_working_status_ = 0; *bullet_feeder_control_velocity_ = 0.0; @@ -144,8 +159,9 @@ class BulletFeederController17mm return; } - double new_control_velocity = bullet_allowance > 1 ? bullet_feeder_working_velocity - : bullet_feeder_safe_shot_velocity; + double new_control_velocity = rotary_knob_low_speed ? bullet_feeder_rotary_knob_velocity_ + : bullet_allowance > 1 ? bullet_feeder_working_velocity + : bullet_feeder_safe_shot_velocity; if (new_control_velocity > *bullet_feeder_control_velocity_) bullet_feeder_working_status_ = std::min(0, bullet_feeder_working_status_); *bullet_feeder_control_velocity_ = new_control_velocity; @@ -208,6 +224,7 @@ class BulletFeederController17mm InputInterface switch_left_; InputInterface mouse_; InputInterface keyboard_; + InputInterface rotary_knob_switch_; InputInterface auto_aim_should_shoot_; @@ -221,6 +238,8 @@ class BulletFeederController17mm int bullet_feeder_jammed_count_ = 0; int bullet_feeder_cool_down_ = 0; + double bullet_feeder_rotary_knob_velocity_ = 0.0; + int temporary_single_shot_counter_ = 0; OutputInterface shoot_mode_; diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/flight.cpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/flight.cpp index 9ef80e79..520f46b9 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/flight.cpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/flight.cpp @@ -1,5 +1,8 @@ #include +#include #include +#include +#include #include #include @@ -16,6 +19,9 @@ class Flight : public rmcs_executor::Component , public rclcpp::Node { public: + static constexpr uint8_t kUiModeCombat = 0; + static constexpr uint8_t kUiModeOutpostOnly = 1; + Flight() : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} , crosshair_circle_(Shape::Color::WHITE, x_center - 2, y_center - 30, 8, 2) @@ -26,7 +32,8 @@ class Flight {Shape::Color::WHITE, 2, x_center + 110, y_center, x_center + 360, y_center}) , vertical_center_guidelines_( {Shape::Color::WHITE, 2, x_center, 800, x_center, y_center + 110}, - {Shape::Color::WHITE, 2, x_center, y_center - 110, x_center, 200}) { + {Shape::Color::WHITE, 2, x_center, y_center - 110, x_center, 200}) + , auto_aim_text_(Shape::Color::WHITE, 16, 2, 1320, 60, auto_aim_buffers_[0].data()) { register_input("/referee/shooter/bullet_allowance", robot_bullet_allowance_); @@ -34,7 +41,11 @@ class Flight register_input("/gimbal/left_friction/velocity", left_friction_velocity_); register_input("/gimbal/right_friction/velocity", right_friction_velocity_); + register_input("/auto_aim/ui_mode", auto_aim_mode_, false); + register_input("/remote/mouse", mouse_); + + write_text(auto_aim_buffers_[0], "MODE:COMBAT"); } void update() override { @@ -43,11 +54,53 @@ class Flight std::min(*left_friction_velocity_, *right_friction_velocity_), *left_friction_control_velocity_ > 0); status_ring_.update_auto_aim_enable(mouse_->right == 1); + + update_auto_aim_text(); } private: static constexpr uint16_t screen_width = 1920, screen_height = 1080; static constexpr uint16_t x_center = screen_width / 2, y_center = screen_height / 2; + static constexpr std::size_t text_capacity = 31; + + static auto to_mode_string(uint8_t mode) -> const char* { + switch (mode) { + case kUiModeCombat: return "COMBAT"; + case kUiModeOutpostOnly: return "OUTPOST"; + default: return "UNKNOWN"; + } + } + + static auto write_text( + std::array& buffer, const char* format, const char* mode_value) + -> void { + std::snprintf(buffer.data(), buffer.size(), format, mode_value); + } + + static auto write_text(std::array& buffer, const char* value) -> void { + std::snprintf(buffer.data(), buffer.size(), "%s", value); + } + + auto update_text_shape(Text& text, std::array (&buffers)[2], + uint8_t& active_buffer, const char* format, const char* mode_value) -> void { + auto next_buffer = static_cast(1 - active_buffer); + write_text(buffers[next_buffer], format, mode_value); + if (std::string_view { buffers[next_buffer].data() } + == std::string_view { buffers[active_buffer].data() }) { + return; + } + + active_buffer = next_buffer; + text.set_value(buffers[active_buffer].data()); + } + + auto update_auto_aim_text() -> void { + auto mode_value = auto_aim_mode_.ready() ? *auto_aim_mode_ : kUiModeCombat; + + update_text_shape( + auto_aim_text_, auto_aim_buffers_, active_auto_aim_buffer_, "MODE:%s", + to_mode_string(mode_value)); + } InputInterface robot_bullet_allowance_; @@ -55,6 +108,8 @@ class Flight InputInterface left_friction_velocity_; InputInterface right_friction_velocity_; + InputInterface auto_aim_mode_; + InputInterface mouse_; CrossHairCircle crosshair_circle_; @@ -62,6 +117,11 @@ class Flight Line horizontal_center_guidelines_[2]; Line vertical_center_guidelines_[2]; + + std::array auto_aim_buffers_[2] {}; + uint8_t active_auto_aim_buffer_ { 0 }; + + Text auto_aim_text_; }; } // namespace rmcs_core::referee::app::ui diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/shape.hpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/shape.hpp index 37be26ae..ea064f50 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/shape.hpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/shape.hpp @@ -192,6 +192,8 @@ class Shape enter_run_queue(); } + void set_text_shape() { is_text_shape_ = true; } + virtual size_t write_description_field(std::byte* buffer) = 0; DescriptionField::Part2 part2_ alignas(4); @@ -722,8 +724,10 @@ class Float : public Integer { class Text : public Shape { public: - Text() - : Shape(true) {} + Text() { + value_ = nullptr; + set_text_shape(); + }; Text( Color color, uint16_t font_size, uint16_t width, uint16_t x, uint16_t y, const char* value, bool visible = true) diff --git a/rmcs_ws/src/rmcs_core/src/referee/command/interaction/ui.cpp b/rmcs_ws/src/rmcs_core/src/referee/command/interaction/ui.cpp index cea7953c..4d57f606 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/command/interaction/ui.cpp +++ b/rmcs_ws/src/rmcs_core/src/referee/command/interaction/ui.cpp @@ -87,6 +87,10 @@ class Ui } size_t write_updating_field(std::byte* buffer) const { + if (auto written = write_updating_text_field(buffer); written != 0) { + return written; + } + size_t written = 0; auto& header = *new (buffer + written) Header{}; @@ -98,15 +102,9 @@ class Ui int slot = 0; intptr_t updated[7]; for (auto it = CfsScheduler::get_update_iterator(); it && slot < 7;) { - // Ignore text shape unless it is the first. if (it->is_text_shape()) { - if (slot == 0) { - header.command_id = 0x0110; // Draw text shape - return written + it.update().write(buffer + written); - } else { - it.ignore(); - continue; - } + it.ignore(); + continue; } auto operation = it->predict_update(); @@ -148,6 +146,28 @@ class Ui return written; } + size_t write_updating_text_field(std::byte* buffer) const { + size_t written = 0; + + auto& header = *new (buffer + written) Header{}; + auto full_robot_id = rmcs_msgs::FullRobotId { *robot_id_ }; + header.command_id = 0x0110; // Draw text shape + header.sender_id = full_robot_id; + header.receiver_id = full_robot_id.client(); + written += sizeof(Header); + + for (auto it = CfsScheduler::get_update_iterator(); it;) { + if (!it->is_text_shape()) { + it.ignore(); + continue; + } + + return written + it.update().write(buffer + written); + } + + return 0; + } + InputInterface robot_id_; InputInterface game_stage_; @@ -165,4 +185,4 @@ class Ui #include -PLUGINLIB_EXPORT_CLASS(rmcs_core::referee::command::interaction::Ui, rmcs_executor::Component) \ No newline at end of file +PLUGINLIB_EXPORT_CLASS(rmcs_core::referee::command::interaction::Ui, rmcs_executor::Component) From cfc7bd7d1e1c889ad0935a905b1c562ea6b1238c Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Sat, 23 May 2026 21:32:20 +0800 Subject: [PATCH 19/30] refactor(ui):update flight ui, add mode line --- .../rmcs_core/src/referee/app/ui/flight.cpp | 65 ++++--------------- 1 file changed, 11 insertions(+), 54 deletions(-) diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/flight.cpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/flight.cpp index 520f46b9..12219d38 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/flight.cpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/flight.cpp @@ -1,8 +1,5 @@ #include -#include #include -#include -#include #include #include @@ -13,27 +10,26 @@ #include "referee/app/ui/widget/status_ring.hpp" namespace rmcs_core::referee::app::ui { -using namespace std::chrono_literals; - class Flight : public rmcs_executor::Component , public rclcpp::Node { public: - static constexpr uint8_t kUiModeCombat = 0; + static constexpr uint8_t kUiModeCombat = 0; static constexpr uint8_t kUiModeOutpostOnly = 1; Flight() : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} , crosshair_circle_(Shape::Color::WHITE, x_center - 2, y_center - 30, 8, 2) - , status_ring_( - 26.5, 26.5, 600, 300, StatusRing::DynamicArcsVisibility{false, false}) + , status_ring_(26.5, 26.5, 600, 300, StatusRing::DynamicArcsVisibility{false, false}) , horizontal_center_guidelines_( {Shape::Color::WHITE, 2, x_center - 360, y_center, x_center - 110, y_center}, {Shape::Color::WHITE, 2, x_center + 110, y_center, x_center + 360, y_center}) , vertical_center_guidelines_( {Shape::Color::WHITE, 2, x_center, 800, x_center, y_center + 110}, {Shape::Color::WHITE, 2, x_center, y_center - 110, x_center, 200}) - , auto_aim_text_(Shape::Color::WHITE, 16, 2, 1320, 60, auto_aim_buffers_[0].data()) { + , mode_indicator_( + Shape::Color::YELLOW, 30, x_center + 360, y_center + 220, x_center + 400, + y_center + 220) { register_input("/referee/shooter/bullet_allowance", robot_bullet_allowance_); @@ -44,8 +40,6 @@ class Flight register_input("/auto_aim/ui_mode", auto_aim_mode_, false); register_input("/remote/mouse", mouse_); - - write_text(auto_aim_buffers_[0], "MODE:COMBAT"); } void update() override { @@ -55,51 +49,17 @@ class Flight *left_friction_control_velocity_ > 0); status_ring_.update_auto_aim_enable(mouse_->right == 1); - update_auto_aim_text(); + update_mode_indicator(); } private: static constexpr uint16_t screen_width = 1920, screen_height = 1080; static constexpr uint16_t x_center = screen_width / 2, y_center = screen_height / 2; - static constexpr std::size_t text_capacity = 31; - - static auto to_mode_string(uint8_t mode) -> const char* { - switch (mode) { - case kUiModeCombat: return "COMBAT"; - case kUiModeOutpostOnly: return "OUTPOST"; - default: return "UNKNOWN"; - } - } - static auto write_text( - std::array& buffer, const char* format, const char* mode_value) - -> void { - std::snprintf(buffer.data(), buffer.size(), format, mode_value); - } - - static auto write_text(std::array& buffer, const char* value) -> void { - std::snprintf(buffer.data(), buffer.size(), "%s", value); - } - - auto update_text_shape(Text& text, std::array (&buffers)[2], - uint8_t& active_buffer, const char* format, const char* mode_value) -> void { - auto next_buffer = static_cast(1 - active_buffer); - write_text(buffers[next_buffer], format, mode_value); - if (std::string_view { buffers[next_buffer].data() } - == std::string_view { buffers[active_buffer].data() }) { - return; - } - - active_buffer = next_buffer; - text.set_value(buffers[active_buffer].data()); - } - - auto update_auto_aim_text() -> void { - auto mode_value = auto_aim_mode_.ready() ? *auto_aim_mode_ : kUiModeCombat; - - update_text_shape( - auto_aim_text_, auto_aim_buffers_, active_auto_aim_buffer_, "MODE:%s", - to_mode_string(mode_value)); + auto update_mode_indicator() -> void { + auto mode_value = auto_aim_mode_.ready() ? *auto_aim_mode_ : kUiModeCombat; + mode_indicator_.set_color( + mode_value == kUiModeOutpostOnly ? Shape::Color::GREEN : Shape::Color::YELLOW); } InputInterface robot_bullet_allowance_; @@ -118,10 +78,7 @@ class Flight Line horizontal_center_guidelines_[2]; Line vertical_center_guidelines_[2]; - std::array auto_aim_buffers_[2] {}; - uint8_t active_auto_aim_buffer_ { 0 }; - - Text auto_aim_text_; + Line mode_indicator_; }; } // namespace rmcs_core::referee::app::ui From 6b689f4b98ffc2941102b673fcd4d18b9c461547 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Tue, 2 Jun 2026 17:22:34 +0800 Subject: [PATCH 20/30] feat: add engineer UI mode for flight --- rmcs_ws/src/rmcs_auto_aim_v2 | 2 +- .../src/rmcs_core/src/referee/app/ui/flight.cpp | 16 ++++++++++++++-- 2 files changed, 15 insertions(+), 3 deletions(-) diff --git a/rmcs_ws/src/rmcs_auto_aim_v2 b/rmcs_ws/src/rmcs_auto_aim_v2 index 8c16a237..f3bae295 160000 --- a/rmcs_ws/src/rmcs_auto_aim_v2 +++ b/rmcs_ws/src/rmcs_auto_aim_v2 @@ -1 +1 @@ -Subproject commit 8c16a23745ed4c880f0823f90d37d5fd2362c015 +Subproject commit f3bae2952106975478809f6b21fabf7dc36f2668 diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/flight.cpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/flight.cpp index 12219d38..b9bbd968 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/flight.cpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/flight.cpp @@ -16,6 +16,7 @@ class Flight public: static constexpr uint8_t kUiModeCombat = 0; static constexpr uint8_t kUiModeOutpostOnly = 1; + static constexpr uint8_t kUiModeEngineer = 2; Flight() : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} @@ -58,8 +59,19 @@ class Flight auto update_mode_indicator() -> void { auto mode_value = auto_aim_mode_.ready() ? *auto_aim_mode_ : kUiModeCombat; - mode_indicator_.set_color( - mode_value == kUiModeOutpostOnly ? Shape::Color::GREEN : Shape::Color::YELLOW); + + switch (mode_value) { + case kUiModeOutpostOnly: + mode_indicator_.set_color(Shape::Color::GREEN); + break; + case kUiModeEngineer: + mode_indicator_.set_color(Shape::Color::PINK); + break; + case kUiModeCombat: + default: + mode_indicator_.set_color(Shape::Color::YELLOW); + break; + } } InputInterface robot_bullet_allowance_; From 63c79e81c70d2c68dcf471b4e7bab48a564e9017 Mon Sep 17 00:00:00 2001 From: noskillzheng <3515964992@qq.com> Date: Thu, 18 Jun 2026 17:41:24 +0800 Subject: [PATCH 21/30] feat: 1.add yaw limit 2.permit testing feeder without fricition wheel; fix:reverse the feeder's rotation direction --- rmcs_ws/src/rmcs_bringup/config/flight.yaml | 4 +- .../gimbal/simple_gimbal_controller.cpp | 8 ++++ .../gimbal/two_axis_gimbal_solver.hpp | 38 +++++++++++++++++++ .../bullet_feeder_controller_17mm.cpp | 14 ++++++- rmcs_ws/src/rmcs_core/src/hardware/flight.cpp | 1 - 5 files changed, 61 insertions(+), 4 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/flight.yaml b/rmcs_ws/src/rmcs_bringup/config/flight.yaml index db7bea5b..aaadd145 100644 --- a/rmcs_ws/src/rmcs_bringup/config/flight.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/flight.yaml @@ -71,7 +71,8 @@ gimbal_controller: ros__parameters: upper_limit: -0.39518 # -0.39518 rad ≈ -22.6° lower_limit: 0.7 # 0.7 rad ≈ 40.1° - # TODO: yaw limite + yaw_lower_limit: 0.1745 + yaw_upper_limit: 2.5708 yaw_angle_pid_controller: ros__parameters: @@ -115,6 +116,7 @@ heat_controller: bullet_feeder_controller: ros__parameters: + debug_ignore_friction_ready: true # 调试用：不开摩擦轮也允许拨弹，复原时改为 false bullets_per_feeder_turn: 8.0 shot_frequency: 25.0 safe_shot_frequency: 10.0 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 7873188b..0857c178 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 @@ -35,11 +35,19 @@ class SimpleGimbalController register_output("/gimbal/yaw/control_angle_error", yaw_angle_error_, nan_); register_output("/gimbal/pitch/control_angle_error", pitch_angle_error_, nan_); + + rclcpp::Parameter yaw_upper, yaw_lower; + if (get_parameter("yaw_upper_limit", yaw_upper) + && get_parameter("yaw_lower_limit", yaw_lower)) { + two_axis_gimbal_solver.enable_yaw_limit( + *this, yaw_upper.as_double(), yaw_lower.as_double()); + } } void update() override { auto angle_error = calculate_angle_error(); *yaw_angle_error_ = angle_error.yaw_angle_error; + *pitch_angle_error_ = angle_error.pitch_angle_error; } 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 c1fc772f..711abb21 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,7 +1,10 @@ #pragma once +#include #include #include +#include +#include #include #include @@ -32,6 +35,13 @@ class TwoAxisGimbalSolver { component.register_input("/tf", tf_); } + void enable_yaw_limit( + rmcs_executor::Component& component, double yaw_upper_limit, double yaw_lower_limit) { + yaw_cw_max_ = yaw_upper_limit; + yaw_cw_min_ = yaw_lower_limit; + component.register_input("/gimbal/yaw/angle", gimbal_yaw_angle_); + } + class SetDisabled : public Operation { PitchLink::DirectionVector update(TwoAxisGimbalSolver& super) const override { super.control_enabled_ = false; @@ -113,6 +123,8 @@ class TwoAxisGimbalSolver { if (!control_enabled_) return {nan_, nan_}; + clamp_yaw_limit(control_direction_yaw_link); + 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); @@ -188,6 +200,29 @@ class TwoAxisGimbalSolver { *control_direction << lower_limit_.x() * projection, lower_limit_.y(); } + void clamp_yaw_limit(YawLink::DirectionVector& control_direction) { + if (!yaw_cw_min_.has_value()) + return; + + constexpr double two_pi = 2 * std::numbers::pi; + double cw = std::fmod(two_pi - *gimbal_yaw_angle_, two_pi); + if (cw < 0) + cw += two_pi; + + const auto& [x, y, z] = *control_direction; + const double err = std::atan2(y, x); + + const double target_cw = cw - err; + const double clamped_cw = std::clamp(target_cw, *yaw_cw_min_, *yaw_cw_max_); + if (clamped_cw == target_cw) + return; + + // delta = err_new - err = (cw - clamped_cw) - err + const double delta = (cw - clamped_cw) - err; + const double c = std::cos(delta), s = std::sin(delta); + *control_direction << c * x - s * y, s * x + c * y, z; + } + static AngleError calculate_control_errors( const YawLink::DirectionVector& control_direction, const Eigen::Vector2d& pitch) { const auto& [x, y, z] = *control_direction; @@ -209,6 +244,9 @@ class TwoAxisGimbalSolver { rmcs_executor::Component::InputInterface gimbal_pitch_angle_; rmcs_executor::Component::InputInterface tf_; + std::optional yaw_cw_min_, yaw_cw_max_; + rmcs_executor::Component::InputInterface gimbal_yaw_angle_; + OdomImu::DirectionVector yaw_axis_filtered_{Eigen::Vector3d::UnitZ()}; bool control_enabled_ = false; diff --git a/rmcs_ws/src/rmcs_core/src/controller/shooting/bullet_feeder_controller_17mm.cpp b/rmcs_ws/src/rmcs_core/src/controller/shooting/bullet_feeder_controller_17mm.cpp index 950fcdc7..69640700 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/shooting/bullet_feeder_controller_17mm.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/shooting/bullet_feeder_controller_17mm.cpp @@ -45,6 +45,12 @@ class BulletFeederController17mm single_shot_max_stop_delay_ = static_cast( std::round(1000.0 * get_parameter("single_shot_max_stop_delay").as_double())); + // Debug option: allow bullet feeding without friction wheels spinning. + debug_ignore_friction_ready_ = get_parameter_or("debug_ignore_friction_ready", false); + if (debug_ignore_friction_ready_) + RCLCPP_WARN( + logger_, "Friction-ready check bypassed: feeder will spin without friction wheels!"); + register_input("/gimbal/friction_ready", friction_ready_); register_input("/gimbal/bullet_fired", bullet_fired_); register_input( @@ -105,7 +111,8 @@ class BulletFeederController17mm if (*bullet_fired_) single_shot_stop_counter_ = 0; - if (*friction_ready_) { + const bool friction_ready = *friction_ready_ || debug_ignore_friction_ready_; + if (friction_ready) { if (shoot_mode == ShootMode::AUTOMATIC) { bool manual_triggered = mouse.left || switch_left == Switch::DOWN; bool auto_aim_triggered = mouse.right && *auto_aim_should_shoot_; @@ -128,7 +135,8 @@ class BulletFeederController17mm } } - if (!*friction_ready_ || switch_right == Switch::DOWN) + if ((!*friction_ready_ && !debug_ignore_friction_ready_) + || switch_right == Switch::DOWN) update_bullet_feeder_velocity(0, false); } @@ -216,6 +224,8 @@ class BulletFeederController17mm int single_shot_max_stop_delay_, single_shot_stop_counter_ = 0; + bool debug_ignore_friction_ready_ = false; + InputInterface friction_ready_; InputInterface bullet_fired_; InputInterface control_bullet_allowance_limited_by_heat_; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp b/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp index bfbedabb..58b8c5fe 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp @@ -62,7 +62,6 @@ class Flight device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(1.0)); gimbal_bullet_feeder_.configure( device::DjiMotor::Config{device::DjiMotor::Type::kM2006} - .set_reversed() .enable_multi_turn_angle()); register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_); From 6a1a42f2167edb77443d54088523d83feda68f72 Mon Sep 17 00:00:00 2001 From: Zihan Qin Date: Tue, 9 Jun 2026 22:20:27 +0800 Subject: [PATCH 22/30] feat: Integrate steering-hero support (#80) Add support for steering-hero and steering-hero-little, including hardware drivers, chassis control, gimbal control, shooting control, Hero UI integration, corresponding bringup configs, and plugin registration. Also correct LK motor torque constants: - kMG5010Ei10: 0.90909 -> 0.1 - kMG6012Ei8: 1.09 -> 1.09 / 8.0 To keep mainline vehicle control output unchanged, also retune: - omni-infantry yaw velocity PID parameters - sentry bottom yaw velocity PID / viscous FF parameters Also remove the unmaintained mecanum-hero config. Known issues (not blocking this merge): - The Hero UI shooter condition / state_word path is still leftover debug wiring and is not connected to the current active runtime path - HeroFrictionWheelController still preserves the historical friction-wheel index convention; for now this is only documented with a TODO and should later be replaced by an explicit first-stage mapping - steering-hero-little still has a mismatch between PlayerViewer limit parameters and control logic, which requires follow-up calibration - HeroChassisPowerController inherits the existing ChassisPowerController risk of reading uninitialized members / propagating non-finite values; this is a pre-existing shared issue, not newly introduced by this merge - steering-hero-little uses different vehicle_radius values in steering_wheel_status and steering_wheel_controller - HeroFrictionWheelController does not clear its jam fault counter after recovery, and Ctrl+F currently triggers a double profile toggle - PutterController does not reset putter_timeout_count_ during normal stage transitions - ChassisClimberController repeatedly resets back_climber_recover_count during auto-climb - The Hero UI Ctrl+E bottom-yaw tracking toggle is currently level-triggered, so holding the keys causes repeated flipping Co-authored-by: floatpigeon Co-authored-by: dwx5 <1591215786@qq.com> Co-authored-by: zhzy-star <2807406212@qq.com> --- rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/shape.hpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/shape.hpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/shape.hpp index ea064f50..1ceae35e 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/shape.hpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/shape.hpp @@ -724,10 +724,8 @@ class Float : public Integer { class Text : public Shape { public: - Text() { - value_ = nullptr; - set_text_shape(); - }; + Text() + : Shape(true) {} Text( Color color, uint16_t font_size, uint16_t width, uint16_t x, uint16_t y, const char* value, bool visible = true) From fa868402a21b87e7fe1b6727d34cfca2650a465d Mon Sep 17 00:00:00 2001 From: noskillzheng <3515964992@qq.com> Date: Sun, 5 Jul 2026 08:03:14 +0800 Subject: [PATCH 23/30] fix:migrate can data transmit and receive from Cboard to rmcsboardlite --- rmcs_ws/src/rmcs_auto_aim_v2 | 2 +- rmcs_ws/src/rmcs_bringup/config/flight.yaml | 2 +- .../src/rmcs_bringup/launch/rmcs.launch.py | 7 -- rmcs_ws/src/rmcs_core/src/hardware/flight.cpp | 71 +++++++++++++------ .../rmcs_core/src/hardware/omni_infantry.cpp | 2 +- 5 files changed, 52 insertions(+), 32 deletions(-) diff --git a/rmcs_ws/src/rmcs_auto_aim_v2 b/rmcs_ws/src/rmcs_auto_aim_v2 index f3bae295..02a130b7 160000 --- a/rmcs_ws/src/rmcs_auto_aim_v2 +++ b/rmcs_ws/src/rmcs_auto_aim_v2 @@ -1 +1 @@ -Subproject commit f3bae2952106975478809f6b21fabf7dc36f2668 +Subproject commit 02a130b74692922ea76e6c81ea67f4877ae5434d diff --git a/rmcs_ws/src/rmcs_bringup/config/flight.yaml b/rmcs_ws/src/rmcs_bringup/config/flight.yaml index aaadd145..04235226 100644 --- a/rmcs_ws/src/rmcs_bringup/config/flight.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/flight.yaml @@ -59,7 +59,7 @@ tf_broadcaster: flight_hardware: ros__parameters: - board_serial: "d4-9d44" + board_serial: "AF-7C58-5458-E731-9F74-1F9C-CAFD-30AF-9C09" yaw_motor_zero_point: 11720 pitch_motor_zero_point: 18578 diff --git a/rmcs_ws/src/rmcs_bringup/launch/rmcs.launch.py b/rmcs_ws/src/rmcs_bringup/launch/rmcs.launch.py index adcafe2a..19fde5f1 100644 --- a/rmcs_ws/src/rmcs_bringup/launch/rmcs.launch.py +++ b/rmcs_ws/src/rmcs_bringup/launch/rmcs.launch.py @@ -59,13 +59,6 @@ def visit( emulate_tty=True, ) ) - entities.append( - IncludeLaunchDescription( - PythonLaunchDescriptionSource([ - FindPackageShare('rmcs_auto_aim_v2'), '/launch.py' - ]) - ) - ) return entities diff --git a/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp b/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp index 58b8c5fe..db13fa76 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp @@ -5,7 +5,6 @@ #include #include -#include #include #include #include @@ -20,21 +19,23 @@ #include #include "hardware/device/bmi088.hpp" + #include "hardware/device/can_packet.hpp" #include "hardware/device/dji_motor.hpp" #include "hardware/device/dr16.hpp" #include "hardware/device/lk_motor.hpp" +#include "librmcs/agent/rmcs_board_lite.hpp" namespace rmcs_core::hardware { class Flight : public rmcs_executor::Component , public rclcpp::Node - , private librmcs::agent::CBoard { + , private librmcs::agent::RmcsBoardLite { public: Flight() : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} - , librmcs::agent::CBoard{get_parameter("board_serial").as_string()} + , librmcs::agent::RmcsBoardLite{get_parameter("board_serial").as_string()} , logger_(get_logger()) , command_component_( create_partner_component(get_component_name() + "_command", *this)) @@ -69,7 +70,7 @@ class Flight register_output("/tf", tf_); bmi088_.set_coordinate_mapping( - [](double x, double y, double z) { return std::make_tuple(-x, z, y); }); + [](double x, double y, double z) { return std::make_tuple(y, z, x); }); using namespace rmcs_description; // NOLINT(google-build-using-namespace) constexpr double rotor_distance_x = 1.128; @@ -105,7 +106,7 @@ class Flight [&buffer](std::byte byte) noexcept { *buffer++ = byte; }, size); }; referee_serial_->write = [this](const std::byte* buffer, size_t size) { - start_transmit().uart2_transmit( + start_transmit().uart1_transmit( {.uart_data = std::span{buffer, size}}); return size; }; @@ -127,20 +128,32 @@ class Flight void command_update() { auto builder = start_transmit(); builder - .can2_transmit( - {.can_id = 0x141, - .can_data = gimbal_yaw_motor_.generate_torque_command().as_bytes()}) - .can2_transmit( - {.can_id = 0x142, .can_data = gimbal_pitch_motor_.generate_command().as_bytes()}) + .can0_transmit( + {.can_id = 0x200, + .can_data = + device::CanPacket8{ + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + gimbal_left_friction_.generate_command(), + gimbal_right_friction_.generate_command(), + } + .as_bytes()}) .can1_transmit( {.can_id = 0x200, .can_data = device::CanPacket8{ gimbal_bullet_feeder_.generate_command(), device::CanPacket8::PaddingQuarter{}, - gimbal_left_friction_.generate_command(), - gimbal_right_friction_.generate_command()} - .as_bytes()}); + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes()}) + .can2_transmit( + {.can_id = 0x141, + .can_data = gimbal_yaw_motor_.generate_torque_command().as_bytes()}) + .can3_transmit( + {.can_id = 0x142, .can_data = gimbal_pitch_motor_.generate_command().as_bytes()}); + } private: @@ -188,33 +201,47 @@ class Flight }; protected: - void can1_receive_callback(const librmcs::data::CanDataView& data) override { + void can0_receive_callback(const librmcs::data::CanDataView& data) override { if (data.is_extended_can_id || data.is_remote_transmission || data.can_data.size() < 8) [[unlikely]] return; - if (data.can_id == 0x201) { - gimbal_bullet_feeder_.store_status(data.can_data); - } else if (data.can_id == 0x203) { + if (data.can_id == 0x203) { gimbal_left_friction_.store_status(data.can_data); - } else if (data.can_id == 0x204) { + }else if(data.can_id == 0x204) { gimbal_right_friction_.store_status(data.can_data); } } + void can1_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission || data.can_data.size() < 8) + [[unlikely]] + return; + + if (data.can_id == 0x201) { + gimbal_bullet_feeder_.store_status(data.can_data); + } + } void can2_receive_callback(const librmcs::data::CanDataView& data) override { if (data.is_extended_can_id || data.is_remote_transmission || data.can_data.size() < 8) [[unlikely]] return; - if (data.can_id == 0x142) - gimbal_pitch_motor_.store_status(data.can_data); - else if (data.can_id == 0x141) { + if (data.can_id == 0x141){ gimbal_yaw_motor_.store_status(data.can_data); } } + void can3_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission || data.can_data.size() < 8) + [[unlikely]] + return; + + if (data.can_id == 0x142){ + gimbal_pitch_motor_.store_status(data.can_data); + } + } - void uart2_receive_callback(const librmcs::data::UartDataView& data) override { + void uart1_receive_callback(const librmcs::data::UartDataView& data) override { const std::byte* ptr = data.uart_data.data(); referee_ring_buffer_receive_.emplace_back_n( [&ptr](std::byte* storage) noexcept { new (storage) std::byte{*ptr++}; }, diff --git a/rmcs_ws/src/rmcs_core/src/hardware/omni_infantry.cpp b/rmcs_ws/src/rmcs_core/src/hardware/omni_infantry.cpp index 84b72077..472b05a3 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/omni_infantry.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/omni_infantry.cpp @@ -249,7 +249,7 @@ class OmniInfantry motor.store_status(data.can_data); } else if (can_id == 0x202) { auto& motor = chassis_wheel_motors_[1]; - motor.store_status(data.can_data); + motor.store_status(data.can_data); } else if (can_id == 0x203) { auto& motor = chassis_wheel_motors_[2]; motor.store_status(data.can_data); From bd36817527f49e984678a9e8d8265c54401c2136 Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Sun, 5 Jul 2026 19:42:35 +0800 Subject: [PATCH 24/30] wip: Adapt autoaim v2 --- .gitmodules | 6 ++-- rmcs_ws/src/rmcs_auto_aim_v2 | 2 +- rmcs_ws/src/rmcs_bringup/config/flight.yaml | 18 +++++----- .../src/rmcs_bringup/launch/rmcs.launch.py | 16 ++++----- rmcs_ws/src/rmcs_core/src/hardware/flight.cpp | 35 +++++++++++++------ 5 files changed, 46 insertions(+), 31 deletions(-) diff --git a/.gitmodules b/.gitmodules index acf743ec..b7cf4791 100644 --- a/.gitmodules +++ b/.gitmodules @@ -3,10 +3,10 @@ url = https://github.com/qzhhhi/FastTF.git [submodule "rmcs_ws/src/odin_ros_driver"] path = rmcs_ws/src/odin_ros_driver - url = git@github.com:Alliance-Algorithm/odin_ros_driver.git + url = https://github.com/Alliance-Algorithm/odin_ros_driver.git [submodule "rmcs_ws/src/hikcamera"] path = rmcs_ws/src/hikcamera - url = git@github.com:Alliance-Algorithm/ros2-hikcamera.git + url = https://github.com/Alliance-Algorithm/ros2-hikcamera.git [submodule "rmcs_ws/src/rmcs_auto_aim_v2"] path = rmcs_ws/src/rmcs_auto_aim_v2 - url = git@github.com:Alliance-Algorithm/rmcs_auto_aim_v2.git + url = https://github.com/Alliance-Algorithm/rmcs_auto_aim_v2.git diff --git a/rmcs_ws/src/rmcs_auto_aim_v2 b/rmcs_ws/src/rmcs_auto_aim_v2 index 02a130b7..c2da258f 160000 --- a/rmcs_ws/src/rmcs_auto_aim_v2 +++ b/rmcs_ws/src/rmcs_auto_aim_v2 @@ -1 +1 @@ -Subproject commit 02a130b74692922ea76e6c81ea67f4877ae5434d +Subproject commit c2da258f531e927c3a1f22dd3d2861674bc14e6f diff --git a/rmcs_ws/src/rmcs_bringup/config/flight.yaml b/rmcs_ws/src/rmcs_bringup/config/flight.yaml index 04235226..3d9675f8 100644 --- a/rmcs_ws/src/rmcs_bringup/config/flight.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/flight.yaml @@ -3,7 +3,7 @@ rmcs_executor: update_rate: 1000.0 components: - rmcs_core::hardware::Flight -> flight_hardware - + - 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 @@ -22,10 +22,10 @@ rmcs_executor: - rmcs_core::referee::command::interaction::Ui -> referee_ui - rmcs_core::referee::app::ui::Flight -> referee_ui_flight - - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster - - rmcs_core::broadcaster::TfBroadcaster -> tf_broadcaster + # - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster + # - rmcs_core::broadcaster::TfBroadcaster -> tf_broadcaster - - rmcs::AutoAimComponent -> auto_aim_component + - rmcs::AutoAimComponent odin_ros_driver: ros__parameters: @@ -69,10 +69,10 @@ referee_status: gimbal_controller: ros__parameters: - upper_limit: -0.39518 # -0.39518 rad ≈ -22.6° - lower_limit: 0.7 # 0.7 rad ≈ 40.1° - yaw_lower_limit: 0.1745 - yaw_upper_limit: 2.5708 + upper_limit: -0.39518 # -0.39518 rad ≈ -22.6° + lower_limit: 0.7 # 0.7 rad ≈ 40.1° + yaw_lower_limit: 0.1745 + yaw_upper_limit: 2.5708 yaw_angle_pid_controller: ros__parameters: @@ -116,7 +116,7 @@ heat_controller: bullet_feeder_controller: ros__parameters: - debug_ignore_friction_ready: true # 调试用：不开摩擦轮也允许拨弹，复原时改为 false + debug_ignore_friction_ready: true # 调试用：不开摩擦轮也允许拨弹，复原时改为 false bullets_per_feeder_turn: 8.0 shot_frequency: 25.0 safe_shot_frequency: 10.0 diff --git a/rmcs_ws/src/rmcs_bringup/launch/rmcs.launch.py b/rmcs_ws/src/rmcs_bringup/launch/rmcs.launch.py index 19fde5f1..bc23a13b 100644 --- a/rmcs_ws/src/rmcs_bringup/launch/rmcs.launch.py +++ b/rmcs_ws/src/rmcs_bringup/launch/rmcs.launch.py @@ -51,14 +51,14 @@ def visit( if is_automatic: pass - entities.append( - Node( - package="odin_ros_driver", - executable="tmux-launch.sh", - output="screen", - emulate_tty=True, - ) - ) + # entities.append( + # Node( + # package="odin_ros_driver", + # executable="tmux-launch.sh", + # output="screen", + # emulate_tty=True, + # ) + # ) return entities diff --git a/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp b/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp index db13fa76..a8eb2d13 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp @@ -34,7 +34,9 @@ class Flight , private librmcs::agent::RmcsBoardLite { 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::agent::RmcsBoardLite{get_parameter("board_serial").as_string()} , logger_(get_logger()) , command_component_( @@ -62,13 +64,15 @@ class Flight gimbal_right_friction_.configure( device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(1.0)); gimbal_bullet_feeder_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM2006} - .enable_multi_turn_angle()); + device::DjiMotor::Config{device::DjiMotor::Type::kM2006}.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_); + register_output("/auto_aim/camera_transform", camera_transform_); + register_output("/auto_aim/barrel_direction", barrel_direction_); + bmi088_.set_coordinate_mapping( [](double x, double y, double z) { return std::make_tuple(y, z, x); }); @@ -123,6 +127,15 @@ class Flight update_motors(); update_imu(); dr16_.update_status(); + + // 从 TF 树中查询相机位姿 + *camera_transform_ = + fast_tf::lookup_transform( + *tf_); + + // 从 TF 树中查询枪口方向 + *barrel_direction_ = *fast_tf::cast( + rmcs_description::PitchLink::DirectionVector{Eigen::Vector3d::UnitX()}, *tf_); } void command_update() { @@ -136,7 +149,7 @@ class Flight device::CanPacket8::PaddingQuarter{}, gimbal_left_friction_.generate_command(), gimbal_right_friction_.generate_command(), - } + } .as_bytes()}) .can1_transmit( {.can_id = 0x200, @@ -146,14 +159,13 @@ class Flight device::CanPacket8::PaddingQuarter{}, device::CanPacket8::PaddingQuarter{}, device::CanPacket8::PaddingQuarter{}, - } + } .as_bytes()}) .can2_transmit( {.can_id = 0x141, .can_data = gimbal_yaw_motor_.generate_torque_command().as_bytes()}) .can3_transmit( {.can_id = 0x142, .can_data = gimbal_pitch_motor_.generate_command().as_bytes()}); - } private: @@ -208,7 +220,7 @@ class Flight if (data.can_id == 0x203) { gimbal_left_friction_.store_status(data.can_data); - }else if(data.can_id == 0x204) { + } else if (data.can_id == 0x204) { gimbal_right_friction_.store_status(data.can_data); } } @@ -219,7 +231,7 @@ class Flight if (data.can_id == 0x201) { gimbal_bullet_feeder_.store_status(data.can_data); - } + } } void can2_receive_callback(const librmcs::data::CanDataView& data) override { @@ -227,7 +239,7 @@ class Flight [[unlikely]] return; - if (data.can_id == 0x141){ + if (data.can_id == 0x141) { gimbal_yaw_motor_.store_status(data.can_data); } } @@ -236,7 +248,7 @@ class Flight [[unlikely]] return; - if (data.can_id == 0x142){ + if (data.can_id == 0x142) { gimbal_pitch_motor_.store_status(data.can_data); } } @@ -279,6 +291,9 @@ class Flight OutputInterface tf_; OutputInterface referee_serial_; + OutputInterface camera_transform_; + OutputInterface barrel_direction_; + rmcs_utility::RingBuffer referee_ring_buffer_receive_{256}; }; From 77492f1ec90bb66af3ded5f0df0b3d873891e6f0 Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Sun, 5 Jul 2026 20:11:19 +0800 Subject: [PATCH 25/30] wip: remove unused transform and clean up code --- rmcs_ws/src/rmcs_core/src/hardware/flight.cpp | 68 +++++++------------ 1 file changed, 23 insertions(+), 45 deletions(-) diff --git a/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp b/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp index a8eb2d13..36bc1417 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp @@ -6,11 +6,7 @@ #include #include -#include -#include #include -#include -#include #include #include #include @@ -19,7 +15,6 @@ #include #include "hardware/device/bmi088.hpp" - #include "hardware/device/can_packet.hpp" #include "hardware/device/dji_motor.hpp" #include "hardware/device/dr16.hpp" @@ -74,30 +69,7 @@ class Flight register_output("/auto_aim/barrel_direction", barrel_direction_); bmi088_.set_coordinate_mapping( - [](double x, double y, double z) { return std::make_tuple(y, z, x); }); - - using namespace rmcs_description; // NOLINT(google-build-using-namespace) - constexpr double rotor_distance_x = 1.128; - constexpr double rotor_distance_y = 1.128; - - 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}); - - // GimbalCenterLink 原点等效于 YawLink 和 PitchLink 的交点 - constexpr double gimbal_center_z = -0.27812; - tf_->set_transform( - Eigen::Translation3d{0.0, 0.0, gimbal_center_z}); - constexpr auto camera_postion_x = 0.10238; - constexpr auto camera_postion_z = 0.05286; - - tf_->set_transform( - Eigen::Translation3d{camera_postion_x, 0.0, camera_postion_z}); + [](double x, double y, double z) { return std::tuple{y, z, x}; }); gimbal_calibrate_subscription_ = create_subscription( "/gimbal/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { @@ -114,6 +86,13 @@ class Flight {.uart_data = std::span{buffer, size}}); return size; }; + + using namespace rmcs_description; + + constexpr auto kCameraPostionX = 0.10238; + constexpr auto kCameraPostionZ = 0.05286; + tf_->set_transform( + Eigen::Translation3d{kCameraPostionX, 0.0, kCameraPostionZ}); } Flight(const Flight&) = delete; @@ -128,14 +107,10 @@ class Flight update_imu(); dr16_.update_status(); - // 从 TF 树中查询相机位姿 - *camera_transform_ = - fast_tf::lookup_transform( - *tf_); - - // 从 TF 树中查询枪口方向 - *barrel_direction_ = *fast_tf::cast( - rmcs_description::PitchLink::DirectionVector{Eigen::Vector3d::UnitX()}, *tf_); + using namespace rmcs_description; + *camera_transform_ = fast_tf::lookup_transform(*tf_); + *barrel_direction_ = + *fast_tf::cast(PitchLink::DirectionVector{Eigen::Vector3d::UnitX()}, *tf_); } void command_update() { @@ -170,23 +145,26 @@ class Flight private: void update_motors() { - using namespace rmcs_description; // NOLINT(google-build-using-namespace) + gimbal_bullet_feeder_.update_status(); + gimbal_left_friction_.update_status(); + gimbal_right_friction_.update_status(); + + 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() { + using namespace rmcs_description; + bmi088_.update_status(); - Eigen::Quaterniond gimbal_imu_pose{bmi088_.q0(), bmi088_.q1(), bmi088_.q2(), bmi088_.q3()}; - tf_->set_transform( - gimbal_imu_pose.conjugate()); + const auto gimbal_imu_pose = + Eigen::Quaterniond{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(); From 4e6b2efda500e8a71c9e6813d7c7c4dfc53f0e34 Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Sun, 5 Jul 2026 20:20:17 +0800 Subject: [PATCH 26/30] chore: clean up cmake file --- rmcs_ws/src/rmcs_core/CMakeLists.txt | 17 +++++++++-------- 1 file changed, 9 insertions(+), 8 deletions(-) diff --git a/rmcs_ws/src/rmcs_core/CMakeLists.txt b/rmcs_ws/src/rmcs_core/CMakeLists.txt index 38d75ca7..81bf0b61 100644 --- a/rmcs_ws/src/rmcs_core/CMakeLists.txt +++ b/rmcs_ws/src/rmcs_core/CMakeLists.txt @@ -13,24 +13,25 @@ endif() find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() + include(FetchContent) set(BUILD_STATIC_LIBRMCS ON CACHE BOOL "Build static librmcs SDK" FORCE) FetchContent_Declare( - librmcs - URL https://github.com/Alliance-Algorithm/librmcs/releases/download/v3.2.0/librmcs-sdk-src-3.2.0.zip - URL_HASH SHA256=f81c3af7fbcf35727a8a7586200db8e9bf668ee0f448529de4bfd3eb7c36ed6f - DOWNLOAD_EXTRACT_TIMESTAMP TRUE + librmcs + URL https://github.com/Alliance-Algorithm/librmcs/releases/download/v3.2.0/librmcs-sdk-src-3.2.0.zip + URL_HASH SHA256=f81c3af7fbcf35727a8a7586200db8e9bf668ee0f448529de4bfd3eb7c36ed6f + DOWNLOAD_EXTRACT_TIMESTAMP TRUE ) FetchContent_MakeAvailable(librmcs) file(GLOB_RECURSE PROJECT_SOURCE CONFIGURE_DEPENDS - ${PROJECT_SOURCE_DIR}/src/*.cpp - ${PROJECT_SOURCE_DIR}/src/*.c + ${PROJECT_SOURCE_DIR}/src/*.cpp + ${PROJECT_SOURCE_DIR}/src/*.c ) ament_auto_add_library( - ${PROJECT_NAME} SHARED - ${PROJECT_SOURCE} + ${PROJECT_NAME} SHARED + ${PROJECT_SOURCE} ) include_directories(${PROJECT_SOURCE_DIR}/include) From 915c3e4345e6b33994fe1923fd8a597c4ace71e9 Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Sun, 5 Jul 2026 20:38:35 +0800 Subject: [PATCH 27/30] chore: clean up hardware --- rmcs_ws/src/rmcs_core/src/hardware/flight.cpp | 107 ++++++++---------- 1 file changed, 50 insertions(+), 57 deletions(-) diff --git a/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp b/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp index 36bc1417..d24ea80e 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp @@ -12,7 +12,7 @@ #include #include #include -#include +#include #include "hardware/device/bmi088.hpp" #include "hardware/device/can_packet.hpp" @@ -32,17 +32,7 @@ class Flight : Node{ get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} - , librmcs::agent::RmcsBoardLite{get_parameter("board_serial").as_string()} - , logger_(get_logger()) - , command_component_( - create_partner_component(get_component_name() + "_command", *this)) - , gimbal_yaw_motor_(*this, *command_component_, "/gimbal/yaw") - , gimbal_pitch_motor_(*this, *command_component_, "/gimbal/pitch") - , gimbal_left_friction_(*this, *command_component_, "/gimbal/left_friction") - , gimbal_right_friction_(*this, *command_component_, "/gimbal/right_friction") - , gimbal_bullet_feeder_(*this, *command_component_, "/gimbal/bullet_feeder") - , dr16_(*this) - , bmi088_(1000.0, 0.2, 0.00) { + , RmcsBoardLite{get_parameter("board_serial").as_string()} { gimbal_yaw_motor_.configure( device::LkMotor::Config{device::LkMotor::Type::kMHF7015} @@ -61,22 +51,17 @@ class Flight gimbal_bullet_feeder_.configure( device::DjiMotor::Config{device::DjiMotor::Type::kM2006}.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_); - - register_output("/auto_aim/camera_transform", camera_transform_); - register_output("/auto_aim/barrel_direction", barrel_direction_); - bmi088_.set_coordinate_mapping( [](double x, double y, double z) { return std::tuple{y, z, x}; }); - gimbal_calibrate_subscription_ = create_subscription( - "/gimbal/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { - gimbal_calibrate_subscription_callback(std::move(msg)); + status_service_ = create_service( + "/rmcs/service/robot_status", + [this]( + const std_srvs::srv::Trigger::Request::SharedPtr&, + const std_srvs::srv::Trigger::Response::SharedPtr& response) { + status_service_callback(response); }); - register_output("/referee/serial", referee_serial_); referee_serial_->read = [this](std::byte* buffer, size_t size) { return referee_ring_buffer_receive_.pop_front_n( [&buffer](std::byte byte) noexcept { *buffer++ = byte; }, size); @@ -93,14 +78,16 @@ class Flight constexpr auto kCameraPostionZ = 0.05286; tf_->set_transform( Eigen::Translation3d{kCameraPostionX, 0.0, kCameraPostionZ}); - } - Flight(const Flight&) = delete; - Flight& operator=(const Flight&) = delete; - Flight(Flight&&) = delete; - Flight& operator=(Flight&&) = delete; + register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_); + register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_imu_); + register_output("/tf", tf_); - ~Flight() override = default; + register_output("/auto_aim/camera_transform", camera_transform_); + register_output("/auto_aim/barrel_direction", barrel_direction_); + + register_output("/referee/serial", referee_serial_); + } void update() override { update_motors(); @@ -170,25 +157,20 @@ class Flight *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()); - } + void + status_service_callback(const std::shared_ptr& response) { + response->success = true; - class FlightCommand : public rmcs_executor::Component { - public: - explicit FlightCommand(Flight& flight) - : flight_(flight) {} + auto feedback_message = std::ostringstream{}; + auto text = [&](std::format_string format, Args&&... args) { + std::println(feedback_message, format, std::forward(args)...); + }; - void update() override { flight_.command_update(); } + text(" yaw_motor_zero_point: {}", gimbal_yaw_motor_.last_raw_angle()); + text(" pitch_motor_zero_point: {}", gimbal_pitch_motor_.last_raw_angle()); - private: - Flight& flight_; - }; + response->message = feedback_message.str(); + } protected: void can0_receive_callback(const librmcs::data::CanDataView& data) override { @@ -251,18 +233,29 @@ class Flight } private: - rclcpp::Logger logger_; - std::shared_ptr command_component_; - 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_; + class FlightCommand : public rmcs_executor::Component { + public: + explicit FlightCommand(Flight& flight) + : flight_(flight) {} + + void update() override { flight_.command_update(); } + + private: + Flight& flight_; + }; + std::shared_ptr command_component_{ + create_partner_component(get_component_name() + "_command", *this), + }; + std::shared_ptr> status_service_; + + device::LkMotor gimbal_yaw_motor_{*this, *command_component_, "/gimbal/yaw"}; + device::LkMotor gimbal_pitch_motor_{*this, *command_component_, "/gimbal/pitch"}; + device::DjiMotor gimbal_left_friction_{*this, *command_component_, "/gimbal/left_friction"}; + device::DjiMotor gimbal_right_friction_{*this, *command_component_, "/gimbal/right_friction"}; + device::DjiMotor gimbal_bullet_feeder_{*this, *command_component_, "/gimbal/bullet_feeder"}; + + device::Dr16 dr16_{*this}; + device::Bmi088 bmi088_{1000.0, 0.2, 0.00}; OutputInterface gimbal_yaw_velocity_imu_; OutputInterface gimbal_pitch_velocity_imu_; From dfe4f35b6077bf8b5ccd836924c63377ae17ee49 Mon Sep 17 00:00:00 2001 From: zlq040222 <1542498005@qq.com> Date: Mon, 6 Jul 2026 02:27:05 +0800 Subject: [PATCH 28/30] fix(flight): fix initialization order and add tf2_ros dependency --- rmcs_ws/src/rmcs_core/package.xml | 1 + rmcs_ws/src/rmcs_core/src/hardware/flight.cpp | 35 +++++++++---------- 2 files changed, 18 insertions(+), 18 deletions(-) diff --git a/rmcs_ws/src/rmcs_core/package.xml b/rmcs_ws/src/rmcs_core/package.xml index 9eb6b089..e42d0669 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 + tf2_ros ament_lint_auto ament_lint_common diff --git a/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp b/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp index d24ea80e..c35bb8df 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp @@ -54,24 +54,6 @@ class Flight bmi088_.set_coordinate_mapping( [](double x, double y, double z) { return std::tuple{y, z, x}; }); - status_service_ = create_service( - "/rmcs/service/robot_status", - [this]( - const std_srvs::srv::Trigger::Request::SharedPtr&, - const std_srvs::srv::Trigger::Response::SharedPtr& response) { - status_service_callback(response); - }); - - referee_serial_->read = [this](std::byte* buffer, size_t size) { - return referee_ring_buffer_receive_.pop_front_n( - [&buffer](std::byte byte) noexcept { *buffer++ = byte; }, size); - }; - referee_serial_->write = [this](const std::byte* buffer, size_t size) { - start_transmit().uart1_transmit( - {.uart_data = std::span{buffer, size}}); - return size; - }; - using namespace rmcs_description; constexpr auto kCameraPostionX = 0.10238; @@ -87,6 +69,23 @@ class Flight register_output("/auto_aim/barrel_direction", barrel_direction_); register_output("/referee/serial", referee_serial_); + referee_serial_->read = [this](std::byte* buffer, size_t size) { + return referee_ring_buffer_receive_.pop_front_n( + [&buffer](std::byte byte) noexcept { *buffer++ = byte; }, size); + }; + referee_serial_->write = [this](const std::byte* buffer, size_t size) { + start_transmit().uart1_transmit( + {.uart_data = std::span{buffer, size}}); + return size; + }; + + status_service_ = create_service( + "/rmcs/service/robot_status", + [this]( + const std_srvs::srv::Trigger::Request::SharedPtr&, + const std_srvs::srv::Trigger::Response::SharedPtr& response) { + status_service_callback(response); + }); } void update() override { From 1820c84ad2d8cb8472679476ba62ec29632bf66e Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Mon, 6 Jul 2026 17:04:25 +0800 Subject: [PATCH 29/30] wip: clean up code and prepared to merge --- .gitmodules | 10 +--- rmcs_ws/src/hikcamera | 1 - rmcs_ws/src/odin_ros_driver | 1 - rmcs_ws/src/rmcs_auto_aim_v2 | 1 - rmcs_ws/src/rmcs_bringup/config/flight.yaml | 3 +- .../src/rmcs_bringup/launch/rmcs.launch.py | 19 ++---- rmcs_ws/src/rmcs_bringup/package.xml | 4 +- rmcs_ws/src/rmcs_core/package.xml | 4 +- rmcs_ws/src/rmcs_core/plugins.xml | 17 +++--- .../gimbal/simple_gimbal_controller.cpp | 18 +++--- .../gimbal/two_axis_gimbal_solver.hpp | 14 ++--- .../bullet_feeder_controller_17mm.cpp | 60 +++++-------------- .../rmcs_core/src/hardware/omni_infantry.cpp | 2 +- .../rmcs_core/src/referee/app/ui/flight.cpp | 18 +++--- .../src/referee/app/ui/shape/shape.hpp | 2 - .../src/referee/app/ui/widget/status_ring.hpp | 40 ++++--------- .../src/referee/command/interaction/ui.cpp | 36 +++-------- rmcs_ws/src/rmcs_core/src/referee/status.cpp | 7 ++- 18 files changed, 83 insertions(+), 174 deletions(-) delete mode 160000 rmcs_ws/src/hikcamera delete mode 160000 rmcs_ws/src/odin_ros_driver delete mode 160000 rmcs_ws/src/rmcs_auto_aim_v2 diff --git a/.gitmodules b/.gitmodules index b7cf4791..68f1d09b 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,12 +1,4 @@ [submodule "rmcs_ws/src/fast_tf"] path = rmcs_ws/src/fast_tf url = https://github.com/qzhhhi/FastTF.git -[submodule "rmcs_ws/src/odin_ros_driver"] - path = rmcs_ws/src/odin_ros_driver - url = https://github.com/Alliance-Algorithm/odin_ros_driver.git -[submodule "rmcs_ws/src/hikcamera"] - path = rmcs_ws/src/hikcamera - url = https://github.com/Alliance-Algorithm/ros2-hikcamera.git -[submodule "rmcs_ws/src/rmcs_auto_aim_v2"] - path = rmcs_ws/src/rmcs_auto_aim_v2 - url = https://github.com/Alliance-Algorithm/rmcs_auto_aim_v2.git + diff --git a/rmcs_ws/src/hikcamera b/rmcs_ws/src/hikcamera deleted file mode 160000 index f0077f03..00000000 --- a/rmcs_ws/src/hikcamera +++ /dev/null @@ -1 +0,0 @@ -Subproject commit f0077f034800bcd0dde4fffeff270b733772a57e diff --git a/rmcs_ws/src/odin_ros_driver b/rmcs_ws/src/odin_ros_driver deleted file mode 160000 index 5cb37012..00000000 --- a/rmcs_ws/src/odin_ros_driver +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 5cb37012a12686bb757923244eb0612285012c04 diff --git a/rmcs_ws/src/rmcs_auto_aim_v2 b/rmcs_ws/src/rmcs_auto_aim_v2 deleted file mode 160000 index c2da258f..00000000 --- a/rmcs_ws/src/rmcs_auto_aim_v2 +++ /dev/null @@ -1 +0,0 @@ -Subproject commit c2da258f531e927c3a1f22dd3d2861674bc14e6f diff --git a/rmcs_ws/src/rmcs_bringup/config/flight.yaml b/rmcs_ws/src/rmcs_bringup/config/flight.yaml index 3d9675f8..2291e12a 100644 --- a/rmcs_ws/src/rmcs_bringup/config/flight.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/flight.yaml @@ -116,9 +116,8 @@ heat_controller: bullet_feeder_controller: ros__parameters: - debug_ignore_friction_ready: true # 调试用：不开摩擦轮也允许拨弹，复原时改为 false bullets_per_feeder_turn: 8.0 - shot_frequency: 25.0 + shot_frequency: 20.0 safe_shot_frequency: 10.0 eject_frequency: 10.0 eject_time: 0.05 diff --git a/rmcs_ws/src/rmcs_bringup/launch/rmcs.launch.py b/rmcs_ws/src/rmcs_bringup/launch/rmcs.launch.py index bc23a13b..976cb9e7 100644 --- a/rmcs_ws/src/rmcs_bringup/launch/rmcs.launch.py +++ b/rmcs_ws/src/rmcs_bringup/launch/rmcs.launch.py @@ -6,8 +6,7 @@ LaunchDescription, LaunchDescriptionEntity, ) -from launch.actions import IncludeLaunchDescription, LogInfo -from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.actions import LogInfo from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node @@ -29,7 +28,10 @@ def visit( robot_name = robot_config entities.append( - LogInfo(msg=f"Starting RMCS on robot -> {robot_name}.yaml")) + LogInfo( + msg=f"Starting RMCS on robot '{robot_config}'{'(automatic)' if is_automatic else ''} -> {robot_name}.yaml" + ) + ) entities.append( Node( @@ -44,22 +46,13 @@ def visit( ], respawn=True, respawn_delay=1.0, - output="log", + output="log", # stdout and stderr are logged to launch log file and stderr to the screen. ) ) if is_automatic: pass - # entities.append( - # Node( - # package="odin_ros_driver", - # executable="tmux-launch.sh", - # output="screen", - # emulate_tty=True, - # ) - # ) - return entities diff --git a/rmcs_ws/src/rmcs_bringup/package.xml b/rmcs_ws/src/rmcs_bringup/package.xml index 8642966e..c64c661f 100644 --- a/rmcs_ws/src/rmcs_bringup/package.xml +++ b/rmcs_ws/src/rmcs_bringup/package.xml @@ -11,8 +11,6 @@ ament_cmake joint_state_broadcaster - mavros - odin_ros_driver robot_state_publisher rviz2 xacro @@ -23,4 +21,4 @@ ament_cmake - + \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/package.xml b/rmcs_ws/src/rmcs_core/package.xml index e42d0669..4312d334 100644 --- a/rmcs_ws/src/rmcs_core/package.xml +++ b/rmcs_ws/src/rmcs_core/package.xml @@ -10,16 +10,16 @@ ament_cmake rclcpp - nav_msgs std_msgs std_srvs pluginlib + tf2 + tf2_ros serial rmcs_utility rmcs_msgs rmcs_executor rmcs_description - tf2_ros 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 51fb30aa..1f46763e 100644 --- a/rmcs_ws/src/rmcs_core/plugins.xml +++ b/rmcs_ws/src/rmcs_core/plugins.xml @@ -1,7 +1,5 @@ - - - + @@ -9,6 +7,7 @@ + @@ -22,14 +21,14 @@ - + - + @@ -38,19 +37,23 @@ + + + + + + - - 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 0857c178..dfd36131 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 @@ -20,9 +20,11 @@ class SimpleGimbalController : Node( get_component_name(), 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()) { + , two_axis_gimbal_solver{ + *this, + get_parameter("upper_limit").as_double(), + get_parameter("lower_limit").as_double(), + } { register_input("/remote/joystick/left", joystick_left_); register_input("/remote/switch/right", switch_right_); @@ -36,18 +38,14 @@ class SimpleGimbalController register_output("/gimbal/yaw/control_angle_error", yaw_angle_error_, nan_); register_output("/gimbal/pitch/control_angle_error", pitch_angle_error_, nan_); - rclcpp::Parameter yaw_upper, yaw_lower; - if (get_parameter("yaw_upper_limit", yaw_upper) - && get_parameter("yaw_lower_limit", yaw_lower)) { - two_axis_gimbal_solver.enable_yaw_limit( - *this, yaw_upper.as_double(), yaw_lower.as_double()); - } + two_axis_gimbal_solver.enable_yaw_limit( + *this, get_parameter("yaw_upper_limit").as_double(), + get_parameter("yaw_lower_limit").as_double()); } void update() override { auto angle_error = calculate_angle_error(); *yaw_angle_error_ = angle_error.yaw_angle_error; - *pitch_angle_error_ = angle_error.pitch_angle_error; } 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 711abb21..03831969 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 @@ -4,7 +4,6 @@ #include #include #include -#include #include #include @@ -201,7 +200,7 @@ class TwoAxisGimbalSolver { } void clamp_yaw_limit(YawLink::DirectionVector& control_direction) { - if (!yaw_cw_min_.has_value()) + if (!gimbal_yaw_angle_.ready()) return; constexpr double two_pi = 2 * std::numbers::pi; @@ -210,12 +209,12 @@ class TwoAxisGimbalSolver { cw += two_pi; const auto& [x, y, z] = *control_direction; - const double err = std::atan2(y, x); + const double err = std::atan2(y, x); - const double target_cw = cw - err; - const double clamped_cw = std::clamp(target_cw, *yaw_cw_min_, *yaw_cw_max_); + const double target_cw = cw - err; + const double clamped_cw = std::clamp(target_cw, yaw_cw_min_, yaw_cw_max_); if (clamped_cw == target_cw) - return; + return; // delta = err_new - err = (cw - clamped_cw) - err const double delta = (cw - clamped_cw) - err; @@ -244,7 +243,8 @@ class TwoAxisGimbalSolver { rmcs_executor::Component::InputInterface gimbal_pitch_angle_; rmcs_executor::Component::InputInterface tf_; - std::optional yaw_cw_min_, yaw_cw_max_; + double yaw_cw_min_ = 0.; + double yaw_cw_max_ = 0.; rmcs_executor::Component::InputInterface gimbal_yaw_angle_; OdomImu::DirectionVector yaw_axis_filtered_{Eigen::Vector3d::UnitZ()}; diff --git a/rmcs_ws/src/rmcs_core/src/controller/shooting/bullet_feeder_controller_17mm.cpp b/rmcs_ws/src/rmcs_core/src/controller/shooting/bullet_feeder_controller_17mm.cpp index 69640700..4b845caa 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/shooting/bullet_feeder_controller_17mm.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/shooting/bullet_feeder_controller_17mm.cpp @@ -1,7 +1,3 @@ -#include - -#include - #include #include #include @@ -28,9 +24,6 @@ class BulletFeederController17mm bullet_feeder_working_velocity = bullet_feeder_angle_per_bullet * shot_frequency; double safe_shot_frequency = get_parameter("safe_shot_frequency").as_double(); bullet_feeder_safe_shot_velocity = bullet_feeder_angle_per_bullet * safe_shot_frequency; - constexpr double rotary_knob_shot_frequency = 8.0; - bullet_feeder_rotary_knob_velocity_ = - bullet_feeder_angle_per_bullet * rotary_knob_shot_frequency; double eject_frequency = get_parameter("eject_frequency").as_double(); bullet_feeder_eject_velocity_ = -bullet_feeder_angle_per_bullet * eject_frequency; @@ -45,12 +38,6 @@ class BulletFeederController17mm single_shot_max_stop_delay_ = static_cast( std::round(1000.0 * get_parameter("single_shot_max_stop_delay").as_double())); - // Debug option: allow bullet feeding without friction wheels spinning. - debug_ignore_friction_ready_ = get_parameter_or("debug_ignore_friction_ready", false); - if (debug_ignore_friction_ready_) - RCLCPP_WARN( - logger_, "Friction-ready check bypassed: feeder will spin without friction wheels!"); - register_input("/gimbal/friction_ready", friction_ready_); register_input("/gimbal/bullet_fired", bullet_fired_); register_input( @@ -61,9 +48,8 @@ class BulletFeederController17mm register_input("/remote/switch/left", switch_left_); register_input("/remote/mouse", mouse_); register_input("/remote/keyboard", keyboard_); - register_input("/remote/rotary_knob_switch", rotary_knob_switch_); - register_input("/auto_aim/should_shoot", auto_aim_should_shoot_, false); + register_input("/auto_aim/should_shoot", should_shoot_, false); register_input("/gimbal/bullet_feeder/velocity", bullet_feeder_velocity_); register_output( @@ -73,8 +59,8 @@ class BulletFeederController17mm } void before_updating() override { - if (!auto_aim_should_shoot_.ready()) - auto_aim_should_shoot_.bind_directly(false); + if (!should_shoot_.ready()) + should_shoot_.bind_directly(false); } void update() override { @@ -84,14 +70,13 @@ class BulletFeederController17mm const auto switch_left = *switch_left_; const auto mouse = *mouse_; const auto keyboard = *keyboard_; - const auto rotary_knob_switch = *rotary_knob_switch_; using namespace rmcs_msgs; if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) { reset_all_controls(); } else { - int64_t bullet_allowance = 0; + std::int64_t bullet_allowance = 0; if (switch_right != Switch::DOWN) { shoot_mode = keyboard.f ? ShootMode::SINGLE : ShootMode::AUTOMATIC; @@ -111,33 +96,22 @@ class BulletFeederController17mm if (*bullet_fired_) single_shot_stop_counter_ = 0; - const bool friction_ready = *friction_ready_ || debug_ignore_friction_ready_; - if (friction_ready) { + if (*friction_ready_) { if (shoot_mode == ShootMode::AUTOMATIC) { - bool manual_triggered = mouse.left || switch_left == Switch::DOWN; - bool auto_aim_triggered = mouse.right && *auto_aim_should_shoot_; - bool rotary_knob_triggered = rotary_knob_switch == Switch::DOWN; - bool triggered = - manual_triggered || auto_aim_triggered || rotary_knob_triggered; - bool rotary_knob_low_speed = - rotary_knob_triggered && !manual_triggered && !auto_aim_triggered; + auto aiming_enable = mouse_->right || (switch_right == Switch::UP); + auto attack_intent = mouse_->left || (switch_left == Switch::DOWN); + auto triggered = aiming_enable ? *should_shoot_ : attack_intent; bullet_allowance = triggered ? *control_bullet_allowance_limited_by_heat_ : 0; - update_bullet_feeder_velocity(bullet_allowance, rotary_knob_low_speed); } else { - bool triggered = single_shot_stop_counter_ > 0; + auto triggered = single_shot_stop_counter_ > 0; bullet_allowance = triggered && (*control_bullet_allowance_limited_by_heat_ > 0); - update_bullet_feeder_velocity(bullet_allowance, false); } - } else { - update_bullet_feeder_velocity(0, false); } } - if ((!*friction_ready_ && !debug_ignore_friction_ready_) - || switch_right == Switch::DOWN) - update_bullet_feeder_velocity(0, false); + update_bullet_feeder_velocity(bullet_allowance); } last_switch_right_ = switch_right; @@ -153,7 +127,7 @@ class BulletFeederController17mm *bullet_feeder_control_velocity_ = nan_; } - void update_bullet_feeder_velocity(int64_t bullet_allowance, bool rotary_knob_low_speed) { + void update_bullet_feeder_velocity(int64_t bullet_allowance) { if (bullet_allowance <= 0) { bullet_feeder_working_status_ = 0; *bullet_feeder_control_velocity_ = 0.0; @@ -167,9 +141,8 @@ class BulletFeederController17mm return; } - double new_control_velocity = rotary_knob_low_speed ? bullet_feeder_rotary_knob_velocity_ - : bullet_allowance > 1 ? bullet_feeder_working_velocity - : bullet_feeder_safe_shot_velocity; + double new_control_velocity = bullet_allowance > 1 ? bullet_feeder_working_velocity + : bullet_feeder_safe_shot_velocity; if (new_control_velocity > *bullet_feeder_control_velocity_) bullet_feeder_working_status_ = std::min(0, bullet_feeder_working_status_); *bullet_feeder_control_velocity_ = new_control_velocity; @@ -224,8 +197,6 @@ class BulletFeederController17mm int single_shot_max_stop_delay_, single_shot_stop_counter_ = 0; - bool debug_ignore_friction_ready_ = false; - InputInterface friction_ready_; InputInterface bullet_fired_; InputInterface control_bullet_allowance_limited_by_heat_; @@ -234,9 +205,8 @@ class BulletFeederController17mm InputInterface switch_left_; InputInterface mouse_; InputInterface keyboard_; - InputInterface rotary_knob_switch_; - InputInterface auto_aim_should_shoot_; + InputInterface should_shoot_; rmcs_msgs::Switch last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; rmcs_msgs::Switch last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; @@ -248,8 +218,6 @@ class BulletFeederController17mm int bullet_feeder_jammed_count_ = 0; int bullet_feeder_cool_down_ = 0; - double bullet_feeder_rotary_knob_velocity_ = 0.0; - int temporary_single_shot_counter_ = 0; OutputInterface shoot_mode_; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/omni_infantry.cpp b/rmcs_ws/src/rmcs_core/src/hardware/omni_infantry.cpp index 472b05a3..84b72077 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/omni_infantry.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/omni_infantry.cpp @@ -249,7 +249,7 @@ class OmniInfantry motor.store_status(data.can_data); } else if (can_id == 0x202) { auto& motor = chassis_wheel_motors_[1]; - motor.store_status(data.can_data); + motor.store_status(data.can_data); } else if (can_id == 0x203) { auto& motor = chassis_wheel_motors_[2]; motor.store_status(data.can_data); diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/flight.cpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/flight.cpp index b9bbd968..88367c26 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/flight.cpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/flight.cpp @@ -19,9 +19,11 @@ class Flight static constexpr uint8_t kUiModeEngineer = 2; 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)} , crosshair_circle_(Shape::Color::WHITE, x_center - 2, y_center - 30, 8, 2) - , status_ring_(26.5, 26.5, 600, 300, StatusRing::DynamicArcsVisibility{false, false}) + , status_ring_(26.5, 26.5, 600, 300, false, false) , horizontal_center_guidelines_( {Shape::Color::WHITE, 2, x_center - 360, y_center, x_center - 110, y_center}, {Shape::Color::WHITE, 2, x_center + 110, y_center, x_center + 360, y_center}) @@ -61,16 +63,10 @@ class Flight auto mode_value = auto_aim_mode_.ready() ? *auto_aim_mode_ : kUiModeCombat; switch (mode_value) { - case kUiModeOutpostOnly: - mode_indicator_.set_color(Shape::Color::GREEN); - break; - case kUiModeEngineer: - mode_indicator_.set_color(Shape::Color::PINK); - break; + case kUiModeOutpostOnly: mode_indicator_.set_color(Shape::Color::GREEN); break; + case kUiModeEngineer: mode_indicator_.set_color(Shape::Color::PINK); break; case kUiModeCombat: - default: - mode_indicator_.set_color(Shape::Color::YELLOW); - break; + default: mode_indicator_.set_color(Shape::Color::YELLOW); break; } } diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/shape.hpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/shape.hpp index 1ceae35e..37be26ae 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/shape.hpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/shape.hpp @@ -192,8 +192,6 @@ class Shape enter_run_queue(); } - void set_text_shape() { is_text_shape_ = true; } - virtual size_t write_description_field(std::byte* buffer) = 0; DescriptionField::Part2 part2_ alignas(4); diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/status_ring.hpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/status_ring.hpp index 32d8c614..c45eaf93 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/status_ring.hpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/status_ring.hpp @@ -1,11 +1,10 @@ #pragma once #include +#include #include #include #include - -#include #include #include @@ -16,20 +15,12 @@ namespace rmcs_core::referee::app::ui { class StatusRing { public: - struct DynamicArcsVisibility { - bool supercap; - bool battery; - }; - - StatusRing(double supercap_limit, double battery_limit, double friction_limit, int16_t bullet_limit) - : StatusRing( - supercap_limit, battery_limit, friction_limit, bullet_limit, - DynamicArcsVisibility{true, true}) {} - StatusRing( double supercap_limit, double battery_limit, double friction_limit, int16_t bullet_limit, - DynamicArcsVisibility dynamic_arcs_visibility) - : dynamic_arcs_visibility_(dynamic_arcs_visibility) { + bool supercap = true, bool battery = true) + : supercap_ui_{supercap} + , battery_ui_{battery} { + supercap_status_.set_x(x_center); supercap_status_.set_y(y_center); supercap_status_.set_r(visible_radius - width_ring); @@ -37,7 +28,7 @@ class StatusRing { supercap_status_.set_angle_end(275 + visible_angle); supercap_status_.set_width(width_ring); supercap_status_.set_color(Shape::Color::PINK); - supercap_status_.set_visible(true); + supercap_status_.set_visible(supercap); battery_status_.set_x(x_center); battery_status_.set_y(y_center); @@ -46,7 +37,7 @@ class StatusRing { battery_status_.set_angle_end(265); battery_status_.set_width(width_ring); battery_status_.set_color(Shape::Color::PINK); - battery_status_.set_visible(true); + battery_status_.set_visible(battery); friction_wheel_speed_.set_x(x_center); friction_wheel_speed_.set_y(y_center); @@ -120,14 +111,12 @@ class StatusRing { arc_right_down_.set_visible(true); set_limits(supercap_limit, battery_limit, friction_limit, bullet_limit); - supercap_status_.set_visible(dynamic_arcs_visibility_.supercap); - battery_status_.set_visible(dynamic_arcs_visibility_.battery); } void set_visible(bool value) { // Dynamic - supercap_status_.set_visible(value && dynamic_arcs_visibility_.supercap); - battery_status_.set_visible(value && dynamic_arcs_visibility_.battery); + supercap_status_.set_visible(value && supercap_ui_); + battery_status_.set_visible(value && battery_ui_); friction_wheel_speed_.set_visible(value); bullet_status_.set_visible(value); @@ -260,9 +249,6 @@ class StatusRing { } void update_supercap(double value, bool enable) { - if (!dynamic_arcs_visibility_.supercap) - return; - auto angle = 275 + calculate_angle(value, 10.5, supercap_limit_) + 1; supercap_status_.set_angle_end(static_cast(angle)); @@ -276,9 +262,6 @@ class StatusRing { } void update_battery_power(double value) { - if (!dynamic_arcs_visibility_.battery) - return; - auto angle = 265 - calculate_angle(value, 20, 25.7) - 1; battery_status_.set_angle_start(static_cast(angle)); @@ -373,13 +356,14 @@ class StatusRing { constexpr static uint16_t visible_radius = 400; constexpr static uint16_t visible_angle = 40; - DynamicArcsVisibility dynamic_arcs_visibility_; - double supercap_limit_; double battery_limit_; double friction_limit_; int16_t bullet_limit_; + bool supercap_ui_ = true; + bool battery_ui_ = true; + // Dynamic part Arc supercap_status_; diff --git a/rmcs_ws/src/rmcs_core/src/referee/command/interaction/ui.cpp b/rmcs_ws/src/rmcs_core/src/referee/command/interaction/ui.cpp index 4d57f606..57167144 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/command/interaction/ui.cpp +++ b/rmcs_ws/src/rmcs_core/src/referee/command/interaction/ui.cpp @@ -87,10 +87,6 @@ class Ui } size_t write_updating_field(std::byte* buffer) const { - if (auto written = write_updating_text_field(buffer); written != 0) { - return written; - } - size_t written = 0; auto& header = *new (buffer + written) Header{}; @@ -102,9 +98,15 @@ class Ui int slot = 0; intptr_t updated[7]; for (auto it = CfsScheduler::get_update_iterator(); it && slot < 7;) { + // Ignore text shape unless it is the first. if (it->is_text_shape()) { - it.ignore(); - continue; + if (slot == 0) { + header.command_id = 0x0110; // Draw text shape + return written + it.update().write(buffer + written); + } else { + it.ignore(); + continue; + } } auto operation = it->predict_update(); @@ -146,28 +148,6 @@ class Ui return written; } - size_t write_updating_text_field(std::byte* buffer) const { - size_t written = 0; - - auto& header = *new (buffer + written) Header{}; - auto full_robot_id = rmcs_msgs::FullRobotId { *robot_id_ }; - header.command_id = 0x0110; // Draw text shape - header.sender_id = full_robot_id; - header.receiver_id = full_robot_id.client(); - written += sizeof(Header); - - for (auto it = CfsScheduler::get_update_iterator(); it;) { - if (!it->is_text_shape()) { - it.ignore(); - continue; - } - - return written + it.update().write(buffer + written); - } - - return 0; - } - InputInterface robot_id_; InputInterface game_stage_; diff --git a/rmcs_ws/src/rmcs_core/src/referee/status.cpp b/rmcs_ws/src/rmcs_core/src/referee/status.cpp index 6cb4e026..c29bc3c2 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/status.cpp +++ b/rmcs_ws/src/rmcs_core/src/referee/status.cpp @@ -21,7 +21,9 @@ class Status , public rclcpp::Node { public: Status() - : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} + : Node{ + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} , logger_(get_logger()) { register_input("/referee/serial", serial_); @@ -282,7 +284,8 @@ class Status *map_command_received_timestamp_ = std::chrono::duration(now.time_since_epoch()).count(); - if (has_last_map_command_ && std::memcmp(&last_map_command_, &data, sizeof(data)) == 0) { + if (has_last_map_command_ + && std::memcmp(&last_map_command_, &data, sizeof(data)) == 0) { // NOLINT return; } From e100c136ecc91ae3ae1f032915f5ee7be5c62c26 Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Mon, 6 Jul 2026 17:07:28 +0800 Subject: [PATCH 30/30] chore: apply format --- .gitmodules | 1 - .../rmcs_description/sentry_description.hpp | 20 +++++++++---------- .../rmcs_description/tf_description.hpp | 18 ++++++++--------- 3 files changed, 19 insertions(+), 20 deletions(-) diff --git a/.gitmodules b/.gitmodules index 68f1d09b..61e5f08f 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,4 +1,3 @@ [submodule "rmcs_ws/src/fast_tf"] path = rmcs_ws/src/fast_tf url = https://github.com/qzhhhi/FastTF.git - diff --git a/rmcs_ws/src/rmcs_description/include/rmcs_description/sentry_description.hpp b/rmcs_ws/src/rmcs_description/include/rmcs_description/sentry_description.hpp index 2fc9d17b..1ac5d4ee 100644 --- a/rmcs_ws/src/rmcs_description/include/rmcs_description/sentry_description.hpp +++ b/rmcs_ws/src/rmcs_description/include/rmcs_description/sentry_description.hpp @@ -66,7 +66,7 @@ struct RightFrontWheelLink : fast_tf::Link { template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::BaseLink; + using Parent = rmcs_description::BaseLink; Eigen::Translation3d transform = Eigen::Translation3d::Identity(); }; @@ -114,37 +114,37 @@ struct fast_tf::Joint : fast_tf::ModificationTracka template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::PitchLink; + using Parent = rmcs_description::PitchLink; Eigen::Translation3d transform = Eigen::Translation3d::Identity(); }; template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::PitchLink; + using Parent = rmcs_description::PitchLink; Eigen::Translation3d transform = Eigen::Translation3d::Identity(); }; template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::PitchLink; + using Parent = rmcs_description::PitchLink; Eigen::Isometry3d transform = Eigen::Isometry3d::Identity(); }; template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::BottomYawLink; + using Parent = rmcs_description::BottomYawLink; Eigen::Quaterniond transform = Eigen::Quaterniond::Identity(); }; template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::PitchLink; + using Parent = rmcs_description::PitchLink; Eigen::Quaterniond transform = Eigen::Quaterniond::Identity(); }; template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::BaseLink; + using Parent = rmcs_description::BaseLink; Eigen::Isometry3d transform = Eigen::Isometry3d::Identity(); void set_state(double angle) { auto rotation = Eigen::AngleAxisd{std::numbers::pi / 4, Eigen::Vector3d::UnitZ()} @@ -155,7 +155,7 @@ struct fast_tf::Joint : fast_tf::Modificat template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::BaseLink; + using Parent = rmcs_description::BaseLink; Eigen::Isometry3d transform = Eigen::Isometry3d::Identity(); void set_state(double angle) { auto rotation = Eigen::AngleAxisd{std::numbers::pi / 4 * 3, Eigen::Vector3d::UnitZ()} @@ -166,7 +166,7 @@ struct fast_tf::Joint : fast_tf::Modificati template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::BaseLink; + using Parent = rmcs_description::BaseLink; Eigen::Isometry3d transform = Eigen::Isometry3d::Identity(); void set_state(double angle) { auto rotation = Eigen::AngleAxisd{-std::numbers::pi / 4 * 3, Eigen::Vector3d::UnitZ()} @@ -177,7 +177,7 @@ struct fast_tf::Joint : fast_tf::Modificat template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::BaseLink; + using Parent = rmcs_description::BaseLink; Eigen::Isometry3d transform = Eigen::Isometry3d::Identity(); void set_state(double angle) { auto rotation = Eigen::AngleAxisd{-std::numbers::pi / 4, Eigen::Vector3d::UnitZ()} diff --git a/rmcs_ws/src/rmcs_description/include/rmcs_description/tf_description.hpp b/rmcs_ws/src/rmcs_description/include/rmcs_description/tf_description.hpp index 650987f5..4a46051c 100644 --- a/rmcs_ws/src/rmcs_description/include/rmcs_description/tf_description.hpp +++ b/rmcs_ws/src/rmcs_description/include/rmcs_description/tf_description.hpp @@ -73,7 +73,7 @@ struct OmniLinkRight : fast_tf::Link { template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::BaseLink; + using Parent = rmcs_description::BaseLink; Eigen::Translation3d transform = Eigen::Translation3d::Identity(); }; @@ -101,25 +101,25 @@ struct fast_tf::Joint : fast_tf::ModificationTracka template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::PitchLink; + using Parent = rmcs_description::PitchLink; Eigen::Translation3d transform = Eigen::Translation3d::Identity(); }; template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::PitchLink; + using Parent = rmcs_description::PitchLink; Eigen::Translation3d transform = Eigen::Translation3d::Identity(); }; template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::PitchLink; + using Parent = rmcs_description::PitchLink; Eigen::Isometry3d transform = Eigen::Isometry3d::Identity(); }; template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::PitchLink; + using Parent = rmcs_description::PitchLink; Eigen::Quaterniond transform = Eigen::Quaterniond::Identity(); }; @@ -135,7 +135,7 @@ struct fast_tf::Joint : fast_tf::ModificationTrack }; template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::BaseLink; + using Parent = rmcs_description::BaseLink; Eigen::Isometry3d transform = Eigen::Isometry3d::Identity(); void set_state(double angle) { auto rotation = Eigen::AngleAxisd{std::numbers::pi / 4, Eigen::Vector3d::UnitZ()} @@ -146,7 +146,7 @@ struct fast_tf::Joint : fast_tf::Modificat template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::BaseLink; + using Parent = rmcs_description::BaseLink; Eigen::Isometry3d transform = Eigen::Isometry3d::Identity(); void set_state(double angle) { auto rotation = Eigen::AngleAxisd{std::numbers::pi / 4 * 3, Eigen::Vector3d::UnitZ()} @@ -157,7 +157,7 @@ struct fast_tf::Joint : fast_tf::Modificati template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::BaseLink; + using Parent = rmcs_description::BaseLink; Eigen::Isometry3d transform = Eigen::Isometry3d::Identity(); void set_state(double angle) { auto rotation = Eigen::AngleAxisd{-std::numbers::pi / 4 * 3, Eigen::Vector3d::UnitZ()} @@ -168,7 +168,7 @@ struct fast_tf::Joint : fast_tf::Modificat template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::BaseLink; + using Parent = rmcs_description::BaseLink; Eigen::Isometry3d transform = Eigen::Isometry3d::Identity(); void set_state(double angle) { auto rotation = Eigen::AngleAxisd{-std::numbers::pi / 4, Eigen::Vector3d::UnitZ()}