From faa660a00b8f7d047facaa0a6d6d261264bd5f8b Mon Sep 17 00:00:00 2001 From: noskillzheng <3515964992@qq.com> Date: Sun, 19 Jul 2026 09:13:56 +0800 Subject: [PATCH 01/11] feat:adapt autoaim on flight --- rmcs_ws/src/hikcamera | 1 + rmcs_ws/src/odin_ros_driver | 1 + rmcs_ws/src/rmcs_bringup/config/flight.yaml | 47 +++- .../controller/flight/px4_vision_bridge.cpp | 219 ++++++++++++++++++ rmcs_ws/src/rmcs_core/src/hardware/flight.cpp | 60 +++-- 5 files changed, 301 insertions(+), 27 deletions(-) create mode 160000 rmcs_ws/src/hikcamera create mode 160000 rmcs_ws/src/odin_ros_driver create mode 100644 rmcs_ws/src/rmcs_core/src/controller/flight/px4_vision_bridge.cpp 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 diff --git a/rmcs_ws/src/odin_ros_driver b/rmcs_ws/src/odin_ros_driver new file mode 160000 index 00000000..8cbaf718 --- /dev/null +++ b/rmcs_ws/src/odin_ros_driver @@ -0,0 +1 @@ +Subproject commit 8cbaf718ddc383afe98f63e63585e79117d25901 diff --git a/rmcs_ws/src/rmcs_bringup/config/flight.yaml b/rmcs_ws/src/rmcs_bringup/config/flight.yaml index d0082799..14c54cf3 100644 --- a/rmcs_ws/src/rmcs_bringup/config/flight.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/flight.yaml @@ -26,7 +26,50 @@ rmcs_executor: # - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster # - rmcs_core::broadcaster::TfBroadcaster -> tf_broadcaster - - rmcs::AutoAimComponent + - rmcs::AutoAimCapturerComponent -> auto_aim_capturer + # - rmcs::AutoAimPlayerComponent -> auto_aim_player + - rmcs::AutoAimRecorderComponent -> auto_aim_recorder + - rmcs::AutoAimComponent -> auto_aim_component + +auto_aim_capturer: + ros__parameters: + camera_name: "" + exposure_us: 3000.0 + gain: 8.0 + framerate: 120.0 + invert_image: false + rls_tau_sec: 10.0 + use_hardware_sync: false + delay_ms: 6.5 + +auto_aim_recorder: + ros__parameters: + output_path: "/tmp/autoaim/records" + queue_depth: 16 + flush_every_n_frames: 64 + max_duration_seconds: 0 + max_videos_size_gb: 0.0 + +auto_aim_component: + ros__parameters: + # WARN: 危险！可选 red | blue，生效后裁判系统 ID 缺席时 + # 将强行绑定为对应阵营哨兵，仅供调试，严禁比赛启用。 + # 留空或填 unknow 表示禁用。 + dangerous_fallback: "" + manual_shoot: true + camera_translation: [0.10238, 0.0, 0.05286] + fire_control: + bullet_speed: 22.5 + shoot_delay: 0.1 + offset_yaw: +2.0 + offset_pitch: +3.5 + attack_window: 120.0 + window_hysteresis: 0.2 + is_lazy_gimbal: false + attack_preaim: false + require_stable_command: false + yaw_tolerance: 0.07 + pitch_tolerance: 0.04 odin_ros_driver: ros__parameters: @@ -88,7 +131,7 @@ yaw_velocity_pid_controller: measurement: /gimbal/yaw/velocity_imu setpoint: /gimbal/yaw/control_velocity control: /gimbal/yaw/control_torque - kp: 8.0 + kp: 6.0 ki: 0.0 kd: 0.0 diff --git a/rmcs_ws/src/rmcs_core/src/controller/flight/px4_vision_bridge.cpp b/rmcs_ws/src/rmcs_core/src/controller/flight/px4_vision_bridge.cpp new file mode 100644 index 00000000..32bb2cce --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/flight/px4_vision_bridge.cpp @@ -0,0 +1,219 @@ +#include "nav_msgs/msg/odometry.hpp" +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace rmcs_core::hardware{ + +class Px4VisionBridge + : public rmcs_executor::Component + , public rclcpp::Node { +public: + Px4VisionBridge() + :Node{ + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} + , logger_(get_logger()) { + + register_input("/px4/serial", px4_serial_); + + source_topic_ = get_parameter("source_topic").as_string(); + system_id_ = get_parameter("system_id").as_int(); + component_id_ = get_parameter("component_id").as_int(); + max_send_rate_hz_ = get_parameter("max_send_rate_hz").as_double(); + + const auto m =get_parameter("mount_rpy").as_double_array(); + + q_mount_ =quaternion_from_rpy_zyx(m.at(0),m.at(1),m.at(2)); + + min_send_interval_ =rclcpp::Duration::from_seconds(1.0/max_send_rate_hz_); + last_send_time_ =now(); + last_heartbeat_send_ =now(); + + odom_subscription_ =create_subscription( + source_topic_, + rclcpp::SensorDataQoS{}, + [this](nav_msgs::msg::Odometry::UniquePtr msg){ + odometry_callback(std::move(msg)); + } + ); + } + + Px4VisionBridge(const Px4VisionBridge&) = delete; + Px4VisionBridge& operator=(const Px4VisionBridge&) = delete; + + void update() override { + send_heartbeat_if_due(); //1hz HEARTBEAT + send_vision_if_pending(); //消费回调缓存的最新位姿,变换并发送 + } + +private: + + struct OdomSample { + rclcpp::Time stamp{0, 0, RCL_ROS_TIME}; + Eigen::Vector3d position{Eigen::Vector3d::Zero()}; + Eigen::Quaterniond orientation{Eigen::Quaterniond::Identity()}; + }; + + void odometry_callback(const nav_msgs::msg::Odometry::UniquePtr msg){ + const auto& p =msg->pose.pose.position; + const auto& o =msg->pose.pose.orientation; + + const std::scoped_lock lock{odom_mutex_}; + pending_odom_.stamp =rclcpp::Time{msg->header.stamp}; + pending_odom_.position =Eigen::Vector3d{p.x,p.y,p.z}; + pending_odom_.orientation =Eigen::Quaterniond{o.w,o.x,o.y,o.z}; + odom_pending_ =true; + } + + void send_vision_if_pending(){ + if((now() -last_send_time_)(sample.position.y()); + const auto y =static_cast(sample.position.x()); + const auto z =static_cast(-sample.position.z()); + + const Eigen::Quaterniond q_enu_flu =sample.orientation *q_mount_; + + Eigen::Quaterniond q_ned_frd =kNedEnuQ *(q_enu_flu*kAircraftBaselinkQ); + + q_ned_frd.normalize(); + double roll,pitch,yaw; + quaternion_to_rpy_zyx(q_ned_frd,roll,pitch,yaw); + + const auto usec = static_cast(sample.stamp.nanoseconds())/1000ull; + + float covariance[21]; + std::fill(std::begin(covariance),std::end(covariance),NAN); + + mavlink_message_t msg_mavlink; + mavlink_msg_vision_position_estimate_pack( + system_id_, + component_id_, + &msg_mavlink, + usec, + x,y,z, + static_cast(roll), + static_cast(pitch), + static_cast(yaw), + covariance, + /*reset_counter=*/0 + ); + + send_message(msg_mavlink); + } + + void send_heartbeat_if_due(){ + if((now()-last_heartbeat_send_).seconds()<1.0) + return; + last_heartbeat_send_ =now(); + mavlink_message_t msg_mavlink; + mavlink_msg_heartbeat_pack( + system_id_, + component_id_, + &msg_mavlink, + MAV_TYPE_ONBOARD_CONTROLLER, + MAV_AUTOPILOT_INVALID, + /*base_mode=*/0, + /*custom_mode=*/0, + MAV_STATE_ACTIVE + ); + send_message(msg_mavlink); + } + + void send_message(const mavlink_message_t& msg){ + if(!px4_serial_.active())[[unlikely]] + return; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + const uint16_t len =mavlink_msg_to_send_buffer(buffer,&msg); + px4_serial_->write(reinterpret_cast(buffer), len); + } + + static Eigen::Quaterniond quaternion_from_rpy_zyx(double roll, double pitch, double yaw){ + + return Eigen::Quaterniond{ + + Eigen::AngleAxisd{yaw, Eigen::Vector3d::UnitZ()} * + Eigen::AngleAxisd{pitch, Eigen::Vector3d::UnitY()} * + Eigen::AngleAxisd{roll, Eigen::Vector3d::UnitX()} + }; + } + + static void quaternion_to_rpy_zyx(const Eigen::Quaterniond& q, double& roll,double& pitch, double& yaw){ + const double w =q.w(),x =q.x(),y =q.y(),z =q.z(); + const double m20 =2.0*(x*z - w*y); // -sin(pitch) + const double m21 =2.0*(y*z + w*x); + const double m22 =1.0 - 2.0*(x*x + y*y); + const double sin_pitch =std::clamp(-m20, -1.0, 1.0); + const double cos_pitch =std::sqrt(m21*m21 + m22*m22); + + if(cos_pitch > 1e-9){ + pitch = std::atan2(sin_pitch, cos_pitch); + roll = std::atan2(m21, m22); + yaw = std::atan2(2.0*(x*y + w*z), 1.0 - 2.0*(y*y + z*z)); + return; + } + // 万向节锁 (pitch=±π/2)：roll/yaw 只剩组合自由度可观测， + // 约定 roll=0、组合角全部归 yaw，保证三元组重建仍是原旋转 + const double m11 =1.0 - 2.0*(x*x + z*z); + const double m12 =2.0*(y*z - w*x); + roll = 0.0; + pitch = (sin_pitch > 0.0) ? M_PI_2 : -M_PI_2; + yaw = (sin_pitch > 0.0) ? std::atan2(m12, m11) : std::atan2(-m12, m11); + } + //ENU->NED + static inline const Eigen::Quaterniond& kNedEnuQ{ + 0.0,0.70710678118655,0.70710678118655,0.0 + }; + //FLU->FRD + static inline const Eigen::Quaterniond& kAircraftBaselinkQ{ + 0.0,1.0,0.0,0.0 + }; + + rclcpp::Logger logger_; + InputInterface px4_serial_; + rclcpp::Subscription::SharedPtr odom_subscription_; + + // 回调线程(rclcpp spin)与 update() 线程(executor)间的最新位姿交接 + std::mutex odom_mutex_; + OdomSample pending_odom_; + bool odom_pending_ =false; + + std::string source_topic_; + uint8_t system_id_; + uint8_t component_id_; + double max_send_rate_hz_; + Eigen::Quaterniond q_mount_; + + rclcpp::Duration min_send_interval_ =rclcpp::Duration::from_seconds(0.02); + rclcpp::Time last_send_time_ =rclcpp::Time(0, 0, RCL_ROS_TIME); + rclcpp::Time last_heartbeat_send_{0,0,RCL_ROS_TIME}; +}; + +}// namespace rmcs_core::hardware + +#include +PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::Px4VisionBridge, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp b/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp index d1c5b214..fa25de5b 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp @@ -2,7 +2,6 @@ #include #include #include -#include #include #include @@ -10,11 +9,14 @@ #include #include #include +#include +#include #include #include #include -#include "hardware/device/bmi088.hpp" +#include "hardware/device/bmi088_ekf.hpp" +#include "hardware/device/board_clock_lifter.hpp" #include "hardware/device/can_packet.hpp" #include "hardware/device/dji_motor.hpp" #include "hardware/device/dr16.hpp" @@ -51,9 +53,6 @@ class Flight gimbal_bullet_feeder_.configure( device::DjiMotor::Config{device::DjiMotor::Type::kM2006, 1}.enable_multi_turn_angle()); - bmi088_.set_coordinate_mapping( - [](double x, double y, double z) { return std::tuple{y, z, x}; }); - using namespace rmcs_description; constexpr auto kCameraPostionX = 0.10238; @@ -61,12 +60,11 @@ class Flight tf_->set_transform( Eigen::Translation3d{kCameraPostionX, 0.0, kCameraPostionZ}); - register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_); - register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_imu_); + register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_, 0.0); + register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_imu_, 0.0); register_output("/tf", tf_); - register_output("/auto_aim/camera_transform", camera_transform_); - register_output("/auto_aim/barrel_direction", barrel_direction_); + register_output("/gimbal/auto_aim/imu_snapshot", imu_snapshot_output_); register_output("/referee/serial", referee_serial_); referee_serial_->read = [this](std::byte* buffer, size_t size) { @@ -98,12 +96,6 @@ class Flight update_motors(); update_imu(); dr16_.update_status(); - remote_control_->update(); - - 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() { @@ -162,13 +154,12 @@ class Flight void update_imu() { using namespace rmcs_description; - bmi088_.update_status(); - const auto gimbal_imu_pose = - Eigen::Quaterniond{bmi088_.q0(), bmi088_.q1(), bmi088_.q2(), bmi088_.q3()}; - tf_->set_transform(gimbal_imu_pose.conjugate()); + if (const auto snapshot = bmi088_.snapshot()) { + tf_->set_transform(snapshot->orientation.conjugate()); - *gimbal_yaw_velocity_imu_ = bmi088_.gz(); - *gimbal_pitch_velocity_imu_ = bmi088_.gy(); + *gimbal_yaw_velocity_imu_ = snapshot->gyro_body.z(); + *gimbal_pitch_velocity_imu_ = snapshot->gyro_body.y(); + } } void @@ -225,11 +216,21 @@ class Flight } void accelerometer_receive_callback(const View::ImuAccelerometer& data) override { - bmi088_.store_accelerometer_status(data.x, data.y, data.z); + const auto timestamp = board_clock_lifter_.advance_timebase(data.timestamp_quarter_us); + bmi088_.push_accelerometer_sample(data.x, data.y, data.z, timestamp); } void gyroscope_receive_callback(const View::ImuGyroscope& data) override { - bmi088_.store_gyroscope_status(data.x, data.y, data.z); + const auto timestamp = board_clock_lifter_.lift_timestamp(data.timestamp_quarter_us); + if (!timestamp.has_value()) + return; + + const auto snapshot = + bmi088_.try_update_with_gyroscope_sample(data.x, data.y, data.z, *timestamp); + if (!snapshot) + return; + + imu_snapshot_output_.emit(*snapshot); } private: @@ -256,17 +257,26 @@ class Flight device::DjiMotor gimbal_right_friction_{*this, *command_component_, "/gimbal/right_friction"}; device::DjiMotor gimbal_bullet_feeder_{*this, *command_component_, "/gimbal/bullet_feeder"}; +<<<<<<< HEAD device::Dr16 dr16_; std::unique_ptr remote_control_; device::Bmi088 bmi088_{1000.0, 0.2, 0.00}; +======= + device::Dr16 dr16_{*this}; + // 等价于旧 Bmi088 的坐标映射 (x, y, z) -> (y, z, x)：body = body_to_sensor^T * sensor + device::Bmi088Ekf bmi088_{device::Bmi088Ekf::Config{ + .body_to_sensor = + (Eigen::Matrix3d{} << 0, 0, 1, 1, 0, 0, 0, 1, 0).finished(), + }}; + device::BoardClockLifter board_clock_lifter_; +>>>>>>> 8d383fe4 (feat:adapt autoaim on flight) OutputInterface gimbal_yaw_velocity_imu_; OutputInterface gimbal_pitch_velocity_imu_; OutputInterface tf_; OutputInterface referee_serial_; - OutputInterface camera_transform_; - OutputInterface barrel_direction_; + EventOutputInterface imu_snapshot_output_; rmcs_utility::RingBuffer referee_ring_buffer_receive_{256}; }; From f500a608a0a5c0909ca2e0d940065b08460a728f Mon Sep 17 00:00:00 2001 From: noskillzheng <3515964992@qq.com> Date: Mon, 20 Jul 2026 05:33:56 +0800 Subject: [PATCH 02/11] feat:add flight odin support by mavlink component --- rmcs_ws/src/rmcs_bringup/config/flight.yaml | 21 ++-- rmcs_ws/src/rmcs_core/CMakeLists.txt | 2 + rmcs_ws/src/rmcs_core/package.xml | 3 + rmcs_ws/src/rmcs_core/plugins.xml | 2 + .../controller/flight/px4_vision_bridge.cpp | 96 +++++++++++++++++++ rmcs_ws/src/rmcs_core/src/hardware/flight.cpp | 6 +- 6 files changed, 118 insertions(+), 12 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/flight.yaml b/rmcs_ws/src/rmcs_bringup/config/flight.yaml index 14c54cf3..b9fd68a3 100644 --- a/rmcs_ws/src/rmcs_bringup/config/flight.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/flight.yaml @@ -3,6 +3,7 @@ rmcs_executor: update_rate: 1000.0 components: - rmcs_core::hardware::Flight -> flight_hardware + - rmcs_core::hardware::Px4VisionBridge -> px4_vision_bridge - rmcs_core::controller::gimbal::SimpleGimbalController -> gimbal_controller - rmcs_core::controller::pid::ErrorPidController -> yaw_angle_pid_controller @@ -71,13 +72,19 @@ auto_aim_component: yaw_tolerance: 0.07 pitch_tolerance: 0.04 -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 +px4_vision_bridge: + ros__parameters: + source_topic: /odin1/odometry_highfreq + system_id: 1 # 与 PX4 MAV_SYS_ID 一致 + component_id: 197 # MAV_COMP_ID_VISUAL_INERTIAL_ODOMETRY + max_send_rate_hz: 50.0 + # 挂载角(ZYX, rad): 传感器系->机体系, 初值 roll=π pitch=π/2 yaw=0 + mount_rpy: [3.14159265358979, 1.5707963267949, 0.0] + # Odin1 自启看门狗: 里程计断流超过 timeout 秒且距上次拉起超过 cooldown 秒 + # 才执行 tmux-launch.sh; 数据正常时不会重启 Odin1 (保住 SLAM 预热) + odin_autostart: true + odin_watchdog_timeout: 10.0 + odin_restart_cooldown: 60.0 # 需覆盖 SLAM 预热 30~60s value_broadcaster: ros__parameters: diff --git a/rmcs_ws/src/rmcs_core/CMakeLists.txt b/rmcs_ws/src/rmcs_core/CMakeLists.txt index 0ae6350a..234c0eeb 100644 --- a/rmcs_ws/src/rmcs_core/CMakeLists.txt +++ b/rmcs_ws/src/rmcs_core/CMakeLists.txt @@ -13,6 +13,7 @@ endif() find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() +find_package(mavlink REQUIRED) include(FetchContent) set(BUILD_STATIC_LIBRMCS ON CACHE BOOL "Build static librmcs SDK" FORCE) @@ -33,6 +34,7 @@ ament_auto_add_library( ${PROJECT_NAME} SHARED ${PROJECT_SOURCE} ) +target_include_directories(${PROJECT_NAME} PRIVATE ${mavlink_INCLUDE_DIRS}) include_directories(${PROJECT_SOURCE_DIR}/include) include_directories(${PROJECT_SOURCE_DIR}/src) diff --git a/rmcs_ws/src/rmcs_core/package.xml b/rmcs_ws/src/rmcs_core/package.xml index 4312d334..b3d6d14a 100644 --- a/rmcs_ws/src/rmcs_core/package.xml +++ b/rmcs_ws/src/rmcs_core/package.xml @@ -20,6 +20,9 @@ rmcs_msgs rmcs_executor rmcs_description + mavlink + nav_msgs + ament_index_cpp 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 95777e34..a9845583 100644 --- a/rmcs_ws/src/rmcs_core/plugins.xml +++ b/rmcs_ws/src/rmcs_core/plugins.xml @@ -5,6 +5,8 @@ + + diff --git a/rmcs_ws/src/rmcs_core/src/controller/flight/px4_vision_bridge.cpp b/rmcs_ws/src/rmcs_core/src/controller/flight/px4_vision_bridge.cpp index 32bb2cce..8b7e58d2 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/flight/px4_vision_bridge.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/flight/px4_vision_bridge.cpp @@ -2,12 +2,18 @@ #include #include +#include +#include #include +#include #include #include +#include #include #include +#include +#include #include #include #include @@ -49,6 +55,24 @@ class Px4VisionBridge odometry_callback(std::move(msg)); } ); + + odin_autostart_ =get_parameter("odin_autostart").as_bool(); + odin_watchdog_timeout_ =get_parameter("odin_watchdog_timeout").as_double(); + odin_restart_cooldown_ =get_parameter("odin_restart_cooldown").as_double(); + + last_odom_arrival_ns_.store(steady_now_ns(),std::memory_order_relaxed); + if(odin_autostart_) + odin_manager_thread_ =std::thread{[this]{odin_manager_loop();}}; + } + + ~Px4VisionBridge() override { + { + const std::scoped_lock lock{odin_stop_mutex_}; + odin_stop_ =true; + } + odin_stop_cv_.notify_all(); + if(odin_manager_thread_.joinable()) + odin_manager_thread_.join(); } Px4VisionBridge(const Px4VisionBridge&) = delete; @@ -68,6 +92,8 @@ class Px4VisionBridge }; void odometry_callback(const nav_msgs::msg::Odometry::UniquePtr msg){ + last_odom_arrival_ns_.store(steady_now_ns(),std::memory_order_relaxed); + const auto& p =msg->pose.pose.position; const auto& o =msg->pose.pose.orientation; @@ -152,6 +178,66 @@ class Px4VisionBridge px4_serial_->write(reinterpret_cast(buffer), len); } + static int64_t steady_now_ns(){ + return std::chrono::duration_cast( + std::chrono::steady_clock::now().time_since_epoch()) + .count(); + } + + // 独立线程看门狗：断流超时且过了冷却期就拉起 tmux-launch.sh； + void odin_manager_loop(){ + const std::string script = + ament_index_cpp::get_package_prefix("odin_ros_driver") + +"/lib/odin_ros_driver/tmux-launch.sh"; + + const auto timeout_ns =static_cast(odin_watchdog_timeout_*1e9); + const auto cooldown_ns =static_cast(odin_restart_cooldown_*1e9); + int64_t last_launch_ns =steady_now_ns()-cooldown_ns; //首次断流即可拉起 + + bool offline_reported =false; + bool launch_failure_reported =false; + bool online =false; + + std::unique_lock lock{odin_stop_mutex_}; + while(!odin_stop_cv_.wait_for( + lock,std::chrono::seconds{1},[this]{return odin_stop_;})){ + lock.unlock(); + + const auto now_ns =steady_now_ns(); + const bool data_fresh = + (now_ns-last_odom_arrival_ns_.load(std::memory_order_relaxed))cooldown_ns){ + last_launch_ns =now_ns; + // system() 最坏阻塞十余秒（脚本内部的清杀/启动轮询），只能在本线程做 + if(std::system(("\""+script+"\"").c_str())!=0 + &&!launch_failure_reported){ + launch_failure_reported =true; + RCLCPP_ERROR(logger_,"Odin1 launch script failed"); + } + } + } + + lock.lock(); + } + } + static Eigen::Quaterniond quaternion_from_rpy_zyx(double roll, double pitch, double yaw){ return Eigen::Quaterniond{ @@ -211,6 +297,16 @@ class Px4VisionBridge rclcpp::Duration min_send_interval_ =rclcpp::Duration::from_seconds(0.02); rclcpp::Time last_send_time_ =rclcpp::Time(0, 0, RCL_ROS_TIME); rclcpp::Time last_heartbeat_send_{0,0,RCL_ROS_TIME}; + + // Odin1 拉起与看门狗 + bool odin_autostart_; + double odin_watchdog_timeout_; + double odin_restart_cooldown_; + std::atomic last_odom_arrival_ns_{0}; + std::thread odin_manager_thread_; + std::mutex odin_stop_mutex_; + std::condition_variable odin_stop_cv_; + bool odin_stop_ =false; }; }// namespace rmcs_core::hardware diff --git a/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp b/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp index fa25de5b..75e90338 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp @@ -96,6 +96,7 @@ class Flight update_motors(); update_imu(); dr16_.update_status(); + remote_control_->update(); } void command_update() { @@ -257,19 +258,14 @@ class Flight device::DjiMotor gimbal_right_friction_{*this, *command_component_, "/gimbal/right_friction"}; device::DjiMotor gimbal_bullet_feeder_{*this, *command_component_, "/gimbal/bullet_feeder"}; -<<<<<<< HEAD device::Dr16 dr16_; std::unique_ptr remote_control_; - device::Bmi088 bmi088_{1000.0, 0.2, 0.00}; -======= - device::Dr16 dr16_{*this}; // 等价于旧 Bmi088 的坐标映射 (x, y, z) -> (y, z, x)：body = body_to_sensor^T * sensor device::Bmi088Ekf bmi088_{device::Bmi088Ekf::Config{ .body_to_sensor = (Eigen::Matrix3d{} << 0, 0, 1, 1, 0, 0, 0, 1, 0).finished(), }}; device::BoardClockLifter board_clock_lifter_; ->>>>>>> 8d383fe4 (feat:adapt autoaim on flight) OutputInterface gimbal_yaw_velocity_imu_; OutputInterface gimbal_pitch_velocity_imu_; From 718105710012947330ed525a44cd981065202b16 Mon Sep 17 00:00:00 2001 From: noskillzheng <3515964992@qq.com> Date: Mon, 20 Jul 2026 06:14:02 +0800 Subject: [PATCH 03/11] fix:add px4_serial output (forgot in flight.cpp) --- rmcs_ws/src/rmcs_core/src/hardware/flight.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp b/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp index 75e90338..65135426 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp @@ -77,6 +77,14 @@ class Flight return size; }; + register_output("/px4/serial", px4_serial_); + px4_serial_->read = [](std::byte*, size_t) { return size_t{0}; }; + px4_serial_->write = [this](const std::byte* buffer, size_t size) { + board_->start_transmit().uart_transmit( + Spec::kUarts.kUart0, {.uart_data = std::span{buffer, size}}); + return size; + }; + remote_control_ = std::make_unique(*this); remote_control_->register_dr16(&dr16_); @@ -271,6 +279,7 @@ class Flight OutputInterface gimbal_pitch_velocity_imu_; OutputInterface tf_; OutputInterface referee_serial_; + OutputInterface px4_serial_; EventOutputInterface imu_snapshot_output_; From 55659f8d8f5570e967aa8f306c87587cb0d73660 Mon Sep 17 00:00:00 2001 From: noskillzheng <3515964992@qq.com> Date: Tue, 21 Jul 2026 05:13:55 +0800 Subject: [PATCH 04/11] :chore: update autoaim offset --- rmcs_ws/src/rmcs_bringup/config/flight.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/flight.yaml b/rmcs_ws/src/rmcs_bringup/config/flight.yaml index b9fd68a3..33b3a146 100644 --- a/rmcs_ws/src/rmcs_bringup/config/flight.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/flight.yaml @@ -62,8 +62,8 @@ auto_aim_component: fire_control: bullet_speed: 22.5 shoot_delay: 0.1 - offset_yaw: +2.0 - offset_pitch: +3.5 + offset_yaw: +1.5 #越大越左 + offset_pitch: +3.8 #越大越下 attack_window: 120.0 window_hysteresis: 0.2 is_lazy_gimbal: false From 021bca518379f6cef394bec003108d958f30332a Mon Sep 17 00:00:00 2001 From: noskillzheng <3515964992@qq.com> Date: Thu, 23 Jul 2026 22:28:48 +0800 Subject: [PATCH 05/11] chore:add submodule odin and hikcamera --- .gitmodules | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/.gitmodules b/.gitmodules index de2c6a1c..bf739808 100644 --- a/.gitmodules +++ b/.gitmodules @@ -4,3 +4,9 @@ [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 +[submodule "rmcs_ws/src/hikcamera"] + path = rmcs_ws/src/hikcamera + url = https://github.com/Alliance-Algorithm/ros2-hikcamera.git +[submodule "rmcs_ws/src/odin_ros_driver"] + path = rmcs_ws/src/odin_ros_driver + url = https://github.com/noskillzheng/odin_ros_driver.git From 2d2f822dd8c7a7eda985a4d0b2df72fadc232834 Mon Sep 17 00:00:00 2001 From: noskillzheng <3515964992@qq.com> Date: Fri, 24 Jul 2026 01:11:35 +0800 Subject: [PATCH 06/11] fix:add autoaim param degraded_angle_speed,refactor:odin management in px4_vision_bridge --- rmcs_ws/src/rmcs_bringup/config/flight.yaml | 1 + .../controller/flight/px4_vision_bridge.cpp | 75 +++++++------------ 2 files changed, 27 insertions(+), 49 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/flight.yaml b/rmcs_ws/src/rmcs_bringup/config/flight.yaml index 33b3a146..861f855f 100644 --- a/rmcs_ws/src/rmcs_bringup/config/flight.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/flight.yaml @@ -65,6 +65,7 @@ auto_aim_component: offset_yaw: +1.5 #越大越左 offset_pitch: +3.8 #越大越下 attack_window: 120.0 + degraded_angle_speed: 12.0 window_hysteresis: 0.2 is_lazy_gimbal: false attack_preaim: false diff --git a/rmcs_ws/src/rmcs_core/src/controller/flight/px4_vision_bridge.cpp b/rmcs_ws/src/rmcs_core/src/controller/flight/px4_vision_bridge.cpp index 8b7e58d2..3aa18efe 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/flight/px4_vision_bridge.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/flight/px4_vision_bridge.cpp @@ -10,6 +10,7 @@ #include #include #include +#include #include #include @@ -62,17 +63,7 @@ class Px4VisionBridge last_odom_arrival_ns_.store(steady_now_ns(),std::memory_order_relaxed); if(odin_autostart_) - odin_manager_thread_ =std::thread{[this]{odin_manager_loop();}}; - } - - ~Px4VisionBridge() override { - { - const std::scoped_lock lock{odin_stop_mutex_}; - odin_stop_ =true; - } - odin_stop_cv_.notify_all(); - if(odin_manager_thread_.joinable()) - odin_manager_thread_.join(); + odin_manager_thread_ =std::jthread{[this](std::stop_token st){odin_manager_loop(st);}}; } Px4VisionBridge(const Px4VisionBridge&) = delete; @@ -185,56 +176,44 @@ class Px4VisionBridge } // 独立线程看门狗：断流超时且过了冷却期就拉起 tmux-launch.sh； - void odin_manager_loop(){ - const std::string script = - ament_index_cpp::get_package_prefix("odin_ros_driver") - +"/lib/odin_ros_driver/tmux-launch.sh"; + void odin_manager_loop(std::stop_token st){ + const std::string cmd = + "\""+ament_index_cpp::get_package_prefix("odin_ros_driver") + +"/lib/odin_ros_driver/tmux-launch.sh\""; const auto timeout_ns =static_cast(odin_watchdog_timeout_*1e9); const auto cooldown_ns =static_cast(odin_restart_cooldown_*1e9); int64_t last_launch_ns =steady_now_ns()-cooldown_ns; //首次断流即可拉起 - - bool offline_reported =false; - bool launch_failure_reported =false; - bool online =false; + bool online =true; //启动宽限：先假定在线，超时才告警 - std::unique_lock lock{odin_stop_mutex_}; - while(!odin_stop_cv_.wait_for( - lock,std::chrono::seconds{1},[this]{return odin_stop_;})){ - lock.unlock(); + // 锁仅为 wait_for 语义存在 request_stop 会直接唤醒等待 + std::mutex sleep_mutex; + std::condition_variable_any sleep_cv; + std::unique_lock lock{sleep_mutex}; + while(!sleep_cv.wait_for( + lock,st,std::chrono::seconds{1},[&]{return st.stop_requested();})){ const auto now_ns =steady_now_ns(); - const bool data_fresh = + const bool fresh = (now_ns-last_odom_arrival_ns_.load(std::memory_order_relaxed))cooldown_ns){ - last_launch_ns =now_ns; - // system() 最坏阻塞十余秒（脚本内部的清杀/启动轮询），只能在本线程做 - if(std::system(("\""+script+"\"").c_str())!=0 - &&!launch_failure_reported){ - launch_failure_reported =true; - RCLCPP_ERROR(logger_,"Odin1 launch script failed"); - } - } } - lock.lock(); + if(!online &&(now_ns-last_launch_ns)>cooldown_ns){ + last_launch_ns =now_ns; + // （脚本内部的清杀/启动轮询）存在阻塞，单独在本线程做 + if(std::system(cmd.c_str())!=0) + RCLCPP_ERROR(logger_,"Odin1 launch script failed"); + } } } @@ -303,10 +282,8 @@ class Px4VisionBridge double odin_watchdog_timeout_; double odin_restart_cooldown_; std::atomic last_odom_arrival_ns_{0}; - std::thread odin_manager_thread_; - std::mutex odin_stop_mutex_; - std::condition_variable odin_stop_cv_; - bool odin_stop_ =false; + // 必须最后声明：其隐式析构 request_stop+join，须早于线程所用成员的销毁 + std::jthread odin_manager_thread_; }; }// namespace rmcs_core::hardware From d8cdb2b2590bf6e003cc403a6d50c319fd16bfd5 Mon Sep 17 00:00:00 2001 From: noskillzheng <3515964992@qq.com> Date: Sat, 25 Jul 2026 22:24:10 +0800 Subject: [PATCH 07/11] chore:remove hikcamera and odin_ros_driver submodules --- .gitmodules | 6 ------ rmcs_ws/src/hikcamera | 1 - rmcs_ws/src/odin_ros_driver | 1 - rmcs_ws/src/rmcs_bringup/config/flight.yaml | 8 ++++---- 4 files changed, 4 insertions(+), 12 deletions(-) delete mode 160000 rmcs_ws/src/hikcamera delete mode 160000 rmcs_ws/src/odin_ros_driver diff --git a/.gitmodules b/.gitmodules index bf739808..de2c6a1c 100644 --- a/.gitmodules +++ b/.gitmodules @@ -4,9 +4,3 @@ [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 -[submodule "rmcs_ws/src/hikcamera"] - path = rmcs_ws/src/hikcamera - url = https://github.com/Alliance-Algorithm/ros2-hikcamera.git -[submodule "rmcs_ws/src/odin_ros_driver"] - path = rmcs_ws/src/odin_ros_driver - url = https://github.com/noskillzheng/odin_ros_driver.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 8cbaf718..00000000 --- a/rmcs_ws/src/odin_ros_driver +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 8cbaf718ddc383afe98f63e63585e79117d25901 diff --git a/rmcs_ws/src/rmcs_bringup/config/flight.yaml b/rmcs_ws/src/rmcs_bringup/config/flight.yaml index 861f855f..8dba1b22 100644 --- a/rmcs_ws/src/rmcs_bringup/config/flight.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/flight.yaml @@ -61,10 +61,10 @@ auto_aim_component: camera_translation: [0.10238, 0.0, 0.05286] fire_control: bullet_speed: 22.5 - shoot_delay: 0.1 - offset_yaw: +1.5 #越大越左 - offset_pitch: +3.8 #越大越下 - attack_window: 120.0 + shoot_delay: 0.05 + offset_yaw: +1.3 #越大越左 + offset_pitch: +1.7 #越大越下 + attack_window: 80.0 degraded_angle_speed: 12.0 window_hysteresis: 0.2 is_lazy_gimbal: false From 6bd8d103f8401b9d4c9b9988d5fedfad7b45e29c Mon Sep 17 00:00:00 2001 From: noskillzheng <3515964992@qq.com> Date: Tue, 28 Jul 2026 06:33:10 +0800 Subject: [PATCH 08/11] chore:apply format --- .../controller/flight/px4_vision_bridge.cpp | 257 ++++++++---------- rmcs_ws/src/rmcs_core/src/hardware/flight.cpp | 3 +- 2 files changed, 119 insertions(+), 141 deletions(-) diff --git a/rmcs_ws/src/rmcs_core/src/controller/flight/px4_vision_bridge.cpp b/rmcs_ws/src/rmcs_core/src/controller/flight/px4_vision_bridge.cpp index 3aa18efe..e2a2b119 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/flight/px4_vision_bridge.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/flight/px4_vision_bridge.cpp @@ -22,241 +22,220 @@ #include #include -namespace rmcs_core::hardware{ +namespace rmcs_core::hardware { class Px4VisionBridge : public rmcs_executor::Component , public rclcpp::Node { public: Px4VisionBridge() - :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("/px4/serial", px4_serial_); - source_topic_ = get_parameter("source_topic").as_string(); - system_id_ = get_parameter("system_id").as_int(); - component_id_ = get_parameter("component_id").as_int(); + source_topic_ = get_parameter("source_topic").as_string(); + system_id_ = get_parameter("system_id").as_int(); + component_id_ = get_parameter("component_id").as_int(); max_send_rate_hz_ = get_parameter("max_send_rate_hz").as_double(); - const auto m =get_parameter("mount_rpy").as_double_array(); + const auto m = get_parameter("mount_rpy").as_double_array(); - q_mount_ =quaternion_from_rpy_zyx(m.at(0),m.at(1),m.at(2)); + q_mount_ = quaternion_from_rpy_zyx(m.at(0), m.at(1), m.at(2)); - min_send_interval_ =rclcpp::Duration::from_seconds(1.0/max_send_rate_hz_); - last_send_time_ =now(); - last_heartbeat_send_ =now(); + min_send_interval_ = rclcpp::Duration::from_seconds(1.0 / max_send_rate_hz_); + last_send_time_ = now(); + last_heartbeat_send_ = now(); - odom_subscription_ =create_subscription( - source_topic_, - rclcpp::SensorDataQoS{}, - [this](nav_msgs::msg::Odometry::UniquePtr msg){ - odometry_callback(std::move(msg)); - } - ); + odom_subscription_ = create_subscription( + source_topic_, rclcpp::SensorDataQoS{}, + [this](nav_msgs::msg::Odometry::UniquePtr msg) { odometry_callback(std::move(msg)); }); - odin_autostart_ =get_parameter("odin_autostart").as_bool(); - odin_watchdog_timeout_ =get_parameter("odin_watchdog_timeout").as_double(); - odin_restart_cooldown_ =get_parameter("odin_restart_cooldown").as_double(); + odin_autostart_ = get_parameter("odin_autostart").as_bool(); + odin_watchdog_timeout_ = get_parameter("odin_watchdog_timeout").as_double(); + odin_restart_cooldown_ = get_parameter("odin_restart_cooldown").as_double(); - last_odom_arrival_ns_.store(steady_now_ns(),std::memory_order_relaxed); - if(odin_autostart_) - odin_manager_thread_ =std::jthread{[this](std::stop_token st){odin_manager_loop(st);}}; + last_odom_arrival_ns_.store(steady_now_ns(), std::memory_order_relaxed); + if (odin_autostart_) + odin_manager_thread_ = + std::jthread{[this](std::stop_token st) { odin_manager_loop(st); }}; } Px4VisionBridge(const Px4VisionBridge&) = delete; Px4VisionBridge& operator=(const Px4VisionBridge&) = delete; void update() override { - send_heartbeat_if_due(); //1hz HEARTBEAT - send_vision_if_pending(); //消费回调缓存的最新位姿,变换并发送 + send_heartbeat_if_due(); // 1hz HEARTBEAT + send_vision_if_pending(); // 消费回调缓存的最新位姿,变换并发送 } private: - struct OdomSample { rclcpp::Time stamp{0, 0, RCL_ROS_TIME}; Eigen::Vector3d position{Eigen::Vector3d::Zero()}; Eigen::Quaterniond orientation{Eigen::Quaterniond::Identity()}; }; - void odometry_callback(const nav_msgs::msg::Odometry::UniquePtr msg){ - last_odom_arrival_ns_.store(steady_now_ns(),std::memory_order_relaxed); + void odometry_callback(const nav_msgs::msg::Odometry::UniquePtr msg) { + last_odom_arrival_ns_.store(steady_now_ns(), std::memory_order_relaxed); - const auto& p =msg->pose.pose.position; - const auto& o =msg->pose.pose.orientation; + const auto& p = msg->pose.pose.position; + const auto& o = msg->pose.pose.orientation; const std::scoped_lock lock{odom_mutex_}; - pending_odom_.stamp =rclcpp::Time{msg->header.stamp}; - pending_odom_.position =Eigen::Vector3d{p.x,p.y,p.z}; - pending_odom_.orientation =Eigen::Quaterniond{o.w,o.x,o.y,o.z}; - odom_pending_ =true; + pending_odom_.stamp = rclcpp::Time{msg->header.stamp}; + pending_odom_.position = Eigen::Vector3d{p.x, p.y, p.z}; + pending_odom_.orientation = Eigen::Quaterniond{o.w, o.x, o.y, o.z}; + odom_pending_ = true; } - void send_vision_if_pending(){ - if((now() -last_send_time_)(sample.position.y()); - const auto y =static_cast(sample.position.x()); - const auto z =static_cast(-sample.position.z()); + const auto x = static_cast(sample.position.y()); + const auto y = static_cast(sample.position.x()); + const auto z = static_cast(-sample.position.z()); - const Eigen::Quaterniond q_enu_flu =sample.orientation *q_mount_; + const Eigen::Quaterniond q_enu_flu = sample.orientation * q_mount_; - Eigen::Quaterniond q_ned_frd =kNedEnuQ *(q_enu_flu*kAircraftBaselinkQ); + Eigen::Quaterniond q_ned_frd = kNedEnuQ * (q_enu_flu * kAircraftBaselinkQ); q_ned_frd.normalize(); - double roll,pitch,yaw; - quaternion_to_rpy_zyx(q_ned_frd,roll,pitch,yaw); + double roll, pitch, yaw; + quaternion_to_rpy_zyx(q_ned_frd, roll, pitch, yaw); - const auto usec = static_cast(sample.stamp.nanoseconds())/1000ull; + const auto usec = static_cast(sample.stamp.nanoseconds()) / 1000ull; float covariance[21]; - std::fill(std::begin(covariance),std::end(covariance),NAN); + std::fill(std::begin(covariance), std::end(covariance), NAN); mavlink_message_t msg_mavlink; mavlink_msg_vision_position_estimate_pack( - system_id_, - component_id_, - &msg_mavlink, - usec, - x,y,z, - static_cast(roll), - static_cast(pitch), - static_cast(yaw), - covariance, - /*reset_counter=*/0 - ); + system_id_, component_id_, &msg_mavlink, usec, x, y, z, static_cast(roll), + static_cast(pitch), static_cast(yaw), covariance, + /*reset_counter=*/0); send_message(msg_mavlink); } - void send_heartbeat_if_due(){ - if((now()-last_heartbeat_send_).seconds()<1.0) + void send_heartbeat_if_due() { + if ((now() - last_heartbeat_send_).seconds() < 1.0) return; - last_heartbeat_send_ =now(); + last_heartbeat_send_ = now(); mavlink_message_t msg_mavlink; mavlink_msg_heartbeat_pack( - system_id_, - component_id_, - &msg_mavlink, - MAV_TYPE_ONBOARD_CONTROLLER, + system_id_, component_id_, &msg_mavlink, MAV_TYPE_ONBOARD_CONTROLLER, MAV_AUTOPILOT_INVALID, /*base_mode=*/0, - /*custom_mode=*/0, - MAV_STATE_ACTIVE - ); + /*custom_mode=*/0, MAV_STATE_ACTIVE); send_message(msg_mavlink); } - void send_message(const mavlink_message_t& msg){ - if(!px4_serial_.active())[[unlikely]] + void send_message(const mavlink_message_t& msg) { + if (!px4_serial_.active()) [[unlikely]] return; uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; - const uint16_t len =mavlink_msg_to_send_buffer(buffer,&msg); + const uint16_t len = mavlink_msg_to_send_buffer(buffer, &msg); px4_serial_->write(reinterpret_cast(buffer), len); } - static int64_t steady_now_ns(){ + static int64_t steady_now_ns() { return std::chrono::duration_cast( std::chrono::steady_clock::now().time_since_epoch()) .count(); } // 独立线程看门狗：断流超时且过了冷却期就拉起 tmux-launch.sh； - void odin_manager_loop(std::stop_token st){ - const std::string cmd = - "\""+ament_index_cpp::get_package_prefix("odin_ros_driver") - +"/lib/odin_ros_driver/tmux-launch.sh\""; + void odin_manager_loop(std::stop_token st) { + const std::string cmd = "\"" + ament_index_cpp::get_package_prefix("odin_ros_driver") + + "/lib/odin_ros_driver/tmux-launch.sh\""; - const auto timeout_ns =static_cast(odin_watchdog_timeout_*1e9); - const auto cooldown_ns =static_cast(odin_restart_cooldown_*1e9); - int64_t last_launch_ns =steady_now_ns()-cooldown_ns; //首次断流即可拉起 - bool online =true; //启动宽限：先假定在线，超时才告警 + const auto timeout_ns = static_cast(odin_watchdog_timeout_ * 1e9); + const auto cooldown_ns = static_cast(odin_restart_cooldown_ * 1e9); + int64_t last_launch_ns = steady_now_ns() - cooldown_ns; // 首次断流即可拉起 + bool online = true; // 启动宽限：先假定在线，超时才告警 // 锁仅为 wait_for 语义存在 request_stop 会直接唤醒等待 std::mutex sleep_mutex; std::condition_variable_any sleep_cv; std::unique_lock lock{sleep_mutex}; - while(!sleep_cv.wait_for( - lock,st,std::chrono::seconds{1},[&]{return st.stop_requested();})){ + while (!sleep_cv.wait_for( + lock, st, std::chrono::seconds{1}, [&] { return st.stop_requested(); })) { - const auto now_ns =steady_now_ns(); + const auto now_ns = steady_now_ns(); const bool fresh = - (now_ns-last_odom_arrival_ns_.load(std::memory_order_relaxed))cooldown_ns){ - last_launch_ns =now_ns; + if (!online && (now_ns - last_launch_ns) > cooldown_ns) { + last_launch_ns = now_ns; // （脚本内部的清杀/启动轮询）存在阻塞，单独在本线程做 - if(std::system(cmd.c_str())!=0) - RCLCPP_ERROR(logger_,"Odin1 launch script failed"); + if (std::system(cmd.c_str()) != 0) + RCLCPP_ERROR(logger_, "Odin1 launch script failed"); } } } - static Eigen::Quaterniond quaternion_from_rpy_zyx(double roll, double pitch, double yaw){ + static Eigen::Quaterniond quaternion_from_rpy_zyx(double roll, double pitch, double yaw) { return Eigen::Quaterniond{ - - Eigen::AngleAxisd{yaw, Eigen::Vector3d::UnitZ()} * - Eigen::AngleAxisd{pitch, Eigen::Vector3d::UnitY()} * - Eigen::AngleAxisd{roll, Eigen::Vector3d::UnitX()} - }; - } - - static void quaternion_to_rpy_zyx(const Eigen::Quaterniond& q, double& roll,double& pitch, double& yaw){ - const double w =q.w(),x =q.x(),y =q.y(),z =q.z(); - const double m20 =2.0*(x*z - w*y); // -sin(pitch) - const double m21 =2.0*(y*z + w*x); - const double m22 =1.0 - 2.0*(x*x + y*y); - const double sin_pitch =std::clamp(-m20, -1.0, 1.0); - const double cos_pitch =std::sqrt(m21*m21 + m22*m22); - - if(cos_pitch > 1e-9){ + + Eigen::AngleAxisd{yaw, Eigen::Vector3d::UnitZ()} + * Eigen::AngleAxisd{pitch, Eigen::Vector3d::UnitY()} + * Eigen::AngleAxisd{roll, Eigen::Vector3d::UnitX()}}; + } + + static void quaternion_to_rpy_zyx( + const Eigen::Quaterniond& q, double& roll, double& pitch, double& yaw) { + const double w = q.w(), x = q.x(), y = q.y(), z = q.z(); + const double m20 = 2.0 * (x * z - w * y); // -sin(pitch) + const double m21 = 2.0 * (y * z + w * x); + const double m22 = 1.0 - 2.0 * (x * x + y * y); + const double sin_pitch = std::clamp(-m20, -1.0, 1.0); + const double cos_pitch = std::sqrt(m21 * m21 + m22 * m22); + + if (cos_pitch > 1e-9) { pitch = std::atan2(sin_pitch, cos_pitch); - roll = std::atan2(m21, m22); - yaw = std::atan2(2.0*(x*y + w*z), 1.0 - 2.0*(y*y + z*z)); + roll = std::atan2(m21, m22); + yaw = std::atan2(2.0 * (x * y + w * z), 1.0 - 2.0 * (y * y + z * z)); return; } // 万向节锁 (pitch=±π/2)：roll/yaw 只剩组合自由度可观测， // 约定 roll=0、组合角全部归 yaw，保证三元组重建仍是原旋转 - const double m11 =1.0 - 2.0*(x*x + z*z); - const double m12 =2.0*(y*z - w*x); - roll = 0.0; + const double m11 = 1.0 - 2.0 * (x * x + z * z); + const double m12 = 2.0 * (y * z - w * x); + roll = 0.0; pitch = (sin_pitch > 0.0) ? M_PI_2 : -M_PI_2; - yaw = (sin_pitch > 0.0) ? std::atan2(m12, m11) : std::atan2(-m12, m11); + yaw = (sin_pitch > 0.0) ? std::atan2(m12, m11) : std::atan2(-m12, m11); } - //ENU->NED - static inline const Eigen::Quaterniond& kNedEnuQ{ - 0.0,0.70710678118655,0.70710678118655,0.0 - }; - //FLU->FRD - static inline const Eigen::Quaterniond& kAircraftBaselinkQ{ - 0.0,1.0,0.0,0.0 - }; + // ENU->NED + static inline const Eigen::Quaterniond& kNedEnuQ{0.0, 0.70710678118655, 0.70710678118655, 0.0}; + // FLU->FRD + static inline const Eigen::Quaterniond& kAircraftBaselinkQ{0.0, 1.0, 0.0, 0.0}; rclcpp::Logger logger_; InputInterface px4_serial_; @@ -265,20 +244,20 @@ class Px4VisionBridge // 回调线程(rclcpp spin)与 update() 线程(executor)间的最新位姿交接 std::mutex odom_mutex_; OdomSample pending_odom_; - bool odom_pending_ =false; + bool odom_pending_ = false; std::string source_topic_; - uint8_t system_id_; - uint8_t component_id_; - double max_send_rate_hz_; - Eigen::Quaterniond q_mount_; + uint8_t system_id_; + uint8_t component_id_; + double max_send_rate_hz_; + Eigen::Quaterniond q_mount_; - rclcpp::Duration min_send_interval_ =rclcpp::Duration::from_seconds(0.02); - rclcpp::Time last_send_time_ =rclcpp::Time(0, 0, RCL_ROS_TIME); - rclcpp::Time last_heartbeat_send_{0,0,RCL_ROS_TIME}; + rclcpp::Duration min_send_interval_ = rclcpp::Duration::from_seconds(0.02); + rclcpp::Time last_send_time_ = rclcpp::Time(0, 0, RCL_ROS_TIME); + rclcpp::Time last_heartbeat_send_{0, 0, RCL_ROS_TIME}; // Odin1 拉起与看门狗 - bool odin_autostart_; + bool odin_autostart_; double odin_watchdog_timeout_; double odin_restart_cooldown_; std::atomic last_odom_arrival_ns_{0}; @@ -286,7 +265,7 @@ class Px4VisionBridge std::jthread odin_manager_thread_; }; -}// namespace rmcs_core::hardware +} // namespace rmcs_core::hardware #include PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::Px4VisionBridge, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp b/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp index 65135426..75480707 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp @@ -270,8 +270,7 @@ class Flight std::unique_ptr remote_control_; // 等价于旧 Bmi088 的坐标映射 (x, y, z) -> (y, z, x)：body = body_to_sensor^T * sensor device::Bmi088Ekf bmi088_{device::Bmi088Ekf::Config{ - .body_to_sensor = - (Eigen::Matrix3d{} << 0, 0, 1, 1, 0, 0, 0, 1, 0).finished(), + .body_to_sensor = (Eigen::Matrix3d{} << 0, 0, 1, 1, 0, 0, 0, 1, 0).finished(), }}; device::BoardClockLifter board_clock_lifter_; From 37fdc52231b72f95876b2764216ba6e8d8e4b401 Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Mon, 27 Jul 2026 23:28:13 +0800 Subject: [PATCH 09/11] chore: Remove unused cmake config --- rmcs_ws/src/rmcs_core/CMakeLists.txt | 2 -- 1 file changed, 2 deletions(-) diff --git a/rmcs_ws/src/rmcs_core/CMakeLists.txt b/rmcs_ws/src/rmcs_core/CMakeLists.txt index 234c0eeb..0ae6350a 100644 --- a/rmcs_ws/src/rmcs_core/CMakeLists.txt +++ b/rmcs_ws/src/rmcs_core/CMakeLists.txt @@ -13,7 +13,6 @@ endif() find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() -find_package(mavlink REQUIRED) include(FetchContent) set(BUILD_STATIC_LIBRMCS ON CACHE BOOL "Build static librmcs SDK" FORCE) @@ -34,7 +33,6 @@ ament_auto_add_library( ${PROJECT_NAME} SHARED ${PROJECT_SOURCE} ) -target_include_directories(${PROJECT_NAME} PRIVATE ${mavlink_INCLUDE_DIRS}) include_directories(${PROJECT_SOURCE_DIR}/include) include_directories(${PROJECT_SOURCE_DIR}/src) From e379d46f1a7e98f358bc433efc5e21493de2b9c8 Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Mon, 27 Jul 2026 23:38:37 +0800 Subject: [PATCH 10/11] chore: Clean up code --- .../controller/flight/px4_vision_bridge.cpp | 21 +++++++------------ 1 file changed, 8 insertions(+), 13 deletions(-) diff --git a/rmcs_ws/src/rmcs_core/src/controller/flight/px4_vision_bridge.cpp b/rmcs_ws/src/rmcs_core/src/controller/flight/px4_vision_bridge.cpp index e2a2b119..40c64049 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/flight/px4_vision_bridge.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/flight/px4_vision_bridge.cpp @@ -1,6 +1,3 @@ -#include "nav_msgs/msg/odometry.hpp" -#include - #include #include #include @@ -10,15 +7,14 @@ #include #include #include -#include #include #include #include #include +#include #include #include -#include #include #include @@ -60,7 +56,7 @@ class Px4VisionBridge last_odom_arrival_ns_.store(steady_now_ns(), std::memory_order_relaxed); if (odin_autostart_) odin_manager_thread_ = - std::jthread{[this](std::stop_token st) { odin_manager_loop(st); }}; + std::jthread{[this](const std::stop_token& st) { odin_manager_loop(st); }}; } Px4VisionBridge(const Px4VisionBridge&) = delete; @@ -159,14 +155,14 @@ class Px4VisionBridge } // 独立线程看门狗：断流超时且过了冷却期就拉起 tmux-launch.sh； - void odin_manager_loop(std::stop_token st) { + void odin_manager_loop(const std::stop_token& st) { const std::string cmd = "\"" + ament_index_cpp::get_package_prefix("odin_ros_driver") + "/lib/odin_ros_driver/tmux-launch.sh\""; const auto timeout_ns = static_cast(odin_watchdog_timeout_ * 1e9); const auto cooldown_ns = static_cast(odin_restart_cooldown_ * 1e9); int64_t last_launch_ns = steady_now_ns() - cooldown_ns; // 首次断流即可拉起 - bool online = true; // 启动宽限：先假定在线，超时才告警 + bool online = true; // 启动宽限：先假定在线，超时才告警 // 锁仅为 wait_for 语义存在 request_stop 会直接唤醒等待 std::mutex sleep_mutex; @@ -201,9 +197,7 @@ class Px4VisionBridge } static Eigen::Quaterniond quaternion_from_rpy_zyx(double roll, double pitch, double yaw) { - return Eigen::Quaterniond{ - Eigen::AngleAxisd{yaw, Eigen::Vector3d::UnitZ()} * Eigen::AngleAxisd{pitch, Eigen::Vector3d::UnitY()} * Eigen::AngleAxisd{roll, Eigen::Vector3d::UnitX()}}; @@ -232,10 +226,11 @@ class Px4VisionBridge pitch = (sin_pitch > 0.0) ? M_PI_2 : -M_PI_2; yaw = (sin_pitch > 0.0) ? std::atan2(m12, m11) : std::atan2(-m12, m11); } + // ENU->NED - static inline const Eigen::Quaterniond& kNedEnuQ{0.0, 0.70710678118655, 0.70710678118655, 0.0}; + static inline const Eigen::Quaterniond kNedEnuQ{0.0, 0.70710678118655, 0.70710678118655, 0.0}; // FLU->FRD - static inline const Eigen::Quaterniond& kAircraftBaselinkQ{0.0, 1.0, 0.0, 0.0}; + static inline const Eigen::Quaterniond kAircraftBaselinkQ{0.0, 1.0, 0.0, 0.0}; rclcpp::Logger logger_; InputInterface px4_serial_; @@ -268,4 +263,4 @@ class Px4VisionBridge } // namespace rmcs_core::hardware #include -PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::Px4VisionBridge, rmcs_executor::Component) \ No newline at end of file +PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::Px4VisionBridge, rmcs_executor::Component) From 8d6ad420847a5ec5a4caba8c89f1e568d105ef35 Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Mon, 27 Jul 2026 23:44:17 +0800 Subject: [PATCH 11/11] feat: Add mavlink to dockerfile --- Dockerfile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Dockerfile b/Dockerfile index cf973436..f53de75e 100644 --- a/Dockerfile +++ b/Dockerfile @@ -38,7 +38,7 @@ RUN apt-get update && apt-get install -y --no-install-recommends \ libceres-dev \ ros-$ROS_DISTRO-rviz2 ros-$ROS_DISTRO-foxglove-bridge \ ros-$ROS_DISTRO-pcl-ros ros-$ROS_DISTRO-pcl-conversions ros-$ROS_DISTRO-pcl-msgs \ - ros-$ROS_DISTRO-navigation2 ros-$ROS_DISTRO-nav2-msgs \ + ros-$ROS_DISTRO-navigation2 ros-$ROS_DISTRO-nav2-msgs ros-$ROS_DISTRO-mavlink \ lua5.4 liblua5.4-0 liblua5.4-dev && \ apt-get clean && \ rm -rf /var/lib/apt/lists/* /tmp/*