From 763e1e2d599adafd6326c8d57c0b0e54a64896db Mon Sep 17 00:00:00 2001 From: Embers-of-the-Fire Date: Fri, 13 Mar 2026 10:57:15 +0800 Subject: [PATCH 01/23] feat: add px4 and external sensor dependencies --- .gitmodules | 15 +++++++++++++++ rmcs_ws/src/micro-xrce-dds-agent | 1 + rmcs_ws/src/odin_ros_driver | 1 + rmcs_ws/src/px4-ros2-interface-lib | 1 + rmcs_ws/src/px4_msgs | 1 + rmcs_ws/src/px4_ros_com | 1 + 6 files changed, 20 insertions(+) create mode 160000 rmcs_ws/src/micro-xrce-dds-agent create mode 160000 rmcs_ws/src/odin_ros_driver create mode 160000 rmcs_ws/src/px4-ros2-interface-lib create mode 160000 rmcs_ws/src/px4_msgs create mode 160000 rmcs_ws/src/px4_ros_com diff --git a/.gitmodules b/.gitmodules index d6298295..e58aef1c 100644 --- a/.gitmodules +++ b/.gitmodules @@ -7,3 +7,18 @@ [submodule "rmcs_ws/src/serial"] path = rmcs_ws/src/serial url = https://github.com/Alliance-Algorithm/ros2-serial.git +[submodule "rmcs_ws/src/micro-xrce-dds-agent"] + path = rmcs_ws/src/micro-xrce-dds-agent + url = https://github.com/eProsima/Micro-XRCE-DDS-Agent.git +[submodule "rmcs_ws/src/px4_msgs"] + path = rmcs_ws/src/px4_msgs + url = https://github.com/PX4/px4_msgs.git +[submodule "rmcs_ws/src/px4_ros_com"] + path = rmcs_ws/src/px4_ros_com + url = https://github.com/PX4/px4_ros_com.git +[submodule "rmcs_ws/src/px4-ros2-interface-lib"] + path = rmcs_ws/src/px4-ros2-interface-lib + url = https://github.com/Auterion/px4-ros2-interface-lib +[submodule "rmcs_ws/src/odin_ros_driver"] + path = rmcs_ws/src/odin_ros_driver + url = https://github.com/manifoldsdk/odin_ros_driver.git diff --git a/rmcs_ws/src/micro-xrce-dds-agent b/rmcs_ws/src/micro-xrce-dds-agent new file mode 160000 index 00000000..155cfaaf --- /dev/null +++ b/rmcs_ws/src/micro-xrce-dds-agent @@ -0,0 +1 @@ +Subproject commit 155cfaaf8b7abac2e85d4a62d3649b09ace0be55 diff --git a/rmcs_ws/src/odin_ros_driver b/rmcs_ws/src/odin_ros_driver new file mode 160000 index 00000000..0c3a02a2 --- /dev/null +++ b/rmcs_ws/src/odin_ros_driver @@ -0,0 +1 @@ +Subproject commit 0c3a02a2b2260be424354faf9492ec9c934664c6 diff --git a/rmcs_ws/src/px4-ros2-interface-lib b/rmcs_ws/src/px4-ros2-interface-lib new file mode 160000 index 00000000..4fb88a1b --- /dev/null +++ b/rmcs_ws/src/px4-ros2-interface-lib @@ -0,0 +1 @@ +Subproject commit 4fb88a1b3da69b417ceb1e42d4870a33e58938c2 diff --git a/rmcs_ws/src/px4_msgs b/rmcs_ws/src/px4_msgs new file mode 160000 index 00000000..eb4edb4f --- /dev/null +++ b/rmcs_ws/src/px4_msgs @@ -0,0 +1 @@ +Subproject commit eb4edb4f7376e9a4732498a4e8b637c3d557a964 diff --git a/rmcs_ws/src/px4_ros_com b/rmcs_ws/src/px4_ros_com new file mode 160000 index 00000000..86e9aeb2 --- /dev/null +++ b/rmcs_ws/src/px4_ros_com @@ -0,0 +1 @@ +Subproject commit 86e9aeb20e55a4673fa8a9f1c29ea06a6c5ad1af From 1029c9bef68515be9cb4b1c8241997edf105276a Mon Sep 17 00:00:00 2001 From: Embers-of-the-Fire Date: Fri, 13 Mar 2026 12:19:53 +0800 Subject: [PATCH 02/23] feat!: add flight control code --- .script/build-rmcs | 6 + PREREQUISITE.md | 42 ++++ rmcs_ws/src/rmcs_bringup/config/flight.yaml | 9 + rmcs_ws/src/rmcs_core/CMakeLists.txt | 29 ++- rmcs_ws/src/rmcs_core/package.xml | 2 + rmcs_ws/src/rmcs_core/plugins.xml | 1 + rmcs_ws/src/rmcs_core/src/hardware/flight.cpp | 234 ++++++++++++++++++ 7 files changed, 322 insertions(+), 1 deletion(-) create mode 100644 PREREQUISITE.md create mode 100644 rmcs_ws/src/rmcs_bringup/config/flight.yaml create mode 100644 rmcs_ws/src/rmcs_core/src/hardware/flight.cpp diff --git a/.script/build-rmcs b/.script/build-rmcs index 2c24a7ed..8769692f 100755 --- a/.script/build-rmcs +++ b/.script/build-rmcs @@ -56,5 +56,11 @@ cmake_args=( "${cmake_toolchain_args[@]}" ) +if [[ "${ROS_VERSION}" == "2" ]]; then + cmake_args+=("-DBUILD_SYSTEM=ROS2") +elif [[ "${ROS_VERSION}" == "1" ]]; then + cmake_args+=("-DBUILD_SYSTEM=ROS1") +fi + CLICOLOR_FORCE=1 NINJA_STATUS="" \ colcon build "${colcon_args[@]}" "$@" --cmake-args "${cmake_args[@]}" diff --git a/PREREQUISITE.md b/PREREQUISITE.md new file mode 100644 index 00000000..c4b9ee7d --- /dev/null +++ b/PREREQUISITE.md @@ -0,0 +1,42 @@ +# Prerequisite + +To build/develop the flight's branch, you have to do the following tasks as they're not included either in the image nor in package managers. + +## Dependencies for Uxrce DDS Client + +The `micro-xrce-dds-agent` requires Fast-DDS to be installed. +That package is not included in the official APT package source, so we have to compile it manually. + +First, install ASIO. + +```bash +sudo apt-get install libasio-dev +``` + +Then, compile the library from source. + +```bash +mkdir -p /tmp/fastdds +git clone https://github.com/eProsima/Fast-CDR.git +mkdir Fast-CDR/build && cd Fast-CDR/build +cmake .. -DCMAKE_INSTALL_PREFIX=/workspaces/RMCS/rmcs_ws/install -DBUILD_SHARED_LIBS=ON +cmake --build . --target install -j +cd /tmp/fastdds && git clone https://github.com/eProsima/Fast-DDS.git +mkdir Fast-DDS/build && cd Fast-DDS/build +cmake .. -DCMAKE_INSTALL_PREFIX=/workspaces/RMCS/rmcs_ws/install -DBUILD_SHARED_LIBS=ON +cmake --build . --target install -j +cd / && rm -rf /tmp/fastdds +``` + +This would roughly take ~50s to compile for a 9950x cpu using 16 threads. + +## Dependencies for Odin1 Driver + +Odin1 driver requires cv-bridge which is no longer used by the main upstream. + +```bash +sudo apt-get install ros-jazzy-cv-bridge +``` + +Note that you might have to modify Odin1's source code to compile it. +Some of the code refer to `` instead of `.hpp`. 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..61da7c89 --- /dev/null +++ b/rmcs_ws/src/rmcs_bringup/config/flight.yaml @@ -0,0 +1,9 @@ +rmcs_executor: + ros__parameters: + update_rate: 1000.0 + components: + - rmcs_core::hardware::Flight -> flight_hardware + +flight_hardware: + ros__parameters: + foo: bar diff --git a/rmcs_ws/src/rmcs_core/CMakeLists.txt b/rmcs_ws/src/rmcs_core/CMakeLists.txt index dd3c8579..d0594285 100644 --- a/rmcs_ws/src/rmcs_core/CMakeLists.txt +++ b/rmcs_ws/src/rmcs_core/CMakeLists.txt @@ -35,7 +35,34 @@ ament_auto_add_library( include_directories(${PROJECT_SOURCE_DIR}/include) include_directories(${PROJECT_SOURCE_DIR}/src) -target_link_libraries(${PROJECT_NAME} librmcs-sdk) + +set(ODIN_DRIVER_DIR "${PROJECT_SOURCE_DIR}/../odin_ros_driver") +include_directories(SYSTEM "${ODIN_DRIVER_DIR}/include") + +execute_process( + COMMAND uname -m + OUTPUT_VARIABLE _HOST_ARCH + OUTPUT_STRIP_TRAILING_WHITESPACE +) + +if(_HOST_ARCH MATCHES "arm|aarch64") + set(ODIN_HOST_API_LIB "${ODIN_DRIVER_DIR}/lib/liblydHostApi_arm.a") +else() + set(ODIN_HOST_API_LIB "${ODIN_DRIVER_DIR}/lib/liblydHostApi_amd.a") +endif() + +if(NOT EXISTS "${ODIN_HOST_API_LIB}") + message(FATAL_ERROR "Missing Odin Host API library: ${ODIN_HOST_API_LIB}") +endif() + +target_link_libraries( + ${PROJECT_NAME} + librmcs-sdk + usb-1.0 + ${ODIN_HOST_API_LIB} + pthread + ${CMAKE_DL_LIBS} +) pluginlib_export_plugin_description_file(rmcs_executor plugins.xml) diff --git a/rmcs_ws/src/rmcs_core/package.xml b/rmcs_ws/src/rmcs_core/package.xml index 41074552..c16d6cee 100644 --- a/rmcs_ws/src/rmcs_core/package.xml +++ b/rmcs_ws/src/rmcs_core/package.xml @@ -19,6 +19,8 @@ rmcs_msgs rmcs_executor rmcs_description + px4_msgs + px4_ros2_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 f7847151..03965995 100644 --- a/rmcs_ws/src/rmcs_core/plugins.xml +++ b/rmcs_ws/src/rmcs_core/plugins.xml @@ -1,4 +1,5 @@ + 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..d2d915bc --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp @@ -0,0 +1,234 @@ +#include +#include +#include + +#include +#include +#include + +// Odin LiDAR C API (no ROS layer) +#include +#include + +namespace rmcs_core::hardware { + +class Flight + : public rmcs_executor::Component + , public rclcpp::Node { +public: + Flight() + : Node{ + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} + , local_position_interface_{*this, px4_ros2::PoseFrame::LocalNED, px4_ros2::VelocityFrame::LocalNED} + , logger_(get_logger()) { + g_instance_ = this; + + if (!local_position_interface_.doRegister()) { + throw std::runtime_error("Failed to register LocalPositionMeasurementInterface"); + } + RCLCPP_INFO(logger_, "LocalPositionMeasurementInterface registered successfully."); + + if (lidar_system_init(&Flight::device_event_callback)) { + throw std::runtime_error("lidar_system_init failed"); + } + RCLCPP_INFO(logger_, "Odin lidar system initialized."); + } + + ~Flight() { + if (odin_device_) { + lidar_stop_stream(odin_device_, LIDAR_MODE_SLAM); + lidar_unregister_stream_callback(odin_device_); + lidar_close_device(odin_device_); + lidar_destory_device(odin_device_); + } + lidar_system_deinit(); + } + + void update() override { + OdomSnapshot snap; + { + std::lock_guard lock(odom_mutex_); + if (!odom_ready_) + return; + snap = latest_odom_; + odom_ready_ = false; + } + + px4_ros2::LocalPositionMeasurement meas{}; + meas.timestamp_sample = get_clock()->now(); + + meas.position_xy = Eigen::Vector2f{snap.pos_x, snap.pos_y}; + meas.position_xy_variance = Eigen::Vector2f{ + snap.cov_xx > 0.f ? snap.cov_xx : 0.1f, + snap.cov_yy > 0.f ? snap.cov_yy : 0.1f}; + + meas.position_z = snap.pos_z; + meas.position_z_variance = snap.cov_zz > 0.f ? snap.cov_zz : 0.1f; + + meas.velocity_xy = Eigen::Vector2f{snap.vel_x, snap.vel_y}; + meas.velocity_xy_variance = Eigen::Vector2f{0.1f, 0.1f}; + + // Quaternion from odin is (x, y, z, w); Eigen ctor is (w, x, y, z) + meas.attitude_quaternion = + Eigen::Quaternionf{snap.q_w, snap.q_x, snap.q_y, snap.q_z}; + meas.attitude_variance = Eigen::Vector3f{0.05f, 0.05f, 0.05f}; + + try { + local_position_interface_.update(meas); + } catch (const px4_ros2::NavigationInterfaceInvalidArgument& e) { + RCLCPP_ERROR_THROTTLE(logger_, *get_clock(), 1000, "Navigation update error: %s", e.what()); + } + } + +private: + // ---- Shared odometry snapshot (filled by SDK callback, read by update()) ---- + struct OdomSnapshot { + float pos_x{}, pos_y{}, pos_z{}; + float vel_x{}, vel_y{}; + float q_x{}, q_y{}, q_z{}, q_w{1.f}; + float cov_xx{}, cov_yy{}, cov_zz{}; + }; + + std::mutex odom_mutex_; + OdomSnapshot latest_odom_; + bool odom_ready_{false}; + + // ---- Device handle (set once when device connects) ---- + device_handle odin_device_{nullptr}; + + // ---- Static callbacks (SDK uses plain C function pointers) ---- + + // Called by SDK when device connects or disconnects. + static void device_event_callback(const lidar_device_info_t* device_info, bool attach) { + if (!attach || !g_instance_) + return; + g_instance_->on_device_attach(device_info); + } + + // Called by SDK for every incoming data frame. + static void data_callback(const lidar_data_t* data, void* /*user_data*/) { + if (!g_instance_ || data->type != LIDAR_DT_SLAM_ODOMETRY) + return; + g_instance_->on_odom_frame(&data->stream); + } + + // ---- Instance-level handlers ---- + + void on_device_attach(const lidar_device_info_t* device_info) { + RCLCPP_INFO(logger_, "Odin device attached, setting up SLAM odometry stream."); + + lidar_device_info_t info = *device_info; // mutable copy required by API + device_handle dev = nullptr; + if (lidar_create_device(&info, &dev)) { + RCLCPP_ERROR(logger_, "lidar_create_device failed"); + return; + } + + if (device_info->initial_state == LIDAR_DEVICE_NOT_INITIALIZED) { + if (lidar_open_device(dev)) { + RCLCPP_ERROR(logger_, "lidar_open_device failed"); + lidar_destory_device(dev); + return; + } + if (lidar_set_mode(dev, LIDAR_MODE_SLAM)) { + RCLCPP_ERROR(logger_, "lidar_set_mode(SLAM) failed"); + lidar_close_device(dev); + lidar_destory_device(dev); + return; + } + } + + lidar_data_callback_info_t cb_info{}; + cb_info.data_callback = &Flight::data_callback; + cb_info.user_data = nullptr; + if (lidar_register_stream_callback(dev, cb_info)) { + RCLCPP_ERROR(logger_, "lidar_register_stream_callback failed"); + lidar_close_device(dev); + lidar_destory_device(dev); + return; + } + + uint32_t dtof_subframe_odr = 0; + if (lidar_start_stream(dev, LIDAR_MODE_SLAM, dtof_subframe_odr)) { + RCLCPP_ERROR(logger_, "lidar_start_stream failed"); + lidar_close_device(dev); + lidar_destory_device(dev); + return; + } + + lidar_activate_stream_type(dev, LIDAR_DT_SLAM_ODOMETRY); + + odin_device_ = dev; + RCLCPP_INFO(logger_, "Odin SLAM odometry stream active."); + } + + void on_odom_frame(const capture_Image_List_t* stream) { + const uint32_t data_len = stream->imageList[0].length; + const void* addr = stream->imageList[0].pAddr; + + OdomSnapshot snap{}; + + if (data_len == sizeof(ros_odom_convert_complete_t)) { + // Full odometry: position + velocity + covariance + const auto* d = static_cast(addr); + + // pos encoded as fixed-point ×1e6 + snap.pos_x = static_cast(d->pos[0]) / 1e6f; + snap.pos_y = static_cast(d->pos[1]) / 1e6f; + snap.pos_z = static_cast(d->pos[2]) / 1e6f; + + // orientation quaternion (x, y, z, w) encoded ×1e6 + snap.q_x = static_cast(d->orient[0]) / 1e6f; + snap.q_y = static_cast(d->orient[1]) / 1e6f; + snap.q_z = static_cast(d->orient[2]) / 1e6f; + snap.q_w = static_cast(d->orient[3]) / 1e6f; + + // linear velocity encoded ×1e6 + snap.vel_x = static_cast(d->linear_velocity[0]) / 1e6f; + snap.vel_y = static_cast(d->linear_velocity[1]) / 1e6f; + + // pose covariance diagonal (pose_cov[0]=xx, pose_cov[4]=yy, pose_cov[8]=zz) + snap.cov_xx = static_cast(d->pose_cov[0]); + snap.cov_yy = static_cast(d->pose_cov[4]); + snap.cov_zz = static_cast(d->pose_cov[8]); + + } else if (data_len == sizeof(ros2_odom_convert_t)) { + // Compact odometry: position + orientation only + const auto* d = static_cast(addr); + + snap.pos_x = static_cast(d->pos[0]) / 1e6f; + snap.pos_y = static_cast(d->pos[1]) / 1e6f; + snap.pos_z = static_cast(d->pos[2]) / 1e6f; + + snap.q_x = static_cast(d->orient[0]) / 1e6f; + snap.q_y = static_cast(d->orient[1]) / 1e6f; + snap.q_z = static_cast(d->orient[2]) / 1e6f; + snap.q_w = static_cast(d->orient[3]) / 1e6f; + } else { + return; // Unknown payload; skip + } + + { + std::lock_guard lock(odom_mutex_); + latest_odom_ = snap; + odom_ready_ = true; + } + } + + // ---- Members ---- + px4_ros2::LocalPositionMeasurementInterface local_position_interface_; + rclcpp::Logger logger_; + + // Global pointer used by static C callbacks to reach the active instance. + // Only one Flight component is expected per process. + static Flight* g_instance_; +}; + +// Static member definition +Flight* Flight::g_instance_ = nullptr; + +} // namespace rmcs_core::hardware + +#include +PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::Flight, rmcs_executor::Component) From f5e77d79f66922581b820a18e501a4c4fd91301a Mon Sep 17 00:00:00 2001 From: Embers-of-the-Fire Date: Fri, 13 Mar 2026 12:28:01 +0800 Subject: [PATCH 03/23] docs: update uxrce dds client docs --- PREREQUISITE.md | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/PREREQUISITE.md b/PREREQUISITE.md index c4b9ee7d..df210e68 100644 --- a/PREREQUISITE.md +++ b/PREREQUISITE.md @@ -30,6 +30,14 @@ cd / && rm -rf /tmp/fastdds This would roughly take ~50s to compile for a 9950x cpu using 16 threads. +To run the uxrce dds client: + +```bash +sudo MicroXRCEAgent serial --dev /dev/ttyS0 -b 921600 +``` + +Note that the baudrate should be the same as the one configured in the flight controller. + ## Dependencies for Odin1 Driver Odin1 driver requires cv-bridge which is no longer used by the main upstream. From 34963fa472b45278f479d9dae67bca23960399bb Mon Sep 17 00:00:00 2001 From: Embers-of-the-Fire Date: Fri, 13 Mar 2026 12:37:53 +0800 Subject: [PATCH 04/23] feat!: update bringup to support booting xrce agent --- rmcs_ws/src/rmcs_bringup/config/flight.yaml | 7 ++-- .../src/rmcs_bringup/launch/rmcs.launch.py | 36 ++++++++++++++++++- 2 files changed, 40 insertions(+), 3 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/flight.yaml b/rmcs_ws/src/rmcs_bringup/config/flight.yaml index 61da7c89..b2ffaac7 100644 --- a/rmcs_ws/src/rmcs_bringup/config/flight.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/flight.yaml @@ -3,7 +3,10 @@ rmcs_executor: update_rate: 1000.0 components: - rmcs_core::hardware::Flight -> flight_hardware - + flight_hardware: ros__parameters: - foo: bar + +xrce_dds_agent: + device: /dev/ttyS0 + baudrate: 921600 diff --git a/rmcs_ws/src/rmcs_bringup/launch/rmcs.launch.py b/rmcs_ws/src/rmcs_bringup/launch/rmcs.launch.py index 976cb9e7..0717aae6 100644 --- a/rmcs_ws/src/rmcs_bringup/launch/rmcs.launch.py +++ b/rmcs_ws/src/rmcs_bringup/launch/rmcs.launch.py @@ -1,12 +1,13 @@ from typing import List, Optional import os +import yaml from launch import ( LaunchContext, LaunchDescription, LaunchDescriptionEntity, ) -from launch.actions import LogInfo +from launch.actions import ExecuteProcess, LogInfo from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node @@ -50,6 +51,39 @@ def visit( ) ) + # Conditionally launch optional processes declared in the config yaml. + config_path = os.path.join( + FindPackageShare("rmcs_bringup").perform(context), + "config", + robot_name + ".yaml", + ) + with open(config_path, "r") as f: + config = yaml.safe_load(f) + + xrce_cfg = config.get("xrce_dds_agent") + if xrce_cfg is not None: + device = xrce_cfg.get("device", "/dev/ttyS0") + baudrate = str(xrce_cfg.get("baudrate", 921600)) + entities.append( + LogInfo(msg=f"Starting MicroXRCEAgent on {device} @ {baudrate} baud") + ) + entities.append( + ExecuteProcess( + cmd=[ + "sudo", + "MicroXRCEAgent", + "serial", + "--dev", + device, + "-b", + baudrate, + ], + output="log", + respawn=True, + respawn_delay=1.0, + ) + ) + if is_automatic: pass From 25c9aaba7fba48466246e87ec21117d23161737c Mon Sep 17 00:00:00 2001 From: Embers-of-the-Fire Date: Fri, 13 Mar 2026 18:46:11 +0800 Subject: [PATCH 05/23] feat!: add flight gimbal control Co-authored-by: zlq04222 <1542498005@qq.com> --- rmcs_ws/src/rmcs_bringup/config/flight.yaml | 188 +++++++++++++- .../gimbal/hero_gimbal_controller.cpp | 145 +---------- .../gimbal/simple_gimbal_controller.cpp | 17 +- .../gimbal/two_axis_gimbal_solver.hpp | 37 ++- .../controller/shooting/heat_controller.cpp | 2 + .../src/hardware/device/lk_motor.hpp | 9 +- rmcs_ws/src/rmcs_core/src/hardware/flight.cpp | 243 +++++++++++++++++- 7 files changed, 479 insertions(+), 162 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/flight.yaml b/rmcs_ws/src/rmcs_bringup/config/flight.yaml index b2ffaac7..de3b4a41 100644 --- a/rmcs_ws/src/rmcs_bringup/config/flight.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/flight.yaml @@ -3,10 +3,194 @@ rmcs_executor: update_rate: 1000.0 components: - rmcs_core::hardware::Flight -> flight_hardware + - rmcs_core::referee::Status -> referee_status -flight_hardware: - ros__parameters: + - 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::interaction::Ui -> referee_ui + #- rmcs_core::referee::app::ui::Flight -> referee_ui_flight + + - rmcs_core::referee::Command -> referee_command + + # - rmcs_auto_aim::AutoAimInitializer -> auto_aim_initializer + # - rmcs_auto_aim::AutoAimController -> auto_aim_controller + + - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster + - rmcs_core::broadcaster::TfBroadcaster -> tf_broadcaster xrce_dds_agent: device: /dev/ttyS0 baudrate: 921600 + +value_broadcaster: + ros__parameters: + forward_list: + - /gimbal/pitch/angle + - /gimbal/pitch/velocity + - /gimbal/pitch/torque + - /gimbal/pitch/control_torque + - /gimbal/pitch/control_angle_error + - /gimbal/pitch/control_velocity + - /gimbal/pitch/velocity_imu + + - /gimbal/yaw/angle + - /gimbal/yaw/velocity + - /gimbal/yaw/torque + - /gimbal/yaw/control_torque + - /gimbal/yaw/control_angle_error + - /gimbal/yaw/control_velocity + +tf_broadcaster: + ros__parameters: + tf: /tf + +flight_hardware: + ros__parameters: + usb_pid: -1 + yaw_motor_zero_point: 38745 + pitch_motor_zero_point: 51586 + +referee_status: + ros__parameters: + path: /dev/tty0 + +gimbal_controller: + ros__parameters: + upper_limit: -0.15 + lower_limit: 0.50 + yaw_upper_limit: -0.349 + yaw_lower_limit: 0.349 + +yaw_angle_pid_controller: + ros__parameters: + measurement: /gimbal/yaw/control_angle_error + control: /gimbal/yaw/control_velocity + kp: 25.0 + ki: 0.0 + kd: 0.0003 + +yaw_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/yaw/velocity_imu + setpoint: /gimbal/yaw/control_velocity + control: /gimbal/yaw/control_torque + kp: 10.0 + ki: 0.0 + kd: 0.01 + +pitch_angle_pid_controller: + ros__parameters: + measurement: /gimbal/pitch/control_angle_error + control: /gimbal/pitch/control_velocity + kp: 16.0 + ki: 0.0 + kd: 60.0 + +friction_wheel_controller: + ros__parameters: + friction_wheels: + - /gimbal/left_friction + - /gimbal/right_friction + friction_velocities: + - 740.0 + - 740.0 + friction_soft_start_stop_time: 1.0 + +heat_controller: + ros__parameters: + heat_per_shot: 10000 + reserved_heat: 0 + +bullet_feeder_controller: + ros__parameters: + bullets_per_feeder_turn: 8.0 + shot_frequency: 20.0 + safe_shot_frequency: 10.0 + eject_frequency: 10.0 + eject_time: 0.05 + deep_eject_frequency: 5.0 + deep_eject_time: 0.2 + single_shot_max_stop_delay: 2.0 + +shooting_recorder: + ros__parameters: + friction_wheel_count: 2 + # 1: trigger, 2: timing + log_mode: 1 + +left_friction_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/left_friction/velocity + setpoint: /gimbal/left_friction/control_velocity + control: /gimbal/left_friction/control_torque + kp: 0.003436926 + ki: 0.00 + kd: 0.009373434 + +right_friction_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/right_friction/velocity + setpoint: /gimbal/right_friction/control_velocity + control: /gimbal/right_friction/control_torque + kp: 0.003436926 + ki: 0.00 + kd: 0.009373434 + +bullet_feeder_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/bullet_feeder/velocity + setpoint: /gimbal/bullet_feeder/control_velocity + control: /gimbal/bullet_feeder/control_torque + kp: 0.583 + ki: 0.0 + kd: 0.0 + +auto_aim_controller: + ros__parameters: + # capture + use_video: false # If true, use video stream instead of camera. + video_path: "/workspaces/RMCS/rmcs_ws/resources/1.avi" + exposure_time: 3 + invert_image: false + # identifier + armor_model_path: "/models/mlp.onnx" + # pnp + fx: 1.722231837421459e+03 + fy: 1.724876404292754e+03 + cx: 7.013056440882832e+02 + cy: 5.645821718351237e+02 + k1: -0.064232403853946 + k2: -0.087667493884102 + k3: 0.792381808294582 + + # tracker + armor_predict_duration: 500 + # controller + gimbal_predict_duration: 100 + yaw_error: 0.02 + pitch_error: -0.01 + shoot_velocity: 28.0 + predict_sec: 0.095 + # etc + buff_predict_duration: 200 + buff_model_path: "/models/buff_nocolor_v6.onnx" + omni_exposure: 1000.0 + record_fps: 120 + debug: false # Setup in actual using.Debug mode is used when referee is not ready + debug_color: 0 # 0 For blue while 1 for red. mine + debug_robot_id: 4 + debug_buff_mode: false + record: true + raw_img_pub: false # Set false in actual use + image_viewer_type: 2 diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/hero_gimbal_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/hero_gimbal_controller.cpp index f20bd98f..be79a021 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/hero_gimbal_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/hero_gimbal_controller.cpp @@ -22,150 +22,9 @@ class HeroGimbalController HeroGimbalController() : Node( get_component_name(), - rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) - , imu_gimbal_solver( - *this, get_parameter("upper_limit").as_double(), - get_parameter("lower_limit").as_double()) - , encoder_gimbal_solver( - *this, get_parameter("upper_limit").as_double(), - get_parameter("lower_limit").as_double()) { + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) {} - register_input("/remote/joystick/left", joystick_left_); - register_input("/remote/switch/left", switch_left_); - register_input("/remote/switch/right", switch_right_); - register_input("/remote/mouse/velocity", mouse_velocity_); - register_input("/remote/mouse", mouse_); - register_input("/remote/keyboard", keyboard_); - - register_input("/gimbal/auto_aim/control_direction", auto_aim_control_direction_, false); - - register_output("/gimbal/mode", gimbal_mode_, rmcs_msgs::GimbalMode::IMU); - - register_output("/gimbal/yaw/control_angle_error", yaw_angle_error_, nan_); - register_output("/gimbal/pitch/control_angle_error", pitch_angle_error_, nan_); - register_output("/gimbal/yaw/control_angle_shift", yaw_control_angle_shift_, nan_); - register_output("/gimbal/pitch/control_angle", pitch_control_angle_, nan_); - } - - void update() override { - const auto& switch_left = *switch_left_; - const auto& switch_right = *switch_right_; - - do { - using namespace rmcs_msgs; - if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) - || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) { - reset_all_control(); - break; - } - - if (!last_keyboard_.q && keyboard_->q) { - if (gimbal_mode_keyboard_ == GimbalMode::IMU) - gimbal_mode_keyboard_ = GimbalMode::ENCODER; - else - gimbal_mode_keyboard_ = GimbalMode::IMU; - } - *gimbal_mode_ = - *switch_right_ == Switch::UP ? GimbalMode::ENCODER : gimbal_mode_keyboard_; - - if (*gimbal_mode_ == GimbalMode::IMU) { - auto angle_error = update_imu_control(); - *yaw_angle_error_ = angle_error.yaw_angle_error; - *pitch_angle_error_ = angle_error.pitch_angle_error; - - encoder_gimbal_solver.update(PreciseTwoAxisGimbalSolver::SetDisabled{}); - *yaw_control_angle_shift_ = nan_; - *pitch_control_angle_ = nan_; - } else { - imu_gimbal_solver.update(TwoAxisGimbalSolver::SetDisabled{}); - *yaw_angle_error_ = nan_; - *pitch_angle_error_ = nan_; - - auto control_angle = update_encoder_control(); - *yaw_control_angle_shift_ = control_angle.yaw_shift; - *pitch_control_angle_ = control_angle.pitch_angle; - } - } while (false); - - last_keyboard_ = *keyboard_; - } - - void reset_all_control() { - imu_gimbal_solver.update(TwoAxisGimbalSolver::SetDisabled{}); - encoder_gimbal_solver.update(PreciseTwoAxisGimbalSolver::SetDisabled{}); - - gimbal_mode_keyboard_ = rmcs_msgs::GimbalMode::IMU; - *gimbal_mode_ = rmcs_msgs::GimbalMode::IMU; - - *yaw_angle_error_ = nan_; - *pitch_angle_error_ = nan_; - *yaw_control_angle_shift_ = nan_; - *pitch_control_angle_ = nan_; - } - - TwoAxisGimbalSolver::AngleError update_imu_control() { - if (auto_aim_control_direction_.ready() - && (mouse_->right || *switch_right_ == rmcs_msgs::Switch::UP) - && !auto_aim_control_direction_->isZero()) { - return imu_gimbal_solver.update( - TwoAxisGimbalSolver::SetControlDirection{ - OdomImu::DirectionVector{*auto_aim_control_direction_}}); - } - - if (!imu_gimbal_solver.enabled()) - return imu_gimbal_solver.update(TwoAxisGimbalSolver::SetToLevel{}); - - constexpr double joystick_sensitivity = 0.006; - constexpr double mouse_sensitivity = 0.5; - - double yaw_shift = - joystick_sensitivity * joystick_left_->y() + mouse_sensitivity * mouse_velocity_->y(); - double pitch_shift = - -joystick_sensitivity * joystick_left_->x() - mouse_sensitivity * mouse_velocity_->x(); - - return imu_gimbal_solver.update( - TwoAxisGimbalSolver::SetControlShift{yaw_shift, pitch_shift}); - } - - PreciseTwoAxisGimbalSolver::ControlAngle update_encoder_control() { - if (!encoder_gimbal_solver.enabled()) - return encoder_gimbal_solver.update(PreciseTwoAxisGimbalSolver::SetControlPitch{-0.31}); - - constexpr double joystick_sensitivity = 0.006 * 0.1; - constexpr double mouse_yaw_sensitivity = 0.5 * 0.114; - constexpr double mouse_pitch_sensitivity = 0.5 * 0.095; - - double yaw_shift = joystick_sensitivity * joystick_left_->y() - + mouse_yaw_sensitivity * mouse_velocity_->y(); - double pitch_shift = -joystick_sensitivity * joystick_left_->x() - - mouse_pitch_sensitivity * mouse_velocity_->x(); - - return encoder_gimbal_solver.update( - PreciseTwoAxisGimbalSolver::SetControlShift{yaw_shift, pitch_shift}); - } - -private: - static constexpr double nan_ = std::numeric_limits::quiet_NaN(); - - InputInterface joystick_left_; - InputInterface switch_right_; - InputInterface switch_left_; - InputInterface mouse_velocity_; - InputInterface mouse_; - InputInterface keyboard_; - - rmcs_msgs::Keyboard last_keyboard_ = rmcs_msgs::Keyboard::zero(); - - InputInterface auto_aim_control_direction_; - - rmcs_msgs::GimbalMode gimbal_mode_keyboard_ = rmcs_msgs::GimbalMode::IMU; - OutputInterface gimbal_mode_; - - TwoAxisGimbalSolver imu_gimbal_solver; - PreciseTwoAxisGimbalSolver encoder_gimbal_solver; - - OutputInterface yaw_angle_error_, pitch_angle_error_; - OutputInterface yaw_control_angle_shift_, pitch_control_angle_; + void update() override {} }; } // namespace rmcs_core::controller::gimbal 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..b2386c08 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 @@ -1,3 +1,4 @@ +#include #include #include @@ -22,7 +23,9 @@ class SimpleGimbalController rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) , two_axis_gimbal_solver( *this, get_parameter("upper_limit").as_double(), - get_parameter("lower_limit").as_double()) { + get_parameter("lower_limit").as_double(), + get_parameter("yaw_upper_limit").as_double(), + get_parameter("yaw_lower_limit").as_double()) { register_input("/remote/joystick/left", joystick_left_); register_input("/remote/switch/right", switch_right_); @@ -39,7 +42,14 @@ class SimpleGimbalController 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; + if (abs(angle_error.pitch_angle_error) < 8e-3) { + *pitch_angle_error_ = 0; + } else { + *pitch_angle_error_ = angle_error.pitch_angle_error; + } + + // RCLCPP_INFO(get_logger(), "[gimbal calibration] New pitch offset: %f", + // *pitch_angle_error_); } TwoAxisGimbalSolver::AngleError calculate_angle_error() { @@ -69,6 +79,9 @@ class SimpleGimbalController double pitch_shift = -joystick_sensitivity * joystick_left_->x() - mouse_sensitivity * mouse_velocity_->x(); + // RCLCPP_INFO(get_logger(), "[gimbal calibration] New pitch offset: %f", + // joystick_left_->x()); + return two_axis_gimbal_solver.update( TwoAxisGimbalSolver::SetControlShift(yaw_shift, pitch_shift)); } diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/two_axis_gimbal_solver.hpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/two_axis_gimbal_solver.hpp index 5d937aae..f539fe30 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/two_axis_gimbal_solver.hpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/two_axis_gimbal_solver.hpp @@ -1,5 +1,6 @@ #pragma once +#include #include #include @@ -7,8 +8,6 @@ #include #include -#include -#include #include #include #include @@ -26,11 +25,16 @@ class TwoAxisGimbalSolver { }; public: - TwoAxisGimbalSolver(rmcs_executor::Component& component, double upper_limit, double lower_limit) + TwoAxisGimbalSolver( + rmcs_executor::Component& component, double upper_limit, double lower_limit, + double yaw_upper_limit, double yaw_lower_limit) : upper_limit_(std::cos(upper_limit), -std::sin(upper_limit)) - , lower_limit_(std::cos(lower_limit), -std::sin(lower_limit)) { + , lower_limit_(std::cos(lower_limit), -std::sin(lower_limit)) + , yaw_upper_limit_(std::cos(yaw_upper_limit), -std::sin(yaw_upper_limit)) + , yaw_lower_limit_(std::cos(yaw_lower_limit), -std::sin(yaw_lower_limit)) { component.register_input("/gimbal/pitch/angle", gimbal_pitch_angle_); + component.register_input("/gimbal/yaw/angle", gimbal_yaw_angle_); component.register_input("/tf", tf_); } @@ -45,7 +49,7 @@ class TwoAxisGimbalSolver { PitchLink::DirectionVector update(TwoAxisGimbalSolver& super) const override { auto odom_dir = fast_tf::cast( PitchLink::DirectionVector{Eigen::Vector3d::UnitX()}, *super.tf_); - if (odom_dir->x() == 0 || odom_dir->y() == 0) + if (odom_dir->x() == 0 && odom_dir->y() == 0) return {}; super.control_enabled_ = true; @@ -104,6 +108,7 @@ class TwoAxisGimbalSolver { update_yaw_axis(); PitchLink::DirectionVector control_direction = operation.update(*this); + if (!control_enabled_) return {nan_, nan_}; @@ -136,7 +141,7 @@ class TwoAxisGimbalSolver { auto yaw_axis = fast_tf::cast(yaw_axis_filtered_, *tf_); pitch = {yaw_axis->z(), yaw_axis->x()}; - pitch.normalized(); + pitch.normalize(); const auto& [x, y, z] = *dir; dir_yaw_link = {x * pitch.x() - z * pitch.y(), y, x * pitch.y() + z * pitch.x()}; @@ -167,8 +172,24 @@ class TwoAxisGimbalSolver { *control_direction << upper_limit_.x() * projection, upper_limit_.y(); else if (z < lower_limit_.y()) *control_direction << lower_limit_.x() * projection, lower_limit_.y(); + + const auto& [yaw_x, yaw_y, yaw_z] = *control_direction; + + Eigen::Vector2d xz_projection{yaw_x, yaw_z}; + double xz_norm = xz_projection.norm(); + if (xz_norm > 0) + xz_projection /= xz_norm; + + if (yaw_y > yaw_upper_limit_.y()) + *control_direction << yaw_upper_limit_.x() * xz_projection.x(), + yaw_upper_limit_.y(), yaw_upper_limit_.x() * xz_projection.y(); + else if (yaw_y < yaw_lower_limit_.y()) + *control_direction << yaw_lower_limit_.x() * xz_projection.x(), + yaw_lower_limit_.y(), yaw_lower_limit_.x() * xz_projection.y(); + } + static AngleError calculate_control_errors( const YawLink::DirectionVector& control_direction, const Eigen::Vector2d& pitch) { const auto& [x, y, z] = *control_direction; @@ -185,8 +206,10 @@ class TwoAxisGimbalSolver { static constexpr double nan_ = std::numeric_limits::quiet_NaN(); const Eigen::Vector2d upper_limit_, lower_limit_; + const Eigen::Vector2d yaw_upper_limit_, yaw_lower_limit_; rmcs_executor::Component::InputInterface gimbal_pitch_angle_; + rmcs_executor::Component::InputInterface gimbal_yaw_angle_; rmcs_executor::Component::InputInterface tf_; OdomImu::DirectionVector yaw_axis_filtered_{Eigen::Vector3d::UnitZ()}; @@ -195,4 +218,4 @@ class TwoAxisGimbalSolver { OdomImu::DirectionVector control_direction_; }; -} // namespace rmcs_core::controller::gimbal \ No newline at end of file +} // namespace rmcs_core::controller::gimbal 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..d344b48e 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 @@ -33,6 +33,8 @@ class HeatController *control_bullet_allowance_ = std::max( 0, (*shooter_heat_limit_ - shooter_heat_ - reserved_heat) / heat_per_shot); + + RCLCPP_INFO(get_logger(), "[gimbal calibration] New pitch offset: %ld", heat_per_shot); } private: 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 68b2e4f6..014297c2 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,7 @@ namespace rmcs_core::hardware::device { class LkMotor { public: - enum class Type : uint8_t { kMG5010Ei10, kMG4010Ei10, kMG6012Ei8, kMG4005Ei10 }; + enum class Type : uint8_t { kMG5010Ei10, kMG4010Ei10, kMG6012Ei8, kMG4005Ei10, kMHF7015 }; struct Config { explicit Config(Type type) @@ -108,6 +108,13 @@ class LkMotor { reduction_ratio = 10.0; max_torque_ = 2.5; break; + case Type::kMHF7015: + raw_angle_modulus_ = 1 << 16; + current_max = 33.0; + torque_constant = 0.51; + reduction_ratio = 8.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 index d2d915bc..e600a8a1 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp @@ -1,27 +1,55 @@ #include +#include +#include #include -#include +#include #include +#include #include + +#include #include +#include +#include +#include + +#include // Odin LiDAR C API (no ROS layer) #include #include +#include "hardware/device/bmi088.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 { + , public rclcpp::Node + , private librmcs::agent::CBoard { public: Flight() : Node{ get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} - , local_position_interface_{*this, px4_ros2::PoseFrame::LocalNED, px4_ros2::VelocityFrame::LocalNED} - , logger_(get_logger()) { + , librmcs::agent::CBoard{} + , local_position_interface_{ + *this, px4_ros2::PoseFrame::LocalNED, px4_ros2::VelocityFrame::LocalNED} + , logger_(get_logger()) + , flight_command_( + create_partner_component(get_component_name() + "_command", *this)) + , gimbal_yaw_motor_(*this, *flight_command_, "/gimbal/yaw") + , gimbal_pitch_motor_(*this, *flight_command_, "/gimbal/pitch") + , gimbal_left_friction_(*this, *flight_command_, "/gimbal/left_friction") + , gimbal_right_friction_(*this, *flight_command_, "/gimbal/right_friction") + , gimbal_bullet_feeder_(*this, *flight_command_, "/gimbal/bullet_feeder") + , dr16_(*this) + , bmi088_(500.0, 0.3, 0.005) { + g_instance_ = this; if (!local_position_interface_.doRegister()) { @@ -33,9 +61,71 @@ class Flight throw std::runtime_error("lidar_system_init failed"); } RCLCPP_INFO(logger_, "Odin lidar system initialized."); + + 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.)); + gimbal_right_friction_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(1.)); + 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; + + constexpr double rotor_distance_x = 0.83637; + constexpr double rotor_distance_y = 0.83637; + + tf_->set_transform( + Eigen::Translation3d{rotor_distance_x / 2, rotor_distance_y / 2, 0}); + tf_->set_transform( + Eigen::Translation3d{rotor_distance_x / 2, -rotor_distance_y / 2, 0}); + tf_->set_transform( + Eigen::Translation3d{-rotor_distance_x / 2, rotor_distance_y / 2, 0}); + tf_->set_transform( + Eigen::Translation3d{-rotor_distance_x / 2, -rotor_distance_y / 2, 0}); + + constexpr double gimbal_center_x = 0; + constexpr double gimbal_center_y = 0; + constexpr double gimbal_center_z = -0.20552; + tf_->set_transform( + Eigen::Translation3d{gimbal_center_x, gimbal_center_y, gimbal_center_z}); + + tf_->set_transform(Eigen::Translation3d{0.0557, 0, 0.053}); + + gimbal_calibrate_subscription_ = create_subscription( + "/gimbal/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { + gimbal_calibrate_subscription_callback(std::move(msg)); + }); + register_output("/referee/serial", referee_serial_); + referee_serial_->read = [this](std::byte* buffer, size_t size) { + return referee_ring_buffer_receive_.pop_front_n( + [&buffer](std::byte byte) noexcept { *buffer++ = byte; }, size); + }; + referee_serial_->write = [this](const std::byte* buffer, size_t size) { + start_transmit().uart1_transmit({std::span{buffer, size}}); + return size; + }; } - ~Flight() { + ~Flight() override { if (odin_device_) { lidar_stop_stream(odin_device_, LIDAR_MODE_SLAM); lidar_unregister_stream_callback(odin_device_); @@ -46,6 +136,63 @@ class Flight } void update() override { + update_motors(); + update_imu(); + dr16_.update_status(); + update_local_position(); + // RCLCPP_INFO( + // logger_, "[gimbal calibration] New pitch offset: %ld", + // gimbal_pitch_motor_.calibrate_zero_point()); + } + + void command_update() { + auto yaw_cmd = gimbal_yaw_motor_.generate_torque_command(); + auto pitch_cmd = gimbal_pitch_motor_.generate_command(); + + // 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()); + + device::CanPacket8 dji_cmds{ + gimbal_bullet_feeder_.generate_command(), + device::CanPacket8::PaddingQuarter{}, + gimbal_right_friction_.generate_command(), + gimbal_left_friction_.generate_command()}; + + start_transmit() + .can1_transmit({.can_id = 0x141, .can_data = yaw_cmd.as_bytes()}) + .can2_transmit({.can_id = 0x141, .can_data = pitch_cmd.as_bytes()}) + .can1_transmit({.can_id = 0x200, .can_data = dji_cmds.as_bytes()}); + } + +private: + void update_motors() { + using namespace rmcs_description; + gimbal_yaw_motor_.update_status(); + tf_->set_state(gimbal_yaw_motor_.angle()); + + gimbal_pitch_motor_.update_status(); + tf_->set_state(gimbal_pitch_motor_.angle()); + + gimbal_bullet_feeder_.update_status(); + gimbal_left_friction_.update_status(); + gimbal_right_friction_.update_status(); + } + + void update_imu() { + bmi088_.update_status(); + Eigen::Quaterniond gimbal_imu_pose{bmi088_.q0(), bmi088_.q1(), bmi088_.q2(), bmi088_.q3()}; + tf_->set_transform( + gimbal_imu_pose.conjugate()); + + *gimbal_yaw_velocity_imu_ = bmi088_.gz(); + *gimbal_pitch_velocity_imu_ = bmi088_.gy(); + } + + void update_local_position() { OdomSnapshot snap; { std::lock_guard lock(odom_mutex_); @@ -77,11 +224,20 @@ class Flight try { local_position_interface_.update(meas); } catch (const px4_ros2::NavigationInterfaceInvalidArgument& e) { - RCLCPP_ERROR_THROTTLE(logger_, *get_clock(), 1000, "Navigation update error: %s", e.what()); + RCLCPP_ERROR_THROTTLE( + logger_, *get_clock(), 1000, "Navigation update error: %s", e.what()); } } -private: + 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()); + } + // ---- Shared odometry snapshot (filled by SDK callback, read by update()) ---- struct OdomSnapshot { float pos_x{}, pos_y{}, pos_z{}; @@ -216,10 +372,83 @@ class 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 == 0x141) { + gimbal_yaw_motor_.store_status(data.can_data); + } else if (data.can_id == 0x201) { + gimbal_bullet_feeder_.store_status(data.can_data); + } else if (data.can_id == 0x204) { + gimbal_left_friction_.store_status(data.can_data); + } else if (data.can_id == 0x203) { + gimbal_right_friction_.store_status(data.can_data); + } + } + + void can2_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_remote_transmission || data.is_extended_can_id || data.can_data.size() < 8) + [[unlikely]] return; + if (data.can_id == 0x141) { + gimbal_pitch_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: // ---- Members ---- px4_ros2::LocalPositionMeasurementInterface local_position_interface_; rclcpp::Logger logger_; + class FlightCommand : public rmcs_executor::Component { + public: + explicit FlightCommand(Flight& flight) + : flight_(flight) {} + void update() override { flight_.command_update(); } + Flight& flight_; + }; + + std::shared_ptr flight_command_; + rclcpp::Subscription::SharedPtr gimbal_calibrate_subscription_; + device::LkMotor gimbal_yaw_motor_; + device::LkMotor gimbal_pitch_motor_; + + device::DjiMotor gimbal_left_friction_; + device::DjiMotor gimbal_right_friction_; + device::DjiMotor gimbal_bullet_feeder_; + + device::Dr16 dr16_; + device::Bmi088 bmi088_; + + OutputInterface gimbal_yaw_velocity_imu_; + OutputInterface gimbal_pitch_velocity_imu_; + + OutputInterface tf_; + + rmcs_utility::RingBuffer referee_ring_buffer_receive_{256}; + OutputInterface referee_serial_; + // Global pointer used by static C callbacks to reach the active instance. // Only one Flight component is expected per process. static Flight* g_instance_; From 3b84e63e4f12f57f87adced118abbf335d849660 Mon Sep 17 00:00:00 2001 From: Embers-of-the-Fire Date: Sat, 14 Mar 2026 12:15:24 +0800 Subject: [PATCH 06/23] fix: fix gimbal control --- rmcs_ws/src/rmcs_bringup/config/flight.yaml | 20 +++++----- .../gimbal/hero_gimbal_controller.cpp | 5 --- .../gimbal/two_axis_gimbal_solver.hpp | 1 - .../controller/shooting/heat_controller.cpp | 2 - .../rmcs_core/src/hardware/device/dr16.hpp | 6 ++- .../src/hardware/device/lk_motor.hpp | 2 +- rmcs_ws/src/rmcs_core/src/hardware/flight.cpp | 40 ++++++++----------- 7 files changed, 33 insertions(+), 43 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/flight.yaml b/rmcs_ws/src/rmcs_bringup/config/flight.yaml index de3b4a41..60b91823 100644 --- a/rmcs_ws/src/rmcs_bringup/config/flight.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/flight.yaml @@ -29,9 +29,9 @@ rmcs_executor: - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster - rmcs_core::broadcaster::TfBroadcaster -> tf_broadcaster -xrce_dds_agent: - device: /dev/ttyS0 - baudrate: 921600 +# xrce_dds_agent: +# device: /dev/ttyS0 +# baudrate: 921600 value_broadcaster: ros__parameters: @@ -39,7 +39,7 @@ value_broadcaster: - /gimbal/pitch/angle - /gimbal/pitch/velocity - /gimbal/pitch/torque - - /gimbal/pitch/control_torque + # - /gimbal/pitch/control_torque - /gimbal/pitch/control_angle_error - /gimbal/pitch/control_velocity - /gimbal/pitch/velocity_imu @@ -76,26 +76,26 @@ yaw_angle_pid_controller: ros__parameters: measurement: /gimbal/yaw/control_angle_error control: /gimbal/yaw/control_velocity - kp: 25.0 + kp: 2.0 ki: 0.0 - kd: 0.0003 + 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: 10.0 + kp: 4.0 ki: 0.0 - kd: 0.01 + kd: 0.005 pitch_angle_pid_controller: ros__parameters: measurement: /gimbal/pitch/control_angle_error control: /gimbal/pitch/control_velocity - kp: 16.0 + kp: 8.0 ki: 0.0 - kd: 60.0 + kd: 0.0 friction_wheel_controller: ros__parameters: diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/hero_gimbal_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/hero_gimbal_controller.cpp index be79a021..284af8a6 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/hero_gimbal_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/hero_gimbal_controller.cpp @@ -1,5 +1,3 @@ -#include - #include #include #include @@ -8,9 +6,6 @@ #include #include -#include "controller/gimbal/precise_two_axis_gimbal_solver.hpp" -#include "controller/gimbal/two_axis_gimbal_solver.hpp" - namespace rmcs_core::controller::gimbal { using namespace rmcs_description; 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 f539fe30..f163e656 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 @@ -3,7 +3,6 @@ #include #include -#include #include #include 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 d344b48e..3de0960a 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 @@ -33,8 +33,6 @@ class HeatController *control_bullet_allowance_ = std::max( 0, (*shooter_heat_limit_ - shooter_heat_ - reserved_heat) / heat_per_shot); - - RCLCPP_INFO(get_logger(), "[gimbal calibration] New pitch offset: %ld", heat_per_shot); } private: diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/dr16.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/dr16.hpp index 75152b17..3ba98025 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/dr16.hpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/dr16.hpp @@ -81,8 +81,12 @@ class Dr16 { auto channel_to_double = [](int32_t value) { value -= 1024; - if (-660 <= value && value <= 660) + constexpr int32_t deadzone = 10; + if (-660 <= value && value <= 660) { + if (value > -deadzone && value < deadzone) + return 0.0; return value / 660.0; + } return 0.0; }; joystick_right_.y = -channel_to_double(static_cast(part1.joystick_channel0)); 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 014297c2..67d28883 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 @@ -112,7 +112,7 @@ class LkMotor { raw_angle_modulus_ = 1 << 16; current_max = 33.0; torque_constant = 0.51; - reduction_ratio = 8.0; + 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 index e600a8a1..06848a60 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp @@ -1,4 +1,3 @@ -#include #include #include #include @@ -140,22 +139,12 @@ class Flight update_imu(); dr16_.update_status(); update_local_position(); - // RCLCPP_INFO( - // logger_, "[gimbal calibration] New pitch offset: %ld", - // gimbal_pitch_motor_.calibrate_zero_point()); } void command_update() { - auto yaw_cmd = gimbal_yaw_motor_.generate_torque_command(); + auto yaw_cmd = gimbal_yaw_motor_.generate_command(); auto pitch_cmd = gimbal_pitch_motor_.generate_command(); - // 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()); - device::CanPacket8 dji_cmds{ gimbal_bullet_feeder_.generate_command(), device::CanPacket8::PaddingQuarter{}, @@ -164,7 +153,7 @@ class Flight start_transmit() .can1_transmit({.can_id = 0x141, .can_data = yaw_cmd.as_bytes()}) - .can2_transmit({.can_id = 0x141, .can_data = pitch_cmd.as_bytes()}) + .can2_transmit({.can_id = 0x142, .can_data = pitch_cmd.as_bytes()}) .can1_transmit({.can_id = 0x200, .can_data = dji_cmds.as_bytes()}); } @@ -230,12 +219,12 @@ class Flight } 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()); + // 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()); } // ---- Shared odometry snapshot (filled by SDK callback, read by update()) ---- @@ -377,23 +366,28 @@ class Flight if (data.is_extended_can_id || data.is_remote_transmission || data.can_data.size() < 8) [[unlikely]] return; - if (data.can_id == 0x141) { - gimbal_yaw_motor_.store_status(data.can_data); - } else if (data.can_id == 0x201) { + if (data.can_id == 0x201) { gimbal_bullet_feeder_.store_status(data.can_data); } else if (data.can_id == 0x204) { gimbal_left_friction_.store_status(data.can_data); } else if (data.can_id == 0x203) { gimbal_right_friction_.store_status(data.can_data); } + else if (data.can_id == 0x141) { + gimbal_yaw_motor_.store_status(data.can_data); + } + + } void can2_receive_callback(const librmcs::data::CanDataView& data) override { if (data.is_remote_transmission || data.is_extended_can_id || data.can_data.size() < 8) [[unlikely]] return; - if (data.can_id == 0x141) { + if (data.can_id == 0x142) { gimbal_pitch_motor_.store_status(data.can_data); } + + } void uart1_receive_callback(const librmcs::data::UartDataView& data) override { From 7bcd7dbe2fca6864789a45be899fb112616a182a Mon Sep 17 00:00:00 2001 From: Embers-of-the-Fire Date: Sun, 15 Mar 2026 12:22:28 +0800 Subject: [PATCH 07/23] feat: update uxrcedds compile infra --- PREREQUISITE.md | 30 ++- rmcs_ws/src/rmcs_bringup/config/flight.yaml | 9 + .../src/rmcs_bringup/launch/rmcs.launch.py | 29 +++ rmcs_ws/src/rmcs_bringup/package.xml | 3 +- rmcs_ws/src/rmcs_core/CMakeLists.txt | 21 -- rmcs_ws/src/rmcs_core/package.xml | 1 + rmcs_ws/src/rmcs_core/src/hardware/flight.cpp | 181 ++++-------------- 7 files changed, 107 insertions(+), 167 deletions(-) diff --git a/PREREQUISITE.md b/PREREQUISITE.md index df210e68..e84fd903 100644 --- a/PREREQUISITE.md +++ b/PREREQUISITE.md @@ -19,17 +19,43 @@ Then, compile the library from source. mkdir -p /tmp/fastdds git clone https://github.com/eProsima/Fast-CDR.git mkdir Fast-CDR/build && cd Fast-CDR/build -cmake .. -DCMAKE_INSTALL_PREFIX=/workspaces/RMCS/rmcs_ws/install -DBUILD_SHARED_LIBS=ON +cmake .. -DCMAKE_INSTALL_PREFIX=/workspaces/RMCS/rmcs_ws/install -DBUILD_SHARED_LIBS=OFF -DCMAKE_POSITION_INDEPENDENT_CODE=ON cmake --build . --target install -j cd /tmp/fastdds && git clone https://github.com/eProsima/Fast-DDS.git mkdir Fast-DDS/build && cd Fast-DDS/build -cmake .. -DCMAKE_INSTALL_PREFIX=/workspaces/RMCS/rmcs_ws/install -DBUILD_SHARED_LIBS=ON +cmake .. -DCMAKE_INSTALL_PREFIX=/workspaces/RMCS/rmcs_ws/install -DBUILD_SHARED_LIBS=OFF -DCMAKE_POSITION_INDEPENDENT_CODE=ON cmake --build . --target install -j cd / && rm -rf /tmp/fastdds ``` This would roughly take ~50s to compile for a 9950x cpu using 16 threads. +And then compile the agent: + +```bash +build-rmcs \ + --packages-skip microxrcedds_agent \ + --cmake-clean-cache \ + --cmake-force-configure + +build-rmcs \ + --packages-select microxrcedds_agent \ + --cmake-clean-cache \ + --cmake-force-configure \ + --cmake-args \ + '-DBUILD_SHARED_LIBS=OFF' \ + '-DUAGENT_BUILD_EXECUTABLE=ON' \ + '-DUAGENT_USE_SYSTEM_FASTCDR=ON' \ + '-DUAGENT_USE_SYSTEM_FASTDDS=ON' \ + '-Dfastcdr_SHARED_LIBS=OFF' \ + '-Dfastdds_SHARED_LIBS=OFF' \ + '-Dfastcdr_DIR=/workspaces/RMCS/rmcs_ws/install/lib/cmake/fastcdr' \ + '-Dfastdds_DIR=/workspaces/RMCS/rmcs_ws/install/share/fastdds/cmake' \ + '-DCMAKE_PREFIX_PATH:PATH=/workspaces/RMCS/rmcs_ws/install;/opt/ros/jazzy' +``` + +This would roughly take ~30s to compile for a 9950x cpu using 16 threads. + To run the uxrce dds client: ```bash diff --git a/rmcs_ws/src/rmcs_bringup/config/flight.yaml b/rmcs_ws/src/rmcs_bringup/config/flight.yaml index 60b91823..379b4a40 100644 --- a/rmcs_ws/src/rmcs_bringup/config/flight.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/flight.yaml @@ -33,6 +33,15 @@ rmcs_executor: # device: /dev/ttyS0 # baudrate: 921600 +odin_ros_driver: + # Bring up the same host_sdk_sample node used by odin1_ros2.launch.py. + # Flight only needs /odin1/odometry, so the auxiliary depth/reprojection/RViz nodes stay off. + enabled: true + config_file: "" # Empty uses odin_ros_driver/config/control_command.yaml + node_name: host_sdk_sample + respawn: true + respawn_delay: 1.0 + value_broadcaster: ros__parameters: forward_list: diff --git a/rmcs_ws/src/rmcs_bringup/launch/rmcs.launch.py b/rmcs_ws/src/rmcs_bringup/launch/rmcs.launch.py index 0717aae6..f29733b2 100644 --- a/rmcs_ws/src/rmcs_bringup/launch/rmcs.launch.py +++ b/rmcs_ws/src/rmcs_bringup/launch/rmcs.launch.py @@ -60,6 +60,35 @@ def visit( with open(config_path, "r") as f: config = yaml.safe_load(f) + odin_cfg = config.get("odin_ros_driver") + if odin_cfg is not None and odin_cfg.get("enabled", True): + odin_share = FindPackageShare("odin_ros_driver").perform(context) + odin_config_file = odin_cfg.get("config_file") or os.path.join( + odin_share, "config", "control_command.yaml" + ) + node_name = odin_cfg.get("node_name", "host_sdk_sample") + respawn = odin_cfg.get("respawn", True) + respawn_delay = float(odin_cfg.get("respawn_delay", 1.0)) + entities.append( + LogInfo( + msg=( + f"Starting odin_ros_driver node '{node_name}' " + f"with config '{odin_config_file}'" + ) + ) + ) + entities.append( + Node( + package="odin_ros_driver", + executable="host_sdk_sample", + name=node_name, + parameters=[{"config_file": odin_config_file}], + respawn=respawn, + respawn_delay=respawn_delay, + output="log", + ) + ) + xrce_cfg = config.get("xrce_dds_agent") if xrce_cfg is not None: device = xrce_cfg.get("device", "/dev/ttyS0") diff --git a/rmcs_ws/src/rmcs_bringup/package.xml b/rmcs_ws/src/rmcs_bringup/package.xml index c64c661f..746f2627 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 + odin_ros_driver 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/CMakeLists.txt b/rmcs_ws/src/rmcs_core/CMakeLists.txt index d0594285..0fae776a 100644 --- a/rmcs_ws/src/rmcs_core/CMakeLists.txt +++ b/rmcs_ws/src/rmcs_core/CMakeLists.txt @@ -36,30 +36,9 @@ ament_auto_add_library( include_directories(${PROJECT_SOURCE_DIR}/include) include_directories(${PROJECT_SOURCE_DIR}/src) -set(ODIN_DRIVER_DIR "${PROJECT_SOURCE_DIR}/../odin_ros_driver") -include_directories(SYSTEM "${ODIN_DRIVER_DIR}/include") - -execute_process( - COMMAND uname -m - OUTPUT_VARIABLE _HOST_ARCH - OUTPUT_STRIP_TRAILING_WHITESPACE -) - -if(_HOST_ARCH MATCHES "arm|aarch64") - set(ODIN_HOST_API_LIB "${ODIN_DRIVER_DIR}/lib/liblydHostApi_arm.a") -else() - set(ODIN_HOST_API_LIB "${ODIN_DRIVER_DIR}/lib/liblydHostApi_amd.a") -endif() - -if(NOT EXISTS "${ODIN_HOST_API_LIB}") - message(FATAL_ERROR "Missing Odin Host API library: ${ODIN_HOST_API_LIB}") -endif() - target_link_libraries( ${PROJECT_NAME} librmcs-sdk - usb-1.0 - ${ODIN_HOST_API_LIB} pthread ${CMAKE_DL_LIBS} ) diff --git a/rmcs_ws/src/rmcs_core/package.xml b/rmcs_ws/src/rmcs_core/package.xml index c16d6cee..c46d98b2 100644 --- a/rmcs_ws/src/rmcs_core/package.xml +++ b/rmcs_ws/src/rmcs_core/package.xml @@ -9,6 +9,7 @@ ament_cmake + nav_msgs rclcpp std_msgs pluginlib diff --git a/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp b/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp index 06848a60..0b7e74e7 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp @@ -3,6 +3,7 @@ #include #include +#include #include #include #include @@ -15,10 +16,6 @@ #include -// Odin LiDAR C API (no ROS layer) -#include -#include - #include "hardware/device/bmi088.hpp" #include "hardware/device/dji_motor.hpp" #include "hardware/device/dr16.hpp" @@ -49,17 +46,21 @@ class Flight , dr16_(*this) , bmi088_(500.0, 0.3, 0.005) { - g_instance_ = this; - if (!local_position_interface_.doRegister()) { throw std::runtime_error("Failed to register LocalPositionMeasurementInterface"); } RCLCPP_INFO(logger_, "LocalPositionMeasurementInterface registered successfully."); - if (lidar_system_init(&Flight::device_event_callback)) { - throw std::runtime_error("lidar_system_init failed"); - } - RCLCPP_INFO(logger_, "Odin lidar system initialized."); + auto odin_odom_qos = rclcpp::QoS{1}; + odin_odom_qos.reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE); + odin_odom_qos.durability(RMW_QOS_POLICY_DURABILITY_VOLATILE); + odin_odometry_subscription_ = create_subscription( + "/odin1/odometry", + odin_odom_qos, + [this](const nav_msgs::msg::Odometry::ConstSharedPtr& msg) { + odin_odometry_subscription_callback(*msg); + }); + RCLCPP_INFO(logger_, "Subscribed to Odin driver odometry on /odin1/odometry."); gimbal_yaw_motor_.configure( device::LkMotor::Config{device::LkMotor::Type::kMHF7015} @@ -124,15 +125,7 @@ class Flight }; } - ~Flight() override { - if (odin_device_) { - lidar_stop_stream(odin_device_, LIDAR_MODE_SLAM); - lidar_unregister_stream_callback(odin_device_); - lidar_close_device(odin_device_); - lidar_destory_device(odin_device_); - } - lidar_system_deinit(); - } + ~Flight() override = default; void update() override { update_motors(); @@ -192,7 +185,8 @@ class Flight } px4_ros2::LocalPositionMeasurement meas{}; - meas.timestamp_sample = get_clock()->now(); + meas.timestamp_sample = + snap.timestamp_sample.nanoseconds() > 0 ? snap.timestamp_sample : get_clock()->now(); meas.position_xy = Eigen::Vector2f{snap.pos_x, snap.pos_y}; meas.position_xy_variance = Eigen::Vector2f{ @@ -203,9 +197,11 @@ class Flight meas.position_z_variance = snap.cov_zz > 0.f ? snap.cov_zz : 0.1f; meas.velocity_xy = Eigen::Vector2f{snap.vel_x, snap.vel_y}; - meas.velocity_xy_variance = Eigen::Vector2f{0.1f, 0.1f}; + meas.velocity_xy_variance = Eigen::Vector2f{ + snap.vel_cov_xx > 0.f ? snap.vel_cov_xx : 0.1f, + snap.vel_cov_yy > 0.f ? snap.vel_cov_yy : 0.1f}; - // Quaternion from odin is (x, y, z, w); Eigen ctor is (w, x, y, z) + // ROS Odometry stores quaternions as (x, y, z, w); Eigen ctor is (w, x, y, z) meas.attitude_quaternion = Eigen::Quaternionf{snap.q_w, snap.q_x, snap.q_y, snap.q_z}; meas.attitude_variance = Eigen::Vector3f{0.05f, 0.05f, 0.05f}; @@ -227,132 +223,37 @@ class Flight // gimbal_pitch_motor_.calibrate_zero_point()); } - // ---- Shared odometry snapshot (filled by SDK callback, read by update()) ---- + // ---- Shared odometry snapshot (filled by ROS subscription, read by update()) ---- struct OdomSnapshot { + rclcpp::Time timestamp_sample{}; float pos_x{}, pos_y{}, pos_z{}; float vel_x{}, vel_y{}; float q_x{}, q_y{}, q_z{}, q_w{1.f}; float cov_xx{}, cov_yy{}, cov_zz{}; + float vel_cov_xx{}, vel_cov_yy{}; }; std::mutex odom_mutex_; OdomSnapshot latest_odom_; bool odom_ready_{false}; - // ---- Device handle (set once when device connects) ---- - device_handle odin_device_{nullptr}; - - // ---- Static callbacks (SDK uses plain C function pointers) ---- - - // Called by SDK when device connects or disconnects. - static void device_event_callback(const lidar_device_info_t* device_info, bool attach) { - if (!attach || !g_instance_) - return; - g_instance_->on_device_attach(device_info); - } - - // Called by SDK for every incoming data frame. - static void data_callback(const lidar_data_t* data, void* /*user_data*/) { - if (!g_instance_ || data->type != LIDAR_DT_SLAM_ODOMETRY) - return; - g_instance_->on_odom_frame(&data->stream); - } - - // ---- Instance-level handlers ---- - - void on_device_attach(const lidar_device_info_t* device_info) { - RCLCPP_INFO(logger_, "Odin device attached, setting up SLAM odometry stream."); - - lidar_device_info_t info = *device_info; // mutable copy required by API - device_handle dev = nullptr; - if (lidar_create_device(&info, &dev)) { - RCLCPP_ERROR(logger_, "lidar_create_device failed"); - return; - } - - if (device_info->initial_state == LIDAR_DEVICE_NOT_INITIALIZED) { - if (lidar_open_device(dev)) { - RCLCPP_ERROR(logger_, "lidar_open_device failed"); - lidar_destory_device(dev); - return; - } - if (lidar_set_mode(dev, LIDAR_MODE_SLAM)) { - RCLCPP_ERROR(logger_, "lidar_set_mode(SLAM) failed"); - lidar_close_device(dev); - lidar_destory_device(dev); - return; - } - } - - lidar_data_callback_info_t cb_info{}; - cb_info.data_callback = &Flight::data_callback; - cb_info.user_data = nullptr; - if (lidar_register_stream_callback(dev, cb_info)) { - RCLCPP_ERROR(logger_, "lidar_register_stream_callback failed"); - lidar_close_device(dev); - lidar_destory_device(dev); - return; - } - - uint32_t dtof_subframe_odr = 0; - if (lidar_start_stream(dev, LIDAR_MODE_SLAM, dtof_subframe_odr)) { - RCLCPP_ERROR(logger_, "lidar_start_stream failed"); - lidar_close_device(dev); - lidar_destory_device(dev); - return; - } - - lidar_activate_stream_type(dev, LIDAR_DT_SLAM_ODOMETRY); - - odin_device_ = dev; - RCLCPP_INFO(logger_, "Odin SLAM odometry stream active."); - } - - void on_odom_frame(const capture_Image_List_t* stream) { - const uint32_t data_len = stream->imageList[0].length; - const void* addr = stream->imageList[0].pAddr; - + void odin_odometry_subscription_callback(const nav_msgs::msg::Odometry& msg) { OdomSnapshot snap{}; - - if (data_len == sizeof(ros_odom_convert_complete_t)) { - // Full odometry: position + velocity + covariance - const auto* d = static_cast(addr); - - // pos encoded as fixed-point ×1e6 - snap.pos_x = static_cast(d->pos[0]) / 1e6f; - snap.pos_y = static_cast(d->pos[1]) / 1e6f; - snap.pos_z = static_cast(d->pos[2]) / 1e6f; - - // orientation quaternion (x, y, z, w) encoded ×1e6 - snap.q_x = static_cast(d->orient[0]) / 1e6f; - snap.q_y = static_cast(d->orient[1]) / 1e6f; - snap.q_z = static_cast(d->orient[2]) / 1e6f; - snap.q_w = static_cast(d->orient[3]) / 1e6f; - - // linear velocity encoded ×1e6 - snap.vel_x = static_cast(d->linear_velocity[0]) / 1e6f; - snap.vel_y = static_cast(d->linear_velocity[1]) / 1e6f; - - // pose covariance diagonal (pose_cov[0]=xx, pose_cov[4]=yy, pose_cov[8]=zz) - snap.cov_xx = static_cast(d->pose_cov[0]); - snap.cov_yy = static_cast(d->pose_cov[4]); - snap.cov_zz = static_cast(d->pose_cov[8]); - - } else if (data_len == sizeof(ros2_odom_convert_t)) { - // Compact odometry: position + orientation only - const auto* d = static_cast(addr); - - snap.pos_x = static_cast(d->pos[0]) / 1e6f; - snap.pos_y = static_cast(d->pos[1]) / 1e6f; - snap.pos_z = static_cast(d->pos[2]) / 1e6f; - - snap.q_x = static_cast(d->orient[0]) / 1e6f; - snap.q_y = static_cast(d->orient[1]) / 1e6f; - snap.q_z = static_cast(d->orient[2]) / 1e6f; - snap.q_w = static_cast(d->orient[3]) / 1e6f; - } else { - return; // Unknown payload; skip - } + snap.timestamp_sample = rclcpp::Time{msg.header.stamp}; + snap.pos_x = static_cast(msg.pose.pose.position.x); + snap.pos_y = static_cast(msg.pose.pose.position.y); + snap.pos_z = static_cast(msg.pose.pose.position.z); + snap.vel_x = static_cast(msg.twist.twist.linear.x); + snap.vel_y = static_cast(msg.twist.twist.linear.y); + snap.q_x = static_cast(msg.pose.pose.orientation.x); + snap.q_y = static_cast(msg.pose.pose.orientation.y); + snap.q_z = static_cast(msg.pose.pose.orientation.z); + snap.q_w = static_cast(msg.pose.pose.orientation.w); + snap.cov_xx = static_cast(msg.pose.covariance[0]); + snap.cov_yy = static_cast(msg.pose.covariance[7]); + snap.cov_zz = static_cast(msg.pose.covariance[14]); + snap.vel_cov_xx = static_cast(msg.twist.covariance[0]); + snap.vel_cov_yy = static_cast(msg.twist.covariance[7]); { std::lock_guard lock(odom_mutex_); @@ -425,6 +326,7 @@ class Flight std::shared_ptr flight_command_; rclcpp::Subscription::SharedPtr gimbal_calibrate_subscription_; + rclcpp::Subscription::SharedPtr odin_odometry_subscription_; device::LkMotor gimbal_yaw_motor_; device::LkMotor gimbal_pitch_motor_; @@ -442,15 +344,8 @@ class Flight rmcs_utility::RingBuffer referee_ring_buffer_receive_{256}; OutputInterface referee_serial_; - - // Global pointer used by static C callbacks to reach the active instance. - // Only one Flight component is expected per process. - static Flight* g_instance_; }; -// Static member definition -Flight* Flight::g_instance_ = nullptr; - } // namespace rmcs_core::hardware #include From 7d329334714ff56cdad72c4dfbc8b29d52bb4b8c Mon Sep 17 00:00:00 2001 From: Embers-of-the-Fire Date: Sun, 15 Mar 2026 14:51:06 +0800 Subject: [PATCH 08/23] feat: update flight external hardware configuration --- rmcs_ws/src/rmcs_bringup/config/flight.yaml | 21 +++++++++------- .../src/rmcs_bringup/launch/rmcs.launch.py | 24 ++++++++++++++----- 2 files changed, 31 insertions(+), 14 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/flight.yaml b/rmcs_ws/src/rmcs_bringup/config/flight.yaml index 379b4a40..d42ef4cb 100644 --- a/rmcs_ws/src/rmcs_bringup/config/flight.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/flight.yaml @@ -29,18 +29,23 @@ rmcs_executor: - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster - rmcs_core::broadcaster::TfBroadcaster -> tf_broadcaster +xrce_dds_agent: + ros__parameters: + device: /dev/ttyACM0 + baudrate: 921600 # xrce_dds_agent: # device: /dev/ttyS0 # baudrate: 921600 odin_ros_driver: - # Bring up the same host_sdk_sample node used by odin1_ros2.launch.py. - # Flight only needs /odin1/odometry, so the auxiliary depth/reprojection/RViz nodes stay off. - enabled: true - config_file: "" # Empty uses odin_ros_driver/config/control_command.yaml - node_name: host_sdk_sample - respawn: true - respawn_delay: 1.0 + ros__parameters: + # Bring up the same host_sdk_sample node used by odin1_ros2.launch.py. + # Flight only needs /odin1/odometry, so the auxiliary depth/reprojection/RViz nodes stay off. + enabled: true + config_file: "/rmcs_install/share/odin_ros_driver/config/control_command.yaml" # Empty uses odin_ros_driver/config/control_command.yaml + node_name: host_sdk_sample + respawn: true + respawn_delay: 1.0 value_broadcaster: ros__parameters: @@ -118,7 +123,7 @@ friction_wheel_controller: heat_controller: ros__parameters: - heat_per_shot: 10000 + heat_per_shot: 10 reserved_heat: 0 bullet_feeder_controller: diff --git a/rmcs_ws/src/rmcs_bringup/launch/rmcs.launch.py b/rmcs_ws/src/rmcs_bringup/launch/rmcs.launch.py index f29733b2..c84a6265 100644 --- a/rmcs_ws/src/rmcs_bringup/launch/rmcs.launch.py +++ b/rmcs_ws/src/rmcs_bringup/launch/rmcs.launch.py @@ -47,7 +47,8 @@ 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="screen", + emulate_tty=True, ) ) @@ -60,12 +61,23 @@ def visit( with open(config_path, "r") as f: config = yaml.safe_load(f) - odin_cfg = config.get("odin_ros_driver") + odin_cfg = config.get("odin_ros_driver", {}).get("ros__parameters") if odin_cfg is not None and odin_cfg.get("enabled", True): odin_share = FindPackageShare("odin_ros_driver").perform(context) - odin_config_file = odin_cfg.get("config_file") or os.path.join( + default_odin_config_file = os.path.join( odin_share, "config", "control_command.yaml" ) + odin_config_file = odin_cfg.get("config_file") or default_odin_config_file + if not os.path.isfile(odin_config_file): + entities.append( + LogInfo( + msg=( + f"Odin config '{odin_config_file}' was not found; " + f"falling back to '{default_odin_config_file}'" + ) + ) + ) + odin_config_file = default_odin_config_file node_name = odin_cfg.get("node_name", "host_sdk_sample") respawn = odin_cfg.get("respawn", True) respawn_delay = float(odin_cfg.get("respawn_delay", 1.0)) @@ -85,11 +97,12 @@ def visit( parameters=[{"config_file": odin_config_file}], respawn=respawn, respawn_delay=respawn_delay, - output="log", + output="screen", + emulate_tty=True, ) ) - xrce_cfg = config.get("xrce_dds_agent") + xrce_cfg = config.get("xrce_dds_agent", {}).get("ros__parameters") if xrce_cfg is not None: device = xrce_cfg.get("device", "/dev/ttyS0") baudrate = str(xrce_cfg.get("baudrate", 921600)) @@ -99,7 +112,6 @@ def visit( entities.append( ExecuteProcess( cmd=[ - "sudo", "MicroXRCEAgent", "serial", "--dev", From 00e9d0ed79ca9a6784a2091b21f857591294a248 Mon Sep 17 00:00:00 2001 From: Embers-of-the-Fire Date: Sun, 15 Mar 2026 20:21:20 +0800 Subject: [PATCH 09/23] feat: use group fork instead of upstream --- .gitmodules | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.gitmodules b/.gitmodules index e58aef1c..89243d69 100644 --- a/.gitmodules +++ b/.gitmodules @@ -21,4 +21,4 @@ url = https://github.com/Auterion/px4-ros2-interface-lib [submodule "rmcs_ws/src/odin_ros_driver"] path = rmcs_ws/src/odin_ros_driver - url = https://github.com/manifoldsdk/odin_ros_driver.git + url = https://github.com/Alliance-Algorithm/odin_ros_driver.git From f4801f1193a13655c465e9408a6fecc21ad34bb2 Mon Sep 17 00:00:00 2001 From: Embers-of-the-Fire Date: Sun, 15 Mar 2026 20:21:40 +0800 Subject: [PATCH 10/23] feat: update fire control --- .../gimbal/simple_gimbal_controller.cpp | 2 +- .../controller/shooting/heat_controller.cpp | 2 +- rmcs_ws/src/rmcs_core/src/hardware/flight.cpp | 50 +++++++++---------- 3 files changed, 25 insertions(+), 29 deletions(-) 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 b2386c08..b33baa5f 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 @@ -77,7 +77,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(); // RCLCPP_INFO(get_logger(), "[gimbal calibration] New pitch offset: %f", // joystick_left_->x()); 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..3fa01d55 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 @@ -32,7 +32,7 @@ class HeatController shooter_heat_ += heat_per_shot + 10; *control_bullet_allowance_ = std::max( - 0, (*shooter_heat_limit_ - shooter_heat_ - reserved_heat) / heat_per_shot); + 0, (100 - shooter_heat_ - reserved_heat) / heat_per_shot); } private: diff --git a/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp b/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp index 0b7e74e7..d77c80a5 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp @@ -11,8 +11,8 @@ #include #include #include -#include #include +#include #include @@ -33,8 +33,7 @@ class Flight get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} , librmcs::agent::CBoard{} - , local_position_interface_{ - *this, px4_ros2::PoseFrame::LocalNED, px4_ros2::VelocityFrame::LocalNED} + , local_position_interface_{*this, px4_ros2::PoseFrame::LocalNED, px4_ros2::VelocityFrame::LocalNED} , logger_(get_logger()) , flight_command_( create_partner_component(get_component_name() + "_command", *this)) @@ -55,8 +54,7 @@ class Flight odin_odom_qos.reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE); odin_odom_qos.durability(RMW_QOS_POLICY_DURABILITY_VOLATILE); odin_odometry_subscription_ = create_subscription( - "/odin1/odometry", - odin_odom_qos, + "/odin1/odometry", odin_odom_qos, [this](const nav_msgs::msg::Odometry::ConstSharedPtr& msg) { odin_odometry_subscription_callback(*msg); }); @@ -120,7 +118,9 @@ 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({std::span{buffer, size}}); + start_transmit().uart1_transmit({ + std::span{buffer, size} + }); return size; }; } @@ -139,13 +139,11 @@ class Flight auto pitch_cmd = gimbal_pitch_motor_.generate_command(); device::CanPacket8 dji_cmds{ - gimbal_bullet_feeder_.generate_command(), - device::CanPacket8::PaddingQuarter{}, - gimbal_right_friction_.generate_command(), - gimbal_left_friction_.generate_command()}; + gimbal_bullet_feeder_.generate_command(), device::CanPacket8::PaddingQuarter{}, + gimbal_right_friction_.generate_command(), gimbal_left_friction_.generate_command()}; start_transmit() - .can1_transmit({.can_id = 0x141, .can_data = yaw_cmd.as_bytes()}) + .can2_transmit({.can_id = 0x141, .can_data = yaw_cmd.as_bytes()}) .can2_transmit({.can_id = 0x142, .can_data = pitch_cmd.as_bytes()}) .can1_transmit({.can_id = 0x200, .can_data = dji_cmds.as_bytes()}); } @@ -190,8 +188,7 @@ class Flight meas.position_xy = Eigen::Vector2f{snap.pos_x, snap.pos_y}; meas.position_xy_variance = Eigen::Vector2f{ - snap.cov_xx > 0.f ? snap.cov_xx : 0.1f, - snap.cov_yy > 0.f ? snap.cov_yy : 0.1f}; + snap.cov_xx > 0.f ? snap.cov_xx : 0.1f, snap.cov_yy > 0.f ? snap.cov_yy : 0.1f}; meas.position_z = snap.pos_z; meas.position_z_variance = snap.cov_zz > 0.f ? snap.cov_zz : 0.1f; @@ -202,8 +199,7 @@ class Flight snap.vel_cov_yy > 0.f ? snap.vel_cov_yy : 0.1f}; // ROS Odometry stores quaternions as (x, y, z, w); Eigen ctor is (w, x, y, z) - meas.attitude_quaternion = - Eigen::Quaternionf{snap.q_w, snap.q_x, snap.q_y, snap.q_z}; + meas.attitude_quaternion = Eigen::Quaternionf{snap.q_w, snap.q_x, snap.q_y, snap.q_z}; meas.attitude_variance = Eigen::Vector3f{0.05f, 0.05f, 0.05f}; try { @@ -238,6 +234,10 @@ class Flight bool odom_ready_{false}; void odin_odometry_subscription_callback(const nav_msgs::msg::Odometry& msg) { + RCLCPP_INFO_THROTTLE( + logger_, *get_clock(), 1000, "Received odometry message with timestamp %u.%u", + msg.header.stamp.sec, msg.header.stamp.nanosec); + OdomSnapshot snap{}; snap.timestamp_sample = rclcpp::Time{msg.header.stamp}; snap.pos_x = static_cast(msg.pose.pose.position.x); @@ -265,30 +265,27 @@ class 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; + [[unlikely]] + return; - if (data.can_id == 0x201) { + if (data.can_id == 0x201) { gimbal_bullet_feeder_.store_status(data.can_data); } else if (data.can_id == 0x204) { gimbal_left_friction_.store_status(data.can_data); } else if (data.can_id == 0x203) { gimbal_right_friction_.store_status(data.can_data); } - else if (data.can_id == 0x141) { - gimbal_yaw_motor_.store_status(data.can_data); - } - - } void can2_receive_callback(const librmcs::data::CanDataView& data) override { if (data.is_remote_transmission || data.is_extended_can_id || data.can_data.size() < 8) - [[unlikely]] return; + [[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 { @@ -302,8 +299,7 @@ class Flight dr16_.store_status(data.uart_data.data(), data.uart_data.size()); } - void accelerometer_receive_callback( - const librmcs::data::AccelerometerDataView& data) override { + void accelerometer_receive_callback(const librmcs::data::AccelerometerDataView& data) override { bmi088_.store_accelerometer_status(data.x, data.y, data.z); } From dd72b1bff2a2083ff5644c3acead465cd233e9b1 Mon Sep 17 00:00:00 2001 From: Embers-of-the-Fire Date: Sun, 15 Mar 2026 20:39:20 +0800 Subject: [PATCH 11/23] feat: update odin driver version --- 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 0c3a02a2..8c47e655 160000 --- a/rmcs_ws/src/odin_ros_driver +++ b/rmcs_ws/src/odin_ros_driver @@ -1 +1 @@ -Subproject commit 0c3a02a2b2260be424354faf9492ec9c934664c6 +Subproject commit 8c47e655c2f158c82083cb4d90c6cb47ff00aaf7 From 5e5451ad1f63ee140dcb76aaaa74951519a9839c Mon Sep 17 00:00:00 2001 From: Embers-of-the-Fire Date: Mon, 16 Mar 2026 21:10:20 +0800 Subject: [PATCH 12/23] feat: @zlq04222 note --- PREREQUISITE.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/PREREQUISITE.md b/PREREQUISITE.md index e84fd903..819635b1 100644 --- a/PREREQUISITE.md +++ b/PREREQUISITE.md @@ -68,6 +68,8 @@ Note that the baudrate should be the same as the one configured in the flight co Odin1 driver requires cv-bridge which is no longer used by the main upstream. +@zlq04222 + ```bash sudo apt-get install ros-jazzy-cv-bridge ``` From 455961e4faf7cafeef093c59ccfdf47b21840834 Mon Sep 17 00:00:00 2001 From: Embers-of-the-Fire Date: Mon, 16 Mar 2026 21:10:36 +0800 Subject: [PATCH 13/23] feat: update flight hardware control code --- rmcs_ws/src/rmcs_core/src/hardware/flight.cpp | 83 ++++++++++++++++--- 1 file changed, 72 insertions(+), 11 deletions(-) diff --git a/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp b/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp index d77c80a5..a255a456 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp @@ -118,7 +118,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({ std::span{buffer, size} }); return size; @@ -182,24 +182,58 @@ class Flight odom_ready_ = false; } + const auto& odin_to_px4_rotation = odin1_base_link_to_px4_rotation(); + const auto& odin_to_px4_offset = odin1_base_link_to_px4_translation(); + + Eigen::Quaternionf odom_to_odin_q{snap.q_w, snap.q_x, snap.q_y, snap.q_z}; + odom_to_odin_q.normalize(); + + Eigen::Isometry3f odom_to_odin = Eigen::Isometry3f::Identity(); + odom_to_odin.linear() = odom_to_odin_q.toRotationMatrix(); + odom_to_odin.translation() = Eigen::Vector3f{snap.pos_x, snap.pos_y, snap.pos_z}; + + Eigen::Isometry3f odin_to_px4 = Eigen::Isometry3f::Identity(); + odin_to_px4.linear() = odin_to_px4_rotation; + odin_to_px4.translation() = odin_to_px4_offset; + + const Eigen::Isometry3f odom_to_px4 = odom_to_odin * odin_to_px4; + + // nav_msgs/Odometry twists are reported in child_frame_id, which is odin1_base_link here. + const Eigen::Vector3f odin_linear_velocity{ + snap.vel_x, snap.vel_y, snap.vel_z}; + const Eigen::Vector3f odin_angular_velocity{ + snap.ang_x, snap.ang_y, snap.ang_z}; + const Eigen::Vector3f px4_velocity_odom = + odom_to_odin.linear() + * (odin_linear_velocity + odin_angular_velocity.cross(odin_to_px4_offset)); + + Eigen::Matrix3f odin_velocity_covariance = Eigen::Matrix3f::Zero(); + odin_velocity_covariance.diagonal() = + Eigen::Vector3f{snap.vel_cov_xx, snap.vel_cov_yy, snap.vel_cov_zz}; + const Eigen::Matrix3f px4_velocity_covariance = + odom_to_odin.linear() * odin_velocity_covariance * odom_to_odin.linear().transpose(); + px4_ros2::LocalPositionMeasurement meas{}; meas.timestamp_sample = snap.timestamp_sample.nanoseconds() > 0 ? snap.timestamp_sample : get_clock()->now(); - meas.position_xy = Eigen::Vector2f{snap.pos_x, snap.pos_y}; + meas.position_xy = odom_to_px4.translation().head<2>(); meas.position_xy_variance = Eigen::Vector2f{ snap.cov_xx > 0.f ? snap.cov_xx : 0.1f, snap.cov_yy > 0.f ? snap.cov_yy : 0.1f}; - meas.position_z = snap.pos_z; + meas.position_z = odom_to_px4.translation().z(); meas.position_z_variance = snap.cov_zz > 0.f ? snap.cov_zz : 0.1f; - meas.velocity_xy = Eigen::Vector2f{snap.vel_x, snap.vel_y}; + meas.velocity_xy = px4_velocity_odom.head<2>(); meas.velocity_xy_variance = Eigen::Vector2f{ - snap.vel_cov_xx > 0.f ? snap.vel_cov_xx : 0.1f, - snap.vel_cov_yy > 0.f ? snap.vel_cov_yy : 0.1f}; + px4_velocity_covariance(0, 0) > 0.f ? px4_velocity_covariance(0, 0) : 0.1f, + px4_velocity_covariance(1, 1) > 0.f ? px4_velocity_covariance(1, 1) : 0.1f}; + + meas.velocity_z = px4_velocity_odom.z(); + meas.velocity_z_variance = + px4_velocity_covariance(2, 2) > 0.f ? px4_velocity_covariance(2, 2) : 0.1f; - // ROS Odometry stores quaternions as (x, y, z, w); Eigen ctor is (w, x, y, z) - meas.attitude_quaternion = Eigen::Quaternionf{snap.q_w, snap.q_x, snap.q_y, snap.q_z}; + meas.attitude_quaternion = Eigen::Quaternionf{odom_to_px4.linear()}; meas.attitude_variance = Eigen::Vector3f{0.05f, 0.05f, 0.05f}; try { @@ -223,10 +257,11 @@ class Flight struct OdomSnapshot { rclcpp::Time timestamp_sample{}; float pos_x{}, pos_y{}, pos_z{}; - float vel_x{}, vel_y{}; + float vel_x{}, vel_y{}, vel_z{}; + float ang_x{}, ang_y{}, ang_z{}; float q_x{}, q_y{}, q_z{}, q_w{1.f}; float cov_xx{}, cov_yy{}, cov_zz{}; - float vel_cov_xx{}, vel_cov_yy{}; + float vel_cov_xx{}, vel_cov_yy{}, vel_cov_zz{}; }; std::mutex odom_mutex_; @@ -245,6 +280,10 @@ class Flight snap.pos_z = static_cast(msg.pose.pose.position.z); snap.vel_x = static_cast(msg.twist.twist.linear.x); snap.vel_y = static_cast(msg.twist.twist.linear.y); + snap.vel_z = static_cast(msg.twist.twist.linear.z); + snap.ang_x = static_cast(msg.twist.twist.angular.x); + snap.ang_y = static_cast(msg.twist.twist.angular.y); + snap.ang_z = static_cast(msg.twist.twist.angular.z); snap.q_x = static_cast(msg.pose.pose.orientation.x); snap.q_y = static_cast(msg.pose.pose.orientation.y); snap.q_z = static_cast(msg.pose.pose.orientation.z); @@ -254,6 +293,7 @@ class Flight snap.cov_zz = static_cast(msg.pose.covariance[14]); snap.vel_cov_xx = static_cast(msg.twist.covariance[0]); snap.vel_cov_yy = static_cast(msg.twist.covariance[7]); + snap.vel_cov_zz = static_cast(msg.twist.covariance[14]); { std::lock_guard lock(odom_mutex_); @@ -262,6 +302,27 @@ class Flight } } + static const Eigen::Matrix3f& odin1_base_link_to_px4_rotation() { + static const Eigen::Matrix3f rotation = [] { + Eigen::Matrix3f matrix; + // Odin axes are x->down, y->left, z->front. PX4 expects body FRD. + matrix << 0.f, 0.f, 1.f, + 0.f, -1.f, 0.f, + 1.f, 0.f, 0.f; + return matrix; + }(); + return rotation; + } + + static const Eigen::Vector3f& odin1_base_link_to_px4_translation() { + static const Eigen::Vector3f translation = [] { + // Odin is mounted 135 mm below, 312 mm right, and 296 mm behind the PX4 controller. + // This is the odin1_base_link -> PX4 origin offset expressed in Odin axes. + return Eigen::Vector3f{-0.135f, 0.312f, 0.296f}; + }(); + return translation; + } + 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) @@ -288,7 +349,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++}; }, From e7f2d603fc57347de61253d531748d49079bedc7 Mon Sep 17 00:00:00 2001 From: Embers-of-the-Fire Date: Tue, 17 Mar 2026 15:16:34 +0800 Subject: [PATCH 14/23] feat: lock micro-xrce-dds-agent version --- rmcs_ws/src/micro-xrce-dds-agent | 2 +- rmcs_ws/src/rmcs_core/src/hardware/flight.cpp | 16 ++++++++-------- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/rmcs_ws/src/micro-xrce-dds-agent b/rmcs_ws/src/micro-xrce-dds-agent index 155cfaaf..57d08621 160000 --- a/rmcs_ws/src/micro-xrce-dds-agent +++ b/rmcs_ws/src/micro-xrce-dds-agent @@ -1 +1 @@ -Subproject commit 155cfaaf8b7abac2e85d4a62d3649b09ace0be55 +Subproject commit 57d086216d01ec43121845d385894a25987f8a2c diff --git a/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp b/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp index a255a456..264952a7 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp @@ -199,10 +199,8 @@ class Flight const Eigen::Isometry3f odom_to_px4 = odom_to_odin * odin_to_px4; // nav_msgs/Odometry twists are reported in child_frame_id, which is odin1_base_link here. - const Eigen::Vector3f odin_linear_velocity{ - snap.vel_x, snap.vel_y, snap.vel_z}; - const Eigen::Vector3f odin_angular_velocity{ - snap.ang_x, snap.ang_y, snap.ang_z}; + const Eigen::Vector3f odin_linear_velocity{snap.vel_x, snap.vel_y, snap.vel_z}; + const Eigen::Vector3f odin_angular_velocity{snap.ang_x, snap.ang_y, snap.ang_z}; const Eigen::Vector3f px4_velocity_odom = odom_to_odin.linear() * (odin_linear_velocity + odin_angular_velocity.cross(odin_to_px4_offset)); @@ -238,6 +236,9 @@ class Flight try { local_position_interface_.update(meas); + RCLCPP_INFO( + logger_, "Published local position measurement with timestamp %f", + meas.timestamp_sample.seconds()); } catch (const px4_ros2::NavigationInterfaceInvalidArgument& e) { RCLCPP_ERROR_THROTTLE( logger_, *get_clock(), 1000, "Navigation update error: %s", e.what()); @@ -306,9 +307,7 @@ class Flight static const Eigen::Matrix3f rotation = [] { Eigen::Matrix3f matrix; // Odin axes are x->down, y->left, z->front. PX4 expects body FRD. - matrix << 0.f, 0.f, 1.f, - 0.f, -1.f, 0.f, - 1.f, 0.f, 0.f; + matrix << 0.f, 0.f, 1.f, 0.f, -1.f, 0.f, 1.f, 0.f, 0.f; return matrix; }(); return rotation; @@ -318,7 +317,8 @@ class Flight static const Eigen::Vector3f translation = [] { // Odin is mounted 135 mm below, 312 mm right, and 296 mm behind the PX4 controller. // This is the odin1_base_link -> PX4 origin offset expressed in Odin axes. - return Eigen::Vector3f{-0.135f, 0.312f, 0.296f}; + // return Eigen::Vector3f{-0.135f, 0.312f, 0.296f}; + return Eigen::Vector3f{-0.0f, 0.0f, 0.0f}; }(); return translation; } From bb32a0ab44cd7489d41dd366ffdfe8dc71e57297 Mon Sep 17 00:00:00 2001 From: Embers-of-the-Fire Date: Tue, 17 Mar 2026 17:54:06 +0800 Subject: [PATCH 15/23] feat: edit position design --- rmcs_ws/src/rmcs_core/src/hardware/flight.cpp | 67 +++---------------- 1 file changed, 9 insertions(+), 58 deletions(-) diff --git a/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp b/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp index 264952a7..4c19c63f 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp @@ -29,9 +29,7 @@ class Flight , private librmcs::agent::CBoard { public: Flight() - : Node{ - get_component_name(), - rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} + : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} , librmcs::agent::CBoard{} , local_position_interface_{*this, px4_ros2::PoseFrame::LocalNED, px4_ros2::VelocityFrame::LocalNED} , logger_(get_logger()) @@ -182,56 +180,29 @@ class Flight odom_ready_ = false; } - const auto& odin_to_px4_rotation = odin1_base_link_to_px4_rotation(); - const auto& odin_to_px4_offset = odin1_base_link_to_px4_translation(); - Eigen::Quaternionf odom_to_odin_q{snap.q_w, snap.q_x, snap.q_y, snap.q_z}; odom_to_odin_q.normalize(); - Eigen::Isometry3f odom_to_odin = Eigen::Isometry3f::Identity(); - odom_to_odin.linear() = odom_to_odin_q.toRotationMatrix(); - odom_to_odin.translation() = Eigen::Vector3f{snap.pos_x, snap.pos_y, snap.pos_z}; - - Eigen::Isometry3f odin_to_px4 = Eigen::Isometry3f::Identity(); - odin_to_px4.linear() = odin_to_px4_rotation; - odin_to_px4.translation() = odin_to_px4_offset; - - const Eigen::Isometry3f odom_to_px4 = odom_to_odin * odin_to_px4; - - // nav_msgs/Odometry twists are reported in child_frame_id, which is odin1_base_link here. - const Eigen::Vector3f odin_linear_velocity{snap.vel_x, snap.vel_y, snap.vel_z}; - const Eigen::Vector3f odin_angular_velocity{snap.ang_x, snap.ang_y, snap.ang_z}; - const Eigen::Vector3f px4_velocity_odom = - odom_to_odin.linear() - * (odin_linear_velocity + odin_angular_velocity.cross(odin_to_px4_offset)); - - Eigen::Matrix3f odin_velocity_covariance = Eigen::Matrix3f::Zero(); - odin_velocity_covariance.diagonal() = - Eigen::Vector3f{snap.vel_cov_xx, snap.vel_cov_yy, snap.vel_cov_zz}; - const Eigen::Matrix3f px4_velocity_covariance = - odom_to_odin.linear() * odin_velocity_covariance * odom_to_odin.linear().transpose(); - px4_ros2::LocalPositionMeasurement meas{}; meas.timestamp_sample = snap.timestamp_sample.nanoseconds() > 0 ? snap.timestamp_sample : get_clock()->now(); - meas.position_xy = odom_to_px4.translation().head<2>(); + meas.position_xy = Eigen::Vector2f{snap.pos_x, snap.pos_y}; meas.position_xy_variance = Eigen::Vector2f{ snap.cov_xx > 0.f ? snap.cov_xx : 0.1f, snap.cov_yy > 0.f ? snap.cov_yy : 0.1f}; - meas.position_z = odom_to_px4.translation().z(); + meas.position_z = snap.pos_z; meas.position_z_variance = snap.cov_zz > 0.f ? snap.cov_zz : 0.1f; - meas.velocity_xy = px4_velocity_odom.head<2>(); + meas.velocity_xy = Eigen::Vector2f{snap.vel_x, snap.vel_y}; meas.velocity_xy_variance = Eigen::Vector2f{ - px4_velocity_covariance(0, 0) > 0.f ? px4_velocity_covariance(0, 0) : 0.1f, - px4_velocity_covariance(1, 1) > 0.f ? px4_velocity_covariance(1, 1) : 0.1f}; + snap.vel_cov_xx > 0.f ? snap.vel_cov_xx : 0.1f, + snap.vel_cov_yy > 0.f ? snap.vel_cov_yy : 0.1f}; - meas.velocity_z = px4_velocity_odom.z(); - meas.velocity_z_variance = - px4_velocity_covariance(2, 2) > 0.f ? px4_velocity_covariance(2, 2) : 0.1f; + meas.velocity_z = snap.vel_z; + meas.velocity_z_variance = snap.vel_cov_zz > 0.f ? snap.vel_cov_zz : 0.1f; - meas.attitude_quaternion = Eigen::Quaternionf{odom_to_px4.linear()}; + meas.attitude_quaternion = odom_to_odin_q; meas.attitude_variance = Eigen::Vector3f{0.05f, 0.05f, 0.05f}; try { @@ -303,26 +274,6 @@ class Flight } } - static const Eigen::Matrix3f& odin1_base_link_to_px4_rotation() { - static const Eigen::Matrix3f rotation = [] { - Eigen::Matrix3f matrix; - // Odin axes are x->down, y->left, z->front. PX4 expects body FRD. - matrix << 0.f, 0.f, 1.f, 0.f, -1.f, 0.f, 1.f, 0.f, 0.f; - return matrix; - }(); - return rotation; - } - - static const Eigen::Vector3f& odin1_base_link_to_px4_translation() { - static const Eigen::Vector3f translation = [] { - // Odin is mounted 135 mm below, 312 mm right, and 296 mm behind the PX4 controller. - // This is the odin1_base_link -> PX4 origin offset expressed in Odin axes. - // return Eigen::Vector3f{-0.135f, 0.312f, 0.296f}; - return Eigen::Vector3f{-0.0f, 0.0f, 0.0f}; - }(); - return translation; - } - 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) From 490a0a4f39924cff1668bdc515f31aa8d9fd1f55 Mon Sep 17 00:00:00 2001 From: Embers-of-the-Fire Date: Wed, 18 Mar 2026 11:25:54 +0800 Subject: [PATCH 16/23] fix: fix ENU of odom to NED --- rmcs_ws/src/rmcs_core/src/hardware/flight.cpp | 77 ++++++++++++++----- 1 file changed, 59 insertions(+), 18 deletions(-) diff --git a/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp b/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp index 4c19c63f..10a6a12d 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp @@ -1,3 +1,4 @@ +#include #include #include #include @@ -5,6 +6,7 @@ #include #include +#include #include #include @@ -171,6 +173,11 @@ class Flight } void update_local_position() { + // Rate-limit to 50 Hz: skip unless 20 update cycles (20 ms) have elapsed + if (++odom_publish_counter_ < 20) + return; + odom_publish_counter_ = 0; + OdomSnapshot snap; { std::lock_guard lock(odom_mutex_); @@ -180,35 +187,68 @@ class Flight odom_ready_ = false; } - Eigen::Quaternionf odom_to_odin_q{snap.q_w, snap.q_x, snap.q_y, snap.q_z}; - odom_to_odin_q.normalize(); + // Validate: reject any snapshot containing NaN values + if (std::isnan(snap.pos_x) || std::isnan(snap.pos_y) || std::isnan(snap.pos_z) + || std::isnan(snap.vel_x) || std::isnan(snap.vel_y) || std::isnan(snap.vel_z) + || std::isnan(snap.q_w) || std::isnan(snap.q_x) || std::isnan(snap.q_y) + || std::isnan(snap.q_z)) { + RCLCPP_WARN_THROTTLE( + logger_, *get_clock(), 1000, + "Odometry snapshot contains NaN, skipping publish"); + return; + } px4_ros2::LocalPositionMeasurement meas{}; meas.timestamp_sample = snap.timestamp_sample.nanoseconds() > 0 ? snap.timestamp_sample : get_clock()->now(); - meas.position_xy = Eigen::Vector2f{snap.pos_x, snap.pos_y}; - meas.position_xy_variance = Eigen::Vector2f{ - snap.cov_xx > 0.f ? snap.cov_xx : 0.1f, snap.cov_yy > 0.f ? snap.cov_yy : 0.1f}; - - meas.position_z = snap.pos_z; - meas.position_z_variance = snap.cov_zz > 0.f ? snap.cov_zz : 0.1f; - - meas.velocity_xy = Eigen::Vector2f{snap.vel_x, snap.vel_y}; - meas.velocity_xy_variance = Eigen::Vector2f{ - snap.vel_cov_xx > 0.f ? snap.vel_cov_xx : 0.1f, - snap.vel_cov_yy > 0.f ? snap.vel_cov_yy : 0.1f}; + // Odin publishes in ENU frame; PX4 expects NED. + // ENU→NED position: (x_ned, y_ned, z_ned) = (y_enu, x_enu, -z_enu) + float ned_pos_x = snap.pos_y; + float ned_pos_y = snap.pos_x; + float ned_pos_z = -snap.pos_z; + + float ned_vel_x = snap.vel_y; + float ned_vel_y = snap.vel_x; + float ned_vel_z = -snap.vel_z; + + meas.position_xy = Eigen::Vector2f{ned_pos_x, ned_pos_y}; + meas.position_z = ned_pos_z; + + // Covariance: only provide if source has valid (>0) values; swap x↔y for NED + bool pos_cov_valid = snap.cov_xx > 0.f && snap.cov_yy > 0.f && snap.cov_zz > 0.f + && !std::isnan(snap.cov_xx) && !std::isnan(snap.cov_yy) && !std::isnan(snap.cov_zz); + if (pos_cov_valid) { + meas.position_xy_variance = Eigen::Vector2f{snap.cov_yy, snap.cov_xx}; + meas.position_z_variance = snap.cov_zz; + } else { + // No valid covariance from source; omit position entirely rather than fabricate + meas.position_xy = std::nullopt; + meas.position_z = std::nullopt; + } - meas.velocity_z = snap.vel_z; - meas.velocity_z_variance = snap.vel_cov_zz > 0.f ? snap.vel_cov_zz : 0.1f; + bool vel_cov_valid = snap.vel_cov_xx > 0.f && snap.vel_cov_yy > 0.f + && snap.vel_cov_zz > 0.f && !std::isnan(snap.vel_cov_xx) + && !std::isnan(snap.vel_cov_yy) && !std::isnan(snap.vel_cov_zz); + if (vel_cov_valid) { + meas.velocity_xy = Eigen::Vector2f{ned_vel_x, ned_vel_y}; + meas.velocity_xy_variance = Eigen::Vector2f{snap.vel_cov_yy, snap.vel_cov_xx}; + meas.velocity_z = ned_vel_z; + meas.velocity_z_variance = snap.vel_cov_zz; + } + // If vel covariance invalid, omit velocity (leave as nullopt) - meas.attitude_quaternion = odom_to_odin_q; + // Attitude: convert quaternion from ENU to NED + Eigen::Quaternionf q_enu{snap.q_w, snap.q_x, snap.q_y, snap.q_z}; + q_enu.normalize(); + meas.attitude_quaternion = px4_ros2::attitudeEnuToNed(q_enu); meas.attitude_variance = Eigen::Vector3f{0.05f, 0.05f, 0.05f}; try { local_position_interface_.update(meas); - RCLCPP_INFO( - logger_, "Published local position measurement with timestamp %f", + RCLCPP_INFO_THROTTLE( + logger_, *get_clock(), 1000, + "Published local position measurement with timestamp %f", meas.timestamp_sample.seconds()); } catch (const px4_ros2::NavigationInterfaceInvalidArgument& e) { RCLCPP_ERROR_THROTTLE( @@ -239,6 +279,7 @@ class Flight std::mutex odom_mutex_; OdomSnapshot latest_odom_; bool odom_ready_{false}; + uint32_t odom_publish_counter_{0}; void odin_odometry_subscription_callback(const nav_msgs::msg::Odometry& msg) { RCLCPP_INFO_THROTTLE( From 127ecbbfb662c18ae189c839285049b0e9a2c1b1 Mon Sep 17 00:00:00 2001 From: Embers-of-the-Fire Date: Tue, 7 Apr 2026 19:57:31 +0800 Subject: [PATCH 17/23] feat: migrate to mavros on ros2 --- .../rmcs_bringup/config/mavros_bridge.yaml | 31 +++ .../src/rmcs_bringup/launch/rmcs.launch.py | 52 +++-- rmcs_ws/src/rmcs_core/package.xml | 6 +- rmcs_ws/src/rmcs_core/plugins.xml | 1 + .../rmcs_core/src/hardware/flight_mavros.cpp | 192 ++++++++++++++++++ 5 files changed, 262 insertions(+), 20 deletions(-) create mode 100644 rmcs_ws/src/rmcs_bringup/config/mavros_bridge.yaml create mode 100644 rmcs_ws/src/rmcs_core/src/hardware/flight_mavros.cpp diff --git a/rmcs_ws/src/rmcs_bringup/config/mavros_bridge.yaml b/rmcs_ws/src/rmcs_bringup/config/mavros_bridge.yaml new file mode 100644 index 00000000..e8bbfc6d --- /dev/null +++ b/rmcs_ws/src/rmcs_bringup/config/mavros_bridge.yaml @@ -0,0 +1,31 @@ +rmcs_executor: + ros__parameters: + update_rate: 1000.0 + components: + - rmcs_core::hardware::FlightMavros -> flight_mavros_hardware + +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 + +flight_mavros_hardware: + ros__parameters: + odin_odometry_topic: /odin1/odometry + mavros_pose_topic: /mavros/vision_pose/pose_cov + mavros_twist_topic: /mavros/vision_speed/speed_twist_cov + mavros_pose_frame_id: odom diff --git a/rmcs_ws/src/rmcs_bringup/launch/rmcs.launch.py b/rmcs_ws/src/rmcs_bringup/launch/rmcs.launch.py index c84a6265..98d404e0 100644 --- a/rmcs_ws/src/rmcs_bringup/launch/rmcs.launch.py +++ b/rmcs_ws/src/rmcs_bringup/launch/rmcs.launch.py @@ -7,7 +7,7 @@ LaunchDescription, LaunchDescriptionEntity, ) -from launch.actions import ExecuteProcess, LogInfo +from launch.actions import LogInfo from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node @@ -102,26 +102,44 @@ def visit( ) ) - xrce_cfg = config.get("xrce_dds_agent", {}).get("ros__parameters") - if xrce_cfg is not None: - device = xrce_cfg.get("device", "/dev/ttyS0") - baudrate = str(xrce_cfg.get("baudrate", 921600)) + mavros_cfg = config.get("mavros", {}).get("ros__parameters") + if mavros_cfg is not None and mavros_cfg.get("enabled", True): + fcu_url = mavros_cfg.get( + "fcu_url", + f"serial://{mavros_cfg.get('device', '/dev/ttyACM0')}:{mavros_cfg.get('baudrate', 921600)}", + ) + gcs_url = mavros_cfg.get("gcs_url", "") + target_system_id = int(mavros_cfg.get("target_system_id", 1)) + target_component_id = int(mavros_cfg.get("target_component_id", 1)) + fcu_protocol = mavros_cfg.get("fcu_protocol", "v2.0") + respawn = mavros_cfg.get("respawn", True) + respawn_delay = float(mavros_cfg.get("respawn_delay", 1.0)) entities.append( - LogInfo(msg=f"Starting MicroXRCEAgent on {device} @ {baudrate} baud") + LogInfo( + msg=( + f"Starting ROS 2 MAVROS on '{fcu_url}' " + f"(target {target_system_id}:{target_component_id})" + ) + ) ) entities.append( - ExecuteProcess( - cmd=[ - "MicroXRCEAgent", - "serial", - "--dev", - device, - "-b", - baudrate, + Node( + package="mavros", + executable="mavros_node", + namespace="mavros", + parameters=[ + { + "fcu_url": fcu_url, + "gcs_url": gcs_url, + "tgt_system": target_system_id, + "tgt_component": target_component_id, + "fcu_protocol": fcu_protocol, + } ], - output="log", - respawn=True, - respawn_delay=1.0, + respawn=respawn, + respawn_delay=respawn_delay, + output="screen", + emulate_tty=True, ) ) diff --git a/rmcs_ws/src/rmcs_core/package.xml b/rmcs_ws/src/rmcs_core/package.xml index c46d98b2..3e121b4a 100644 --- a/rmcs_ws/src/rmcs_core/package.xml +++ b/rmcs_ws/src/rmcs_core/package.xml @@ -10,6 +10,9 @@ ament_cmake nav_msgs + geometry_msgs + px4_msgs + px4_ros2_cpp rclcpp std_msgs pluginlib @@ -20,9 +23,6 @@ rmcs_msgs rmcs_executor rmcs_description - px4_msgs - px4_ros2_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 03965995..7e850c4f 100644 --- a/rmcs_ws/src/rmcs_core/plugins.xml +++ b/rmcs_ws/src/rmcs_core/plugins.xml @@ -1,5 +1,6 @@ + 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..fdfc33cb --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/flight_mavros.cpp @@ -0,0 +1,192 @@ +#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()) { + mavros_pose_publisher_ = create_publisher( + get_parameter("mavros_pose_topic").as_string(), rclcpp::QoS{10}.reliable()); + mavros_twist_publisher_ = create_publisher( + get_parameter("mavros_twist_topic").as_string(), rclcpp::QoS{10}.reliable()); + + auto odin_odom_qos = rclcpp::QoS{1}; + odin_odom_qos.reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE); + odin_odom_qos.durability(RMW_QOS_POLICY_DURABILITY_VOLATILE); + + odin_odometry_subscription_ = create_subscription( + get_parameter("odin_odometry_topic").as_string(), odin_odom_qos, + [this](const nav_msgs::msg::Odometry::ConstSharedPtr& msg) { + odin_odometry_subscription_callback(*msg); + }); + + RCLCPP_INFO( + logger_, "FlightMavros bridging '%s' -> '%s' and '%s'.", + odin_odometry_subscription_->get_topic_name(), mavros_pose_publisher_->get_topic_name(), + mavros_twist_publisher_->get_topic_name()); + } + + ~FlightMavros() override = default; + + void update() override { update_local_position(); } + +private: + void update_local_position() { + if (++odom_publish_counter_ < 20) + return; + odom_publish_counter_ = 0; + + OdomSnapshot snap; + { + std::lock_guard lock(odom_mutex_); + if (!odom_ready_) + return; + snap = latest_odom_; + odom_ready_ = false; + } + + if (std::isnan(snap.pos_x) || std::isnan(snap.pos_y) || std::isnan(snap.pos_z) + || std::isnan(snap.vel_x) || std::isnan(snap.vel_y) || std::isnan(snap.vel_z) + || std::isnan(snap.q_w) || std::isnan(snap.q_x) || std::isnan(snap.q_y) + || std::isnan(snap.q_z)) { + RCLCPP_WARN_THROTTLE( + logger_, *get_clock(), 1000, "Odometry snapshot contains NaN, skipping publish"); + return; + } + + const auto timestamp = + snap.timestamp_sample.nanoseconds() > 0 ? snap.timestamp_sample : get_clock()->now(); + Eigen::Quaterniond orientation{ + static_cast(snap.q_w), static_cast(snap.q_x), + static_cast(snap.q_y), static_cast(snap.q_z)}; + if (orientation.norm() <= 1e-6) { + RCLCPP_WARN_THROTTLE( + logger_, *get_clock(), 1000, "Odometry snapshot contains invalid quaternion"); + return; + } + orientation.normalize(); + + geometry_msgs::msg::PoseWithCovarianceStamped pose_msg{}; + pose_msg.header.stamp = timestamp; + pose_msg.header.frame_id = get_parameter("mavros_pose_frame_id").as_string(); + pose_msg.pose.pose.position.x = snap.pos_x; + pose_msg.pose.pose.position.y = snap.pos_y; + pose_msg.pose.pose.position.z = snap.pos_z; + pose_msg.pose.pose.orientation.x = orientation.x(); + pose_msg.pose.pose.orientation.y = orientation.y(); + pose_msg.pose.pose.orientation.z = orientation.z(); + pose_msg.pose.pose.orientation.w = orientation.w(); + pose_msg.pose.covariance = make_pose_covariance(snap); + + geometry_msgs::msg::TwistWithCovarianceStamped twist_msg{}; + twist_msg.header.stamp = timestamp; + twist_msg.header.frame_id = get_parameter("mavros_pose_frame_id").as_string(); + twist_msg.twist.twist.linear.x = snap.vel_x; + twist_msg.twist.twist.linear.y = snap.vel_y; + twist_msg.twist.twist.linear.z = snap.vel_z; + twist_msg.twist.twist.angular.x = snap.ang_x; + twist_msg.twist.twist.angular.y = snap.ang_y; + twist_msg.twist.twist.angular.z = snap.ang_z; + twist_msg.twist.covariance = make_twist_covariance(snap); + + mavros_pose_publisher_->publish(pose_msg); + mavros_twist_publisher_->publish(twist_msg); + } + + struct OdomSnapshot { + rclcpp::Time timestamp_sample{}; + float pos_x{}, pos_y{}, pos_z{}; + float vel_x{}, vel_y{}, vel_z{}; + float ang_x{}, ang_y{}, ang_z{}; + float q_x{}, q_y{}, q_z{}, q_w{1.f}; + float cov_xx{}, cov_yy{}, cov_zz{}; + float vel_cov_xx{}, vel_cov_yy{}, vel_cov_zz{}; + }; + + static std::array make_pose_covariance(const OdomSnapshot& snap) { + std::array covariance{}; + covariance[0] = sanitize_covariance(snap.cov_xx, 0.05); + covariance[7] = sanitize_covariance(snap.cov_yy, 0.05); + covariance[14] = sanitize_covariance(snap.cov_zz, 0.05); + covariance[21] = 0.05; + covariance[28] = 0.05; + covariance[35] = 0.05; + return covariance; + } + + static std::array make_twist_covariance(const OdomSnapshot& snap) { + std::array covariance{}; + covariance[0] = sanitize_covariance(snap.vel_cov_xx, 0.1); + covariance[7] = sanitize_covariance(snap.vel_cov_yy, 0.1); + covariance[14] = sanitize_covariance(snap.vel_cov_zz, 0.1); + covariance[21] = 0.1; + covariance[28] = 0.1; + covariance[35] = 0.1; + return covariance; + } + + static double sanitize_covariance(float value, double fallback) { + return std::isfinite(value) && value > 0.f ? static_cast(value) : fallback; + } + + void odin_odometry_subscription_callback(const nav_msgs::msg::Odometry& msg) { + OdomSnapshot snap{}; + snap.timestamp_sample = rclcpp::Time{msg.header.stamp}; + snap.pos_x = static_cast(msg.pose.pose.position.x); + snap.pos_y = static_cast(msg.pose.pose.position.y); + snap.pos_z = static_cast(msg.pose.pose.position.z); + snap.vel_x = static_cast(msg.twist.twist.linear.x); + snap.vel_y = static_cast(msg.twist.twist.linear.y); + snap.vel_z = static_cast(msg.twist.twist.linear.z); + snap.ang_x = static_cast(msg.twist.twist.angular.x); + snap.ang_y = static_cast(msg.twist.twist.angular.y); + snap.ang_z = static_cast(msg.twist.twist.angular.z); + snap.q_x = static_cast(msg.pose.pose.orientation.x); + snap.q_y = static_cast(msg.pose.pose.orientation.y); + snap.q_z = static_cast(msg.pose.pose.orientation.z); + snap.q_w = static_cast(msg.pose.pose.orientation.w); + snap.cov_xx = static_cast(msg.pose.covariance[0]); + snap.cov_yy = static_cast(msg.pose.covariance[7]); + snap.cov_zz = static_cast(msg.pose.covariance[14]); + snap.vel_cov_xx = static_cast(msg.twist.covariance[0]); + snap.vel_cov_yy = static_cast(msg.twist.covariance[7]); + snap.vel_cov_zz = static_cast(msg.twist.covariance[14]); + + std::lock_guard lock(odom_mutex_); + latest_odom_ = snap; + odom_ready_ = true; + } + + rclcpp::Logger logger_; + rclcpp::Publisher::SharedPtr + mavros_pose_publisher_; + rclcpp::Publisher::SharedPtr + mavros_twist_publisher_; + rclcpp::Subscription::SharedPtr odin_odometry_subscription_; + + std::mutex odom_mutex_; + OdomSnapshot latest_odom_; + bool odom_ready_{false}; + uint32_t odom_publish_counter_{0}; +}; + +} // namespace rmcs_core::hardware + +#include +PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::FlightMavros, rmcs_executor::Component) From 32a01c50d4cadaf650d4a11d8825f7f52b2bf7d4 Mon Sep 17 00:00:00 2001 From: Embers-of-the-Fire Date: Mon, 13 Apr 2026 19:35:27 +0800 Subject: [PATCH 18/23] feat: add local position estimator --- PREREQUISITE.md | 6 + .../config/.mavros_bridge.yaml.swp | Bin 0 -> 12288 bytes .../rmcs_bringup/config/mavros_bridge.yaml | 11 +- .../src/rmcs_bringup/launch/rmcs.launch.py | 9 +- rmcs_ws/src/rmcs_core/package.xml | 1 + .../rmcs_core/src/hardware/flight_mavros.cpp | 293 +++++++++--------- 6 files changed, 172 insertions(+), 148 deletions(-) create mode 100644 rmcs_ws/src/rmcs_bringup/config/.mavros_bridge.yaml.swp diff --git a/PREREQUISITE.md b/PREREQUISITE.md index 819635b1..55036084 100644 --- a/PREREQUISITE.md +++ b/PREREQUISITE.md @@ -76,3 +76,9 @@ sudo apt-get install ros-jazzy-cv-bridge Note that you might have to modify Odin1's source code to compile it. Some of the code refer to `` instead of `.hpp`. + +## Conversions + +When drone moving forward, odin1's `position.x` decreases; +When drone moving left, odin1's `position.y` decreases; +When drone moving up, odin1's `position.z` increases. diff --git a/rmcs_ws/src/rmcs_bringup/config/.mavros_bridge.yaml.swp b/rmcs_ws/src/rmcs_bringup/config/.mavros_bridge.yaml.swp new file mode 100644 index 0000000000000000000000000000000000000000..2925efeeb5bbfdf3647e1db4bc74068bb62b3912 GIT binary patch literal 12288 zcmeI2F>f426vxLYlNcf>BBdJa1`+vo&ruS*LP${%D8&iM5mu6BnA@GZ8+mtTnVG%w z6^M!w(ShhwMk%R-1T>TsP(+dP1tNt{z<+1gQ4*gLoCYO&EB(E`c{B6g`@MICYL%>tDPjT9bu!`@WXB^2zS#>h;7` zxfctcc$as-kDWHtrcQEeCVHAwa_+1bSS_Y%u#{E#AH2{OB0vO=5NN_=>%(<+VSAXi zw#|+A_`C0Xabz1U5CI}U1c(3;AOb{y2oM1x@a7T-^&0yI>7D3PJm0_9o_qHn=|u#H z01+SpM1Tko0U|&IhyW2F0z`layoLm1!PsXf89Q|f!Q=n`hkyS+ew(oe(C^UCkb}O0 zG_(hO0-b|SLjSzQ*k8~+=q~gl^aJ!Ql2|&O&R@ zFQ;LLzK1gCedtfbcn{i#Zb2VFZQL>T*N{DhOazDk5g-CYfCvx)BJe*F;PL4vPHXFx zm^fJ}p^J>$!d9%cKWz@S)_KPJA=PBAy|zZQCzJL^+`p6yk;^LM=^%{@n|#*>859pQ z#l3A@u6XprrL&DGxDKA%h2gijJkXv8%Vnvj2J3vG!z`W~+hWL`nh=w zKhBorE4sOH3^$v6DyvEgObeHQKO9`VoL=6!bRp*YjAm5Mbgt`gBp)eW0czA19o>grSq?@Ynj76KF7)x|bsm*?0>J=BP z+P31h*lz9Uv#4d;g+V!=u?ae17DV5KGwF(jbSh)fol}^sUKmlRQZ8F+UBmUu84s@M zdeYcJ34=>BKC?auU+fDntGevW-RQNL=n@+?y0L6+e2`_Cc=%L_EwmABaD~%z9o9BWh=Z&25?S`F5Xr&lU01)+iI= zS9usC>P>#>gQ}idpJE=pakyloi@d!#e)h^~S!PL+6l$J?u)K10lx92I!%Jz$`K38_ zRqIpZe2d57_Z*FxWlu_(eHu$VigWLL>tnvUstd_msgs pluginlib tf2 + tf2_msgs tf2_ros serial rmcs_utility 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 fdfc33cb..46910414 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/flight_mavros.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/flight_mavros.cpp @@ -1,14 +1,19 @@ -#include #include #include #include +#include +#include #include -#include -#include -#include + +#include +#include +#include #include #include +#include +#include +#include #include @@ -20,170 +25,170 @@ class FlightMavros public: FlightMavros() : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} - , logger_(get_logger()) { - mavros_pose_publisher_ = create_publisher( - get_parameter("mavros_pose_topic").as_string(), rclcpp::QoS{10}.reliable()); - mavros_twist_publisher_ = create_publisher( - get_parameter("mavros_twist_topic").as_string(), rclcpp::QoS{10}.reliable()); - - auto odin_odom_qos = rclcpp::QoS{1}; - odin_odom_qos.reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE); - odin_odom_qos.durability(RMW_QOS_POLICY_DURABILITY_VOLATILE); - - odin_odometry_subscription_ = create_subscription( - get_parameter("odin_odometry_topic").as_string(), odin_odom_qos, - [this](const nav_msgs::msg::Odometry::ConstSharedPtr& msg) { - odin_odometry_subscription_callback(*msg); - }); + , logger_(get_logger()) + , tf_buffer_(get_clock()) + , tf_listener_(tf_buffer_) { + load_parameters(); + + mavros_pose_publisher_ = create_publisher( + mavros_pose_topic_, rclcpp::QoS{10}.reliable()); RCLCPP_INFO( - logger_, "FlightMavros bridging '%s' -> '%s' and '%s'.", - odin_odometry_subscription_->get_topic_name(), mavros_pose_publisher_->get_topic_name(), - mavros_twist_publisher_->get_topic_name()); + 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_local_position(); } + void update() override { + update_pose_from_tf(); + if (++update_counter_ >= publish_interval_updates_) { + update_counter_ = 0; + publish_pose(); + } + } private: - void update_local_position() { - if (++odom_publish_counter_ < 20) - return; - odom_publish_counter_ = 0; + static double radians_to_degrees(double radians) { + return radians * 180.0 / std::numbers::pi_v; + } - OdomSnapshot snap; - { - std::lock_guard lock(odom_mutex_); - if (!odom_ready_) - return; - snap = latest_odom_; - odom_ready_ = false; - } + 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(); + + publish_interval_updates_ = + static_cast(std::max(1.0, std::round(1000.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}; - if (std::isnan(snap.pos_x) || std::isnan(snap.pos_y) || std::isnan(snap.pos_z) - || std::isnan(snap.vel_x) || std::isnan(snap.vel_y) || std::isnan(snap.vel_z) - || std::isnan(snap.q_w) || std::isnan(snap.q_x) || std::isnan(snap.q_y) - || std::isnan(snap.q_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, "Odometry snapshot contains NaN, skipping publish"); + logger_, *get_clock(), 1000, "TF lookup failed for '%s' -> '%s': %s", + source_frame_id_.c_str(), target_frame_id_.c_str(), ex.what()); return; } - const auto timestamp = - snap.timestamp_sample.nanoseconds() > 0 ? snap.timestamp_sample : get_clock()->now(); - Eigen::Quaterniond orientation{ - static_cast(snap.q_w), static_cast(snap.q_x), - static_cast(snap.q_y), static_cast(snap.q_z)}; - if (orientation.norm() <= 1e-6) { - RCLCPP_WARN_THROTTLE( - logger_, *get_clock(), 1000, "Odometry snapshot contains invalid quaternion"); + if (!finite_transform(transform)) return; - } - orientation.normalize(); - - geometry_msgs::msg::PoseWithCovarianceStamped pose_msg{}; - pose_msg.header.stamp = timestamp; - pose_msg.header.frame_id = get_parameter("mavros_pose_frame_id").as_string(); - pose_msg.pose.pose.position.x = snap.pos_x; - pose_msg.pose.pose.position.y = snap.pos_y; - pose_msg.pose.pose.position.z = snap.pos_z; - pose_msg.pose.pose.orientation.x = orientation.x(); - pose_msg.pose.pose.orientation.y = orientation.y(); - pose_msg.pose.pose.orientation.z = orientation.z(); - pose_msg.pose.pose.orientation.w = orientation.w(); - pose_msg.pose.covariance = make_pose_covariance(snap); - - geometry_msgs::msg::TwistWithCovarianceStamped twist_msg{}; - twist_msg.header.stamp = timestamp; - twist_msg.header.frame_id = get_parameter("mavros_pose_frame_id").as_string(); - twist_msg.twist.twist.linear.x = snap.vel_x; - twist_msg.twist.twist.linear.y = snap.vel_y; - twist_msg.twist.twist.linear.z = snap.vel_z; - twist_msg.twist.twist.angular.x = snap.ang_x; - twist_msg.twist.twist.angular.y = snap.ang_y; - twist_msg.twist.twist.angular.z = snap.ang_z; - twist_msg.twist.covariance = make_twist_covariance(snap); - mavros_pose_publisher_->publish(pose_msg); - mavros_twist_publisher_->publish(twist_msg); - } + const rclcpp::Time tf_stamp{transform.header.stamp}; - struct OdomSnapshot { - rclcpp::Time timestamp_sample{}; - float pos_x{}, pos_y{}, pos_z{}; - float vel_x{}, vel_y{}, vel_z{}; - float ang_x{}, ang_y{}, ang_z{}; - float q_x{}, q_y{}, q_z{}, q_w{1.f}; - float cov_xx{}, cov_yy{}, cov_zz{}; - float vel_cov_xx{}, vel_cov_yy{}, vel_cov_zz{}; - }; - - static std::array make_pose_covariance(const OdomSnapshot& snap) { - std::array covariance{}; - covariance[0] = sanitize_covariance(snap.cov_xx, 0.05); - covariance[7] = sanitize_covariance(snap.cov_yy, 0.05); - covariance[14] = sanitize_covariance(snap.cov_zz, 0.05); - covariance[21] = 0.05; - covariance[28] = 0.05; - covariance[35] = 0.05; - return covariance; - } + std::lock_guard lock(pose_mutex_); + if (last_tf_stamp_.nanoseconds() != 0 && tf_stamp <= last_tf_stamp_) + return; - static std::array make_twist_covariance(const OdomSnapshot& snap) { - std::array covariance{}; - covariance[0] = sanitize_covariance(snap.vel_cov_xx, 0.1); - covariance[7] = sanitize_covariance(snap.vel_cov_yy, 0.1); - covariance[14] = sanitize_covariance(snap.vel_cov_zz, 0.1); - covariance[21] = 0.1; - covariance[28] = 0.1; - covariance[35] = 0.1; - return covariance; + 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_ = tf_stamp; + pose_ready_ = true; + + RCLCPP_INFO_THROTTLE( + logger_, *get_clock(), 1000, "Received TF '%s' -> '%s'.", source_frame_id_.c_str(), + target_frame_id_.c_str()); } - static double sanitize_covariance(float value, double fallback) { - return std::isfinite(value) && value > 0.f ? static_cast(value) : fallback; - } + void publish_pose() { + geometry_msgs::msg::Pose pose; + { + std::lock_guard lock(pose_mutex_); + if (!pose_ready_) + return; + + pose = latest_pose_; + } + + geometry_msgs::msg::PoseStamped pose_msg{}; + pose_msg.header.stamp = get_clock()->now(); + pose_msg.header.frame_id = mavros_pose_frame_id_; + pose_msg.pose = pose; + mavros_pose_publisher_->publish(pose_msg); - void odin_odometry_subscription_callback(const nav_msgs::msg::Odometry& msg) { - OdomSnapshot snap{}; - snap.timestamp_sample = rclcpp::Time{msg.header.stamp}; - snap.pos_x = static_cast(msg.pose.pose.position.x); - snap.pos_y = static_cast(msg.pose.pose.position.y); - snap.pos_z = static_cast(msg.pose.pose.position.z); - snap.vel_x = static_cast(msg.twist.twist.linear.x); - snap.vel_y = static_cast(msg.twist.twist.linear.y); - snap.vel_z = static_cast(msg.twist.twist.linear.z); - snap.ang_x = static_cast(msg.twist.twist.angular.x); - snap.ang_y = static_cast(msg.twist.twist.angular.y); - snap.ang_z = static_cast(msg.twist.twist.angular.z); - snap.q_x = static_cast(msg.pose.pose.orientation.x); - snap.q_y = static_cast(msg.pose.pose.orientation.y); - snap.q_z = static_cast(msg.pose.pose.orientation.z); - snap.q_w = static_cast(msg.pose.pose.orientation.w); - snap.cov_xx = static_cast(msg.pose.covariance[0]); - snap.cov_yy = static_cast(msg.pose.covariance[7]); - snap.cov_zz = static_cast(msg.pose.covariance[14]); - snap.vel_cov_xx = static_cast(msg.twist.covariance[0]); - snap.vel_cov_yy = static_cast(msg.twist.covariance[7]); - snap.vel_cov_zz = static_cast(msg.twist.covariance[14]); - - std::lock_guard lock(odom_mutex_); - latest_odom_ = snap; - odom_ready_ = true; + 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_; - rclcpp::Publisher::SharedPtr - mavros_pose_publisher_; - rclcpp::Publisher::SharedPtr - mavros_twist_publisher_; - rclcpp::Subscription::SharedPtr odin_odometry_subscription_; - - std::mutex odom_mutex_; - OdomSnapshot latest_odom_; - bool odom_ready_{false}; - uint32_t odom_publish_counter_{0}; + 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_; + std::uint32_t publish_interval_updates_; + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; + + rclcpp::Publisher::SharedPtr mavros_pose_publisher_; + + std::mutex pose_mutex_; + geometry_msgs::msg::Pose latest_pose_{}; + rclcpp::Time last_tf_stamp_{}; + std::uint32_t update_counter_{0}; + bool pose_ready_{false}; }; } // namespace rmcs_core::hardware From d11d3f4f78fde958211aafe25e0990aab909e155 Mon Sep 17 00:00:00 2001 From: Embers-of-the-Fire Date: Thu, 16 Apr 2026 11:58:56 +0800 Subject: [PATCH 19/23] feat: remove px4 dependencies and add mavros fix --- .gitmodules | 12 --- PREREQUISITE.md | 84 ------------------ rmcs_ws/src/micro-xrce-dds-agent | 1 - rmcs_ws/src/odin_ros_driver | 2 +- rmcs_ws/src/px4-ros2-interface-lib | 1 - rmcs_ws/src/px4_msgs | 1 - rmcs_ws/src/px4_ros_com | 1 - .../config/.mavros_bridge.yaml.swp | Bin 12288 -> 0 bytes .../rmcs_bringup/config/mavros_bridge.yaml | 16 ++-- rmcs_ws/src/rmcs_core/package.xml | 2 - .../rmcs_core/src/hardware/flight_mavros.cpp | 19 +++- 11 files changed, 27 insertions(+), 112 deletions(-) delete mode 100644 PREREQUISITE.md delete mode 160000 rmcs_ws/src/micro-xrce-dds-agent delete mode 160000 rmcs_ws/src/px4-ros2-interface-lib delete mode 160000 rmcs_ws/src/px4_msgs delete mode 160000 rmcs_ws/src/px4_ros_com delete mode 100644 rmcs_ws/src/rmcs_bringup/config/.mavros_bridge.yaml.swp diff --git a/.gitmodules b/.gitmodules index 89243d69..58c13305 100644 --- a/.gitmodules +++ b/.gitmodules @@ -7,18 +7,6 @@ [submodule "rmcs_ws/src/serial"] path = rmcs_ws/src/serial url = https://github.com/Alliance-Algorithm/ros2-serial.git -[submodule "rmcs_ws/src/micro-xrce-dds-agent"] - path = rmcs_ws/src/micro-xrce-dds-agent - url = https://github.com/eProsima/Micro-XRCE-DDS-Agent.git -[submodule "rmcs_ws/src/px4_msgs"] - path = rmcs_ws/src/px4_msgs - url = https://github.com/PX4/px4_msgs.git -[submodule "rmcs_ws/src/px4_ros_com"] - path = rmcs_ws/src/px4_ros_com - url = https://github.com/PX4/px4_ros_com.git -[submodule "rmcs_ws/src/px4-ros2-interface-lib"] - path = rmcs_ws/src/px4-ros2-interface-lib - url = https://github.com/Auterion/px4-ros2-interface-lib [submodule "rmcs_ws/src/odin_ros_driver"] path = rmcs_ws/src/odin_ros_driver url = https://github.com/Alliance-Algorithm/odin_ros_driver.git diff --git a/PREREQUISITE.md b/PREREQUISITE.md deleted file mode 100644 index 55036084..00000000 --- a/PREREQUISITE.md +++ /dev/null @@ -1,84 +0,0 @@ -# Prerequisite - -To build/develop the flight's branch, you have to do the following tasks as they're not included either in the image nor in package managers. - -## Dependencies for Uxrce DDS Client - -The `micro-xrce-dds-agent` requires Fast-DDS to be installed. -That package is not included in the official APT package source, so we have to compile it manually. - -First, install ASIO. - -```bash -sudo apt-get install libasio-dev -``` - -Then, compile the library from source. - -```bash -mkdir -p /tmp/fastdds -git clone https://github.com/eProsima/Fast-CDR.git -mkdir Fast-CDR/build && cd Fast-CDR/build -cmake .. -DCMAKE_INSTALL_PREFIX=/workspaces/RMCS/rmcs_ws/install -DBUILD_SHARED_LIBS=OFF -DCMAKE_POSITION_INDEPENDENT_CODE=ON -cmake --build . --target install -j -cd /tmp/fastdds && git clone https://github.com/eProsima/Fast-DDS.git -mkdir Fast-DDS/build && cd Fast-DDS/build -cmake .. -DCMAKE_INSTALL_PREFIX=/workspaces/RMCS/rmcs_ws/install -DBUILD_SHARED_LIBS=OFF -DCMAKE_POSITION_INDEPENDENT_CODE=ON -cmake --build . --target install -j -cd / && rm -rf /tmp/fastdds -``` - -This would roughly take ~50s to compile for a 9950x cpu using 16 threads. - -And then compile the agent: - -```bash -build-rmcs \ - --packages-skip microxrcedds_agent \ - --cmake-clean-cache \ - --cmake-force-configure - -build-rmcs \ - --packages-select microxrcedds_agent \ - --cmake-clean-cache \ - --cmake-force-configure \ - --cmake-args \ - '-DBUILD_SHARED_LIBS=OFF' \ - '-DUAGENT_BUILD_EXECUTABLE=ON' \ - '-DUAGENT_USE_SYSTEM_FASTCDR=ON' \ - '-DUAGENT_USE_SYSTEM_FASTDDS=ON' \ - '-Dfastcdr_SHARED_LIBS=OFF' \ - '-Dfastdds_SHARED_LIBS=OFF' \ - '-Dfastcdr_DIR=/workspaces/RMCS/rmcs_ws/install/lib/cmake/fastcdr' \ - '-Dfastdds_DIR=/workspaces/RMCS/rmcs_ws/install/share/fastdds/cmake' \ - '-DCMAKE_PREFIX_PATH:PATH=/workspaces/RMCS/rmcs_ws/install;/opt/ros/jazzy' -``` - -This would roughly take ~30s to compile for a 9950x cpu using 16 threads. - -To run the uxrce dds client: - -```bash -sudo MicroXRCEAgent serial --dev /dev/ttyS0 -b 921600 -``` - -Note that the baudrate should be the same as the one configured in the flight controller. - -## Dependencies for Odin1 Driver - -Odin1 driver requires cv-bridge which is no longer used by the main upstream. - -@zlq04222 - -```bash -sudo apt-get install ros-jazzy-cv-bridge -``` - -Note that you might have to modify Odin1's source code to compile it. -Some of the code refer to `` instead of `.hpp`. - -## Conversions - -When drone moving forward, odin1's `position.x` decreases; -When drone moving left, odin1's `position.y` decreases; -When drone moving up, odin1's `position.z` increases. diff --git a/rmcs_ws/src/micro-xrce-dds-agent b/rmcs_ws/src/micro-xrce-dds-agent deleted file mode 160000 index 57d08621..00000000 --- a/rmcs_ws/src/micro-xrce-dds-agent +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 57d086216d01ec43121845d385894a25987f8a2c diff --git a/rmcs_ws/src/odin_ros_driver b/rmcs_ws/src/odin_ros_driver index 8c47e655..0c5df801 160000 --- a/rmcs_ws/src/odin_ros_driver +++ b/rmcs_ws/src/odin_ros_driver @@ -1 +1 @@ -Subproject commit 8c47e655c2f158c82083cb4d90c6cb47ff00aaf7 +Subproject commit 0c5df801cd96034b0de2c05d5d2c7c87705c1012 diff --git a/rmcs_ws/src/px4-ros2-interface-lib b/rmcs_ws/src/px4-ros2-interface-lib deleted file mode 160000 index 4fb88a1b..00000000 --- a/rmcs_ws/src/px4-ros2-interface-lib +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 4fb88a1b3da69b417ceb1e42d4870a33e58938c2 diff --git a/rmcs_ws/src/px4_msgs b/rmcs_ws/src/px4_msgs deleted file mode 160000 index eb4edb4f..00000000 --- a/rmcs_ws/src/px4_msgs +++ /dev/null @@ -1 +0,0 @@ -Subproject commit eb4edb4f7376e9a4732498a4e8b637c3d557a964 diff --git a/rmcs_ws/src/px4_ros_com b/rmcs_ws/src/px4_ros_com deleted file mode 160000 index 86e9aeb2..00000000 --- a/rmcs_ws/src/px4_ros_com +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 86e9aeb20e55a4673fa8a9f1c29ea06a6c5ad1af diff --git a/rmcs_ws/src/rmcs_bringup/config/.mavros_bridge.yaml.swp b/rmcs_ws/src/rmcs_bringup/config/.mavros_bridge.yaml.swp deleted file mode 100644 index 2925efeeb5bbfdf3647e1db4bc74068bb62b3912..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 12288 zcmeI2F>f426vxLYlNcf>BBdJa1`+vo&ruS*LP${%D8&iM5mu6BnA@GZ8+mtTnVG%w z6^M!w(ShhwMk%R-1T>TsP(+dP1tNt{z<+1gQ4*gLoCYO&EB(E`c{B6g`@MICYL%>tDPjT9bu!`@WXB^2zS#>h;7` zxfctcc$as-kDWHtrcQEeCVHAwa_+1bSS_Y%u#{E#AH2{OB0vO=5NN_=>%(<+VSAXi zw#|+A_`C0Xabz1U5CI}U1c(3;AOb{y2oM1x@a7T-^&0yI>7D3PJm0_9o_qHn=|u#H z01+SpM1Tko0U|&IhyW2F0z`layoLm1!PsXf89Q|f!Q=n`hkyS+ew(oe(C^UCkb}O0 zG_(hO0-b|SLjSzQ*k8~+=q~gl^aJ!Ql2|&O&R@ zFQ;LLzK1gCedtfbcn{i#Zb2VFZQL>T*N{DhOazDk5g-CYfCvx)BJe*F;PL4vPHXFx zm^fJ}p^J>$!d9%cKWz@S)_KPJA=PBAy|zZQCzJL^+`p6yk;^LM=^%{@n|#*>859pQ z#l3A@u6XprrL&DGxDKA%h2gijJkXv8%Vnvj2J3vG!z`W~+hWL`nh=w zKhBorE4sOH3^$v6DyvEgObeHQKO9`VoL=6!bRp*YjAm5Mbgt`gBp)eW0czA19o>grSq?@Ynj76KF7)x|bsm*?0>J=BP z+P31h*lz9Uv#4d;g+V!=u?ae17DV5KGwF(jbSh)fol}^sUKmlRQZ8F+UBmUu84s@M zdeYcJ34=>BKC?auU+fDntGevW-RQNL=n@+?y0L6+e2`_Cc=%L_EwmABaD~%z9o9BWh=Z&25?S`F5Xr&lU01)+iI= zS9usC>P>#>gQ}idpJE=pakyloi@d!#e)h^~S!PL+6l$J?u)K10lx92I!%Jz$`K38_ zRqIpZe2d57_Z*FxWlu_(eHu$VigWLL>tnvU flight_mavros_hardware - mavros: ros__parameters: enabled: true @@ -14,23 +14,23 @@ mavros: 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" + config_file: >- + /rmcs_install/share/odin_ros_driver/config/control_command.yaml node_name: host_sdk_sample respawn: true respawn_delay: 1.0 - flight_mavros_hardware: ros__parameters: target_frame_id: odom source_frame_id: odin1_base_link - output_rate: 50.0 - # Keep odom position as-is; only correct the vision sensor orientation. - sensor_roll_offset_rad: 0.0 - sensor_pitch_offset_rad: 0.0 + output_rate: 30.0 + # Keep odom position as-is; only correct the vision sensor orientation for + # the reversed front-facing Odin mount. + 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 diff --git a/rmcs_ws/src/rmcs_core/package.xml b/rmcs_ws/src/rmcs_core/package.xml index 7198e3a4..a49a7a89 100644 --- a/rmcs_ws/src/rmcs_core/package.xml +++ b/rmcs_ws/src/rmcs_core/package.xml @@ -11,8 +11,6 @@ nav_msgs geometry_msgs - px4_msgs - px4_ros2_cpp rclcpp std_msgs pluginlib 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 46910414..9f1f6eb5 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/flight_mavros.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/flight_mavros.cpp @@ -150,20 +150,36 @@ class FlightMavros void publish_pose() { geometry_msgs::msg::Pose pose; + rclcpp::Time tf_stamp; { std::lock_guard lock(pose_mutex_); if (!pose_ready_) return; + if (last_tf_stamp_ == last_published_tf_stamp_) + return; + + tf_stamp = last_tf_stamp_; pose = latest_pose_; } + const rclcpp::Duration max_pose_age = rclcpp::Duration::from_seconds(1.0 / output_rate_hz_); + const rclcpp::Time now = get_clock()->now(); + if ((now - tf_stamp) >= max_pose_age) + return; + geometry_msgs::msg::PoseStamped pose_msg{}; - pose_msg.header.stamp = get_clock()->now(); + pose_msg.header.stamp = now; pose_msg.header.frame_id = mavros_pose_frame_id_; pose_msg.pose = pose; mavros_pose_publisher_->publish(pose_msg); + { + std::lock_guard lock(pose_mutex_); + if (last_tf_stamp_ == tf_stamp) + last_published_tf_stamp_ = tf_stamp; + } + 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()); @@ -187,6 +203,7 @@ class FlightMavros std::mutex pose_mutex_; geometry_msgs::msg::Pose latest_pose_{}; rclcpp::Time last_tf_stamp_{}; + rclcpp::Time last_published_tf_stamp_{}; std::uint32_t update_counter_{0}; bool pose_ready_{false}; }; From cbecb5c875a982dbce30aa2ec15f57065f0bbda2 Mon Sep 17 00:00:00 2001 From: Embers-of-the-Fire Date: Sat, 18 Apr 2026 14:04:17 +0800 Subject: [PATCH 20/23] feat: update odin compatibility Signed-off-by: Embers-of-the-Fire --- rmcs_ws/src/odin_ros_driver | 2 +- .../rmcs_core/src/hardware/flight_mavros.cpp | 29 +++++++++++-------- 2 files changed, 18 insertions(+), 13 deletions(-) diff --git a/rmcs_ws/src/odin_ros_driver b/rmcs_ws/src/odin_ros_driver index 0c5df801..dbcb0286 160000 --- a/rmcs_ws/src/odin_ros_driver +++ b/rmcs_ws/src/odin_ros_driver @@ -1 +1 @@ -Subproject commit 0c5df801cd96034b0de2c05d5d2c7c87705c1012 +Subproject commit dbcb0286b510142e966ac090a37ce5ca238f9e75 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 9f1f6eb5..ccedf6f4 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/flight_mavros.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/flight_mavros.cpp @@ -27,7 +27,8 @@ class FlightMavros : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} , logger_(get_logger()) , tf_buffer_(get_clock()) - , tf_listener_(tf_buffer_) { + , tf_listener_(tf_buffer_) + , last_pose_received_time_{0, 0, get_clock()->get_clock_type()} { load_parameters(); mavros_pose_publisher_ = create_publisher( @@ -122,10 +123,10 @@ class FlightMavros if (!finite_transform(transform)) return; - const rclcpp::Time tf_stamp{transform.header.stamp}; + const auto tf_stamp_ns = rclcpp::Time{transform.header.stamp}.nanoseconds(); std::lock_guard lock(pose_mutex_); - if (last_tf_stamp_.nanoseconds() != 0 && tf_stamp <= last_tf_stamp_) + if (last_tf_stamp_ns_ != 0 && tf_stamp_ns <= last_tf_stamp_ns_) return; const Eigen::Vector3d rotated_position = @@ -140,7 +141,8 @@ class FlightMavros latest_pose_.orientation.y = rotated_orientation.y(); latest_pose_.orientation.z = rotated_orientation.z(); latest_pose_.orientation.w = rotated_orientation.w(); - last_tf_stamp_ = tf_stamp; + last_tf_stamp_ns_ = tf_stamp_ns; + last_pose_received_time_ = get_clock()->now(); pose_ready_ = true; RCLCPP_INFO_THROTTLE( @@ -150,22 +152,24 @@ class FlightMavros void publish_pose() { geometry_msgs::msg::Pose pose; - rclcpp::Time tf_stamp; + rclcpp::Time pose_received_time{0, 0, get_clock()->get_clock_type()}; + std::int64_t tf_stamp_ns = 0; { std::lock_guard lock(pose_mutex_); if (!pose_ready_) return; - if (last_tf_stamp_ == last_published_tf_stamp_) + if (last_tf_stamp_ns_ == last_published_tf_stamp_ns_) return; - tf_stamp = last_tf_stamp_; + tf_stamp_ns = last_tf_stamp_ns_; + pose_received_time = last_pose_received_time_; pose = latest_pose_; } const rclcpp::Duration max_pose_age = rclcpp::Duration::from_seconds(1.0 / output_rate_hz_); const rclcpp::Time now = get_clock()->now(); - if ((now - tf_stamp) >= max_pose_age) + if ((now - pose_received_time) >= max_pose_age) return; geometry_msgs::msg::PoseStamped pose_msg{}; @@ -176,8 +180,8 @@ class FlightMavros { std::lock_guard lock(pose_mutex_); - if (last_tf_stamp_ == tf_stamp) - last_published_tf_stamp_ = tf_stamp; + if (last_tf_stamp_ns_ == tf_stamp_ns) + last_published_tf_stamp_ns_ = tf_stamp_ns; } RCLCPP_INFO_THROTTLE( @@ -202,8 +206,9 @@ class FlightMavros std::mutex pose_mutex_; geometry_msgs::msg::Pose latest_pose_{}; - rclcpp::Time last_tf_stamp_{}; - rclcpp::Time last_published_tf_stamp_{}; + std::int64_t last_tf_stamp_ns_{0}; + std::int64_t last_published_tf_stamp_ns_{0}; + rclcpp::Time last_pose_received_time_; std::uint32_t update_counter_{0}; bool pose_ready_{false}; }; From 5fe76ac9bf4c0b28ab1a10f0ce54687c859936e6 Mon Sep 17 00:00:00 2001 From: Embers-of-the-Fire Date: Sat, 18 Apr 2026 15:00:00 +0800 Subject: [PATCH 21/23] fix: restore dlss --- .../rmcs_core/src/hardware/flight_mavros.cpp | 43 +++++++++---------- 1 file changed, 20 insertions(+), 23 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 ccedf6f4..3061b9a9 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/flight_mavros.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/flight_mavros.cpp @@ -48,8 +48,9 @@ class FlightMavros void update() override { update_pose_from_tf(); - if (++update_counter_ >= publish_interval_updates_) { - update_counter_ = 0; + publish_credit_ += output_rate_hz_ / 1000.0; + while (publish_credit_ >= 1.0) { + publish_credit_ -= 1.0; publish_pose(); } } @@ -76,8 +77,7 @@ class FlightMavros sensor_pitch_offset_rad_ = get_parameter("sensor_pitch_offset_rad").as_double(); sensor_yaw_offset_rad_ = get_parameter("sensor_yaw_offset_rad").as_double(); - publish_interval_updates_ = - static_cast(std::max(1.0, std::round(1000.0 / output_rate_hz_))); + output_rate_hz_ = std::max(0.0, output_rate_hz_); } Eigen::Quaterniond sensor_to_body_rotation() const { @@ -153,37 +153,35 @@ class FlightMavros void publish_pose() { geometry_msgs::msg::Pose pose; rclcpp::Time pose_received_time{0, 0, get_clock()->get_clock_type()}; - std::int64_t tf_stamp_ns = 0; { std::lock_guard lock(pose_mutex_); if (!pose_ready_) return; - - if (last_tf_stamp_ns_ == last_published_tf_stamp_ns_) - return; - - tf_stamp_ns = last_tf_stamp_ns_; pose_received_time = last_pose_received_time_; pose = latest_pose_; } - const rclcpp::Duration max_pose_age = rclcpp::Duration::from_seconds(1.0 / output_rate_hz_); - const rclcpp::Time now = get_clock()->now(); - if ((now - pose_received_time) >= max_pose_age) + 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 = now; + 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); - { - std::lock_guard lock(pose_mutex_); - if (last_tf_stamp_ns_ == tf_stamp_ns) - last_published_tf_stamp_ns_ = tf_stamp_ns; - } - 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()); @@ -198,7 +196,6 @@ class FlightMavros double sensor_roll_offset_rad_; double sensor_pitch_offset_rad_; double sensor_yaw_offset_rad_; - std::uint32_t publish_interval_updates_; tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; @@ -207,9 +204,9 @@ class FlightMavros std::mutex pose_mutex_; geometry_msgs::msg::Pose latest_pose_{}; std::int64_t last_tf_stamp_ns_{0}; - std::int64_t last_published_tf_stamp_ns_{0}; rclcpp::Time last_pose_received_time_; - std::uint32_t update_counter_{0}; + std::int64_t last_published_stamp_ns_{0}; + double publish_credit_{0.0}; bool pose_ready_{false}; }; From e5a789e4f3c09a17cb9cbeaeaf7fddb9092b8a5f Mon Sep 17 00:00:00 2001 From: noskillzheng <3515964992@qq.com> Date: Sun, 3 May 2026 06:32:30 +0800 Subject: [PATCH 22/23] feat: add flight gimbal hold support --- .gitmodules | 4 + .script/rclcpp-log | 67 +++ Dockerfile | 24 +- rmcs_ws/src/hikcamera | 2 +- rmcs_ws/src/rmcs_auto_aim_v2 | 1 + rmcs_ws/src/rmcs_bringup/config/flight.yaml | 77 ++-- .../src/rmcs_bringup/launch/rmcs.launch.py | 24 ++ rmcs_ws/src/rmcs_bringup/package.xml | 2 + rmcs_ws/src/rmcs_core/CMakeLists.txt | 5 +- rmcs_ws/src/rmcs_core/plugins.xml | 2 + .../gimbal/flight_gimbal_controller.cpp | 394 ++++++++++++++++++ .../src/controller/gimbal/hold_controller.cpp | 74 ++++ .../gimbal/simple_gimbal_controller.cpp | 24 +- .../controller/pid/error_pid_controller.cpp | 9 +- .../src/controller/pid/pid_controller.cpp | 7 + .../src/debug/rclcpp_diagnostic_log.hpp | 3 + rmcs_ws/src/rmcs_core/src/hardware/flight.cpp | 230 +++------- 17 files changed, 734 insertions(+), 215 deletions(-) create mode 100755 .script/rclcpp-log create mode 160000 rmcs_ws/src/rmcs_auto_aim_v2 create mode 100644 rmcs_ws/src/rmcs_core/src/controller/gimbal/flight_gimbal_controller.cpp create mode 100644 rmcs_ws/src/rmcs_core/src/controller/gimbal/hold_controller.cpp create mode 100644 rmcs_ws/src/rmcs_core/src/debug/rclcpp_diagnostic_log.hpp diff --git a/.gitmodules b/.gitmodules index 58c13305..98d15f11 100644 --- a/.gitmodules +++ b/.gitmodules @@ -4,9 +4,13 @@ [submodule "rmcs_ws/src/hikcamera"] path = rmcs_ws/src/hikcamera url = https://github.com/Alliance-Algorithm/ros2-hikcamera.git + branch =2.0 [submodule "rmcs_ws/src/serial"] path = rmcs_ws/src/serial url = https://github.com/Alliance-Algorithm/ros2-serial.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/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/.script/rclcpp-log b/.script/rclcpp-log new file mode 100755 index 00000000..b85218f9 --- /dev/null +++ b/.script/rclcpp-log @@ -0,0 +1,67 @@ +#!/bin/bash + +set -euo pipefail + +: "${RMCS_PATH:=$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)}" + +header="${RMCS_PATH}/rmcs_ws/src/rmcs_core/src/debug/rclcpp_diagnostic_log.hpp" + +usage() { + cat <<'EOF' +Usage: rclcpp-log + +Commands: + insert Enable RMCS diagnostic RCLCPP logs + cancel Disable RMCS diagnostic RCLCPP logs +EOF +} + +set_state() { + local value="$1" + + if [[ ! -f "${header}" ]]; then + echo "> ERROR: Cannot find ${header}" + exit 1 + fi + + python3 - "$header" "$value" <<'PY' +from pathlib import Path +import re +import sys + +path = Path(sys.argv[1]) +value = sys.argv[2] +text = path.read_text(encoding="utf-8") +updated, count = re.subn( + r"^#define RMCS_RCLCPP_DIAGNOSTIC_LOGS [01]$", + f"#define RMCS_RCLCPP_DIAGNOSTIC_LOGS {value}", + text, + count=1, + flags=re.MULTILINE, +) + +if count != 1: + raise SystemExit(f"> ERROR: Cannot update {path}") + +path.write_text(updated, encoding="utf-8") +PY +} + +case "${1:-}" in +insert) + set_state 1 + echo "> RMCS diagnostic RCLCPP logs enabled. Rebuild rmcs_core to apply." + ;; +cancel) + set_state 0 + echo "> RMCS diagnostic RCLCPP logs disabled. Rebuild rmcs_core to apply." + ;; +-h|--help|help|"") + usage + ;; +*) + echo "> ERROR: Unknown command '${1}'" + usage + exit 1 + ;; +esac diff --git a/Dockerfile b/Dockerfile index ecce1882..fb6b0a34 100644 --- a/Dockerfile +++ b/Dockerfile @@ -30,6 +30,8 @@ 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-cv-bridge \ + ros-$ROS_DISTRO-mavros ros-$ROS_DISTRO-mavros-extras \ dotnet-sdk-8.0 \ ros-$ROS_DISTRO-pcl-ros ros-$ROS_DISTRO-pcl-conversions ros-$ROS_DISTRO-pcl-msgs && \ apt-get autoremove -y && apt-get clean && \ @@ -37,12 +39,22 @@ RUN apt-get update && apt-get install -y --no-install-recommends \ # Install openvino runtime -RUN wget https://apt.repos.intel.com/intel-gpg-keys/GPG-PUB-KEY-INTEL-SW-PRODUCTS.PUB && \ - apt-key add ./GPG-PUB-KEY-INTEL-SW-PRODUCTS.PUB && \ - rm ./GPG-PUB-KEY-INTEL-SW-PRODUCTS.PUB && \ - echo "deb https://apt.repos.intel.com/openvino ubuntu24 main" > /etc/apt/sources.list.d/intel-openvino.list && \ +RUN apt-get update && apt-get install -y --no-install-recommends gnupg ca-certificates && \ + mkdir -p /etc/apt/keyrings && \ + wget -qO- https://apt.repos.intel.com/intel-gpg-keys/GPG-PUB-KEY-INTEL-SW-PRODUCTS.PUB | \ + gpg --dearmor -o /etc/apt/keyrings/intel-openvino.gpg && \ + printf 'Acquire::https::apt.repos.intel.com::Verify-Peer "false";\n' > /etc/apt/apt.conf.d/99intel-openvino-tls && \ + echo "deb [signed-by=/etc/apt/keyrings/intel-openvino.gpg] https://apt.repos.intel.com/openvino ubuntu24 main" > /etc/apt/sources.list.d/intel-openvino.list && \ apt-get update && \ - apt-get install -y --no-install-recommends openvino-2025.2.0 && \ + apt-get install -y --no-install-recommends \ + openvino-2025.2.0 \ + ocl-icd-libopencl1 \ + intel-opencl-icd \ + libze1 \ + libze-intel-gpu1 && \ + rm -f /etc/apt/apt.conf.d/99intel-openvino-tls \ + /etc/apt/sources.list.d/intel-openvino.list \ + /etc/apt/keyrings/intel-openvino.gpg && \ apt-get autoremove -y && apt-get clean && \ rm -rf /var/lib/apt/lists/* /tmp/* @@ -92,6 +104,7 @@ ARG LLVM_VERSION=22 RUN mkdir -p /etc/apt/keyrings && \ wget -qO- https://apt.llvm.org/llvm-snapshot.gpg.key | gpg --dearmor -o /etc/apt/keyrings/apt.llvm.org.gpg && \ chmod 644 /etc/apt/keyrings/apt.llvm.org.gpg && \ + printf 'Acquire::https::apt.llvm.org::Verify-Peer "false";\n' > /etc/apt/apt.conf.d/99llvm-tls && \ echo "deb [signed-by=/etc/apt/keyrings/apt.llvm.org.gpg] https://apt.llvm.org/noble/ llvm-toolchain-noble-${LLVM_VERSION} main" \ > /etc/apt/sources.list.d/llvm.list && \ apt-get update && \ @@ -107,6 +120,7 @@ RUN mkdir -p /etc/apt/keyrings && \ update-alternatives --install /usr/bin/llvm-ar llvm-ar /usr/bin/llvm-ar-${LLVM_VERSION} 50 && \ update-alternatives --install /usr/bin/llvm-ranlib llvm-ranlib /usr/bin/llvm-ranlib-${LLVM_VERSION} 50 && \ update-alternatives --install /usr/bin/ld.lld ld.lld /usr/bin/ld.lld-${LLVM_VERSION} 50 && \ + rm -f /etc/apt/apt.conf.d/99llvm-tls /etc/apt/sources.list.d/llvm.list /etc/apt/keyrings/apt.llvm.org.gpg && \ apt-get autoremove -y && apt-get clean && rm -rf /var/lib/apt/lists/* /tmp/* # Generate/load ssh key and setup unison diff --git a/rmcs_ws/src/hikcamera b/rmcs_ws/src/hikcamera index 815952fd..f0077f03 160000 --- a/rmcs_ws/src/hikcamera +++ b/rmcs_ws/src/hikcamera @@ -1 +1 @@ -Subproject commit 815952fd1388e121d2014956932f8796a1c8cdd1 +Subproject commit f0077f034800bcd0dde4fffeff270b733772a57e diff --git a/rmcs_ws/src/rmcs_auto_aim_v2 b/rmcs_ws/src/rmcs_auto_aim_v2 new file mode 160000 index 00000000..bccd90be --- /dev/null +++ b/rmcs_ws/src/rmcs_auto_aim_v2 @@ -0,0 +1 @@ +Subproject commit bccd90bef9a5edcb21e916e0b56759e1b2638475 diff --git a/rmcs_ws/src/rmcs_bringup/config/flight.yaml b/rmcs_ws/src/rmcs_bringup/config/flight.yaml index d42ef4cb..b4ea8830 100644 --- a/rmcs_ws/src/rmcs_bringup/config/flight.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/flight.yaml @@ -3,11 +3,13 @@ rmcs_executor: update_rate: 1000.0 components: - rmcs_core::hardware::Flight -> flight_hardware + - rmcs_core::hardware::FlightMavros -> flight_mavros_hardware - rmcs_core::referee::Status -> referee_status - - rmcs_core::controller::gimbal::SimpleGimbalController -> gimbal_controller + - rmcs_core::controller::gimbal::HoldController -> hold_controller + - rmcs_core::controller::gimbal::FlightGimbalController -> gimbal_controller + - rmcs::AutoAimComponent -> auto_aim_component - 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 @@ -23,30 +25,39 @@ rmcs_executor: - rmcs_core::referee::Command -> referee_command - # - rmcs_auto_aim::AutoAimInitializer -> auto_aim_initializer - # - rmcs_auto_aim::AutoAimController -> auto_aim_controller - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster - rmcs_core::broadcaster::TfBroadcaster -> tf_broadcaster -xrce_dds_agent: +mavros: ros__parameters: - device: /dev/ttyACM0 - baudrate: 921600 -# xrce_dds_agent: -# device: /dev/ttyS0 -# baudrate: 921600 + enabled: false + 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: - # Bring up the same host_sdk_sample node used by odin1_ros2.launch.py. - # Flight only needs /odin1/odometry, so the auxiliary depth/reprojection/RViz nodes stay off. + # FlightMavros reads odom -> odin1_base_link TF and forwards it to MAVROS vision pose. enabled: true config_file: "/rmcs_install/share/odin_ros_driver/config/control_command.yaml" # Empty uses odin_ros_driver/config/control_command.yaml node_name: host_sdk_sample respawn: true respawn_delay: 1.0 +auto_aim_runtime: + ros__parameters: + enabled: true + package: rmcs_auto_aim_v2 + executable: rmcs_auto_aim_v2_runtime + node_name: auto_aim_runtime + respawn: true + respawn_delay: 1.0 + value_broadcaster: ros__parameters: forward_list: @@ -71,45 +82,57 @@ tf_broadcaster: flight_hardware: ros__parameters: - usb_pid: -1 + board_serial: "d4-9d44" yaw_motor_zero_point: 38745 pitch_motor_zero_point: 51586 +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 +hold_controller: + ros__parameters: + joystick_deadband: 0.03 + mouse_deadband: 0.5 + gimbal_controller: ros__parameters: upper_limit: -0.15 lower_limit: 0.50 yaw_upper_limit: -0.349 - yaw_lower_limit: 0.349 + yaw_lower_limit: 3.14 + hold_level_threshold: 0.01 + hold_yaw_kd: 8.0 + hold_pitch_kd: 0.5 yaw_angle_pid_controller: ros__parameters: measurement: /gimbal/yaw/control_angle_error - control: /gimbal/yaw/control_velocity - kp: 2.0 - ki: 0.0 - kd: 0.0 - -yaw_velocity_pid_controller: - ros__parameters: - measurement: /gimbal/yaw/velocity_imu - setpoint: /gimbal/yaw/control_velocity + feedforward: /gimbal/yaw/hold_feedforward control: /gimbal/yaw/control_torque - kp: 4.0 + kp: 70.8943 ki: 0.0 - kd: 0.005 + kd: 1440.0 pitch_angle_pid_controller: ros__parameters: measurement: /gimbal/pitch/control_angle_error + feedforward: /gimbal/pitch/hold_feedforward control: /gimbal/pitch/control_velocity - kp: 8.0 + kp: 10.0 ki: 0.0 - kd: 0.0 + kd: 400.0 friction_wheel_controller: ros__parameters: diff --git a/rmcs_ws/src/rmcs_bringup/launch/rmcs.launch.py b/rmcs_ws/src/rmcs_bringup/launch/rmcs.launch.py index f96b2de1..c53faec4 100644 --- a/rmcs_ws/src/rmcs_bringup/launch/rmcs.launch.py +++ b/rmcs_ws/src/rmcs_bringup/launch/rmcs.launch.py @@ -102,6 +102,30 @@ def visit( ) ) + auto_aim_cfg = config.get("auto_aim_runtime", {}).get("ros__parameters") + if auto_aim_cfg is not None and auto_aim_cfg.get("enabled", True): + package = auto_aim_cfg.get("package", "rmcs_auto_aim_v2") + executable = auto_aim_cfg.get("executable", "rmcs_auto_aim_v2_runtime") + node_name = auto_aim_cfg.get("node_name", "auto_aim_runtime") + respawn = auto_aim_cfg.get("respawn", True) + respawn_delay = float(auto_aim_cfg.get("respawn_delay", 1.0)) + entities.append( + LogInfo( + msg=f"Starting auto aim runtime '{node_name}' from {package}/{executable}" + ) + ) + entities.append( + Node( + package=package, + executable=executable, + name=node_name, + respawn=respawn, + respawn_delay=respawn_delay, + output="screen", + emulate_tty=True, + ) + ) + mavros_cfg = config.get("mavros", {}).get("ros__parameters") if mavros_cfg is not None and mavros_cfg.get("enabled", True): mavros_share = FindPackageShare("mavros").perform(context) diff --git a/rmcs_ws/src/rmcs_bringup/package.xml b/rmcs_ws/src/rmcs_bringup/package.xml index 746f2627..38a8170a 100644 --- a/rmcs_ws/src/rmcs_bringup/package.xml +++ b/rmcs_ws/src/rmcs_bringup/package.xml @@ -11,7 +11,9 @@ ament_cmake joint_state_broadcaster + mavros odin_ros_driver + rmcs_auto_aim_v2 robot_state_publisher rviz2 xacro diff --git a/rmcs_ws/src/rmcs_core/CMakeLists.txt b/rmcs_ws/src/rmcs_core/CMakeLists.txt index 0fae776a..529ee90a 100644 --- a/rmcs_ws/src/rmcs_core/CMakeLists.txt +++ b/rmcs_ws/src/rmcs_core/CMakeLists.txt @@ -17,8 +17,8 @@ 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.0.0/librmcs-sdk-src-3.0.0.zip - URL_HASH SHA256=b39f51c21baacdcbf3f0176119b8850137a108b88a67e12395d37d89e5ef53e8 + URL https://github.com/Alliance-Algorithm/librmcs/releases/download/v3.1.0b1/librmcs-sdk-src-3.1.0-beta.1.zip + URL_HASH SHA256=ec68a8b0e9441bd9e35b859139b3875c559944f7c0ecb4a1539bcdc75442fec4 DOWNLOAD_EXTRACT_TIMESTAMP TRUE ) FetchContent_MakeAvailable(librmcs) @@ -38,6 +38,7 @@ include_directories(${PROJECT_SOURCE_DIR}/src) target_link_libraries( ${PROJECT_NAME} + librmcs-sdk pthread ${CMAKE_DL_LIBS} diff --git a/rmcs_ws/src/rmcs_core/plugins.xml b/rmcs_ws/src/rmcs_core/plugins.xml index 7e850c4f..d8db579a 100644 --- a/rmcs_ws/src/rmcs_core/plugins.xml +++ b/rmcs_ws/src/rmcs_core/plugins.xml @@ -10,6 +10,8 @@ + + diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/flight_gimbal_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/flight_gimbal_controller.cpp new file mode 100644 index 00000000..23ff90cb --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/flight_gimbal_controller.cpp @@ -0,0 +1,394 @@ +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace rmcs_core::controller::gimbal { + +using namespace rmcs_description; // NOLINT(google-build-using-namespace) + +class FlightTwoAxisGimbalSolver { + class Operation { + friend class FlightTwoAxisGimbalSolver; + + virtual PitchLink::DirectionVector update(FlightTwoAxisGimbalSolver& solver) const = 0; + }; + +public: + FlightTwoAxisGimbalSolver( + rmcs_executor::Component& component, double upper_limit, double lower_limit, + double yaw_upper_limit, double yaw_lower_limit) + : upper_limit_(std::cos(upper_limit), -std::sin(upper_limit)) + , lower_limit_(std::cos(lower_limit), -std::sin(lower_limit)) + , yaw_upper_limit_(std::cos(yaw_upper_limit), -std::sin(yaw_upper_limit)) + , yaw_lower_limit_(std::cos(yaw_lower_limit), -std::sin(yaw_lower_limit)) { + component.register_input("/tf", tf_); + } + + // Re-anchor the stored control direction to the gimbal's current physical pose. + // Called on hold-mode exit so that the next SetControlShift starts from "here", + // not from a stale target that predates the hold period. + void anchor_to_current_pose() { + control_direction_ = + fast_tf::cast(PitchLink::DirectionVector{Eigen::Vector3d::UnitX()}, *tf_); + control_enabled_ = true; + } + + class SetDisabled : public Operation { + PitchLink::DirectionVector update(FlightTwoAxisGimbalSolver& solver) const override { + solver.control_enabled_ = false; + return {}; + } + }; + + class SetToLevel : public Operation { + PitchLink::DirectionVector update(FlightTwoAxisGimbalSolver& solver) const override { + auto odom_direction = fast_tf::cast( + PitchLink::DirectionVector{Eigen::Vector3d::UnitX()}, *solver.tf_); + if (std::abs(odom_direction->x()) < 1e-6 && std::abs(odom_direction->y()) < 1e-6) + return {}; + + solver.control_enabled_ = true; + odom_direction->z() = 0; + auto pitch_direction = fast_tf::cast(odom_direction, *solver.tf_); + pitch_direction->normalize(); + return pitch_direction; + } + }; + + class SetControlDirection : public Operation { + public: + explicit SetControlDirection(OdomImu::DirectionVector target) + : target_(std::move(target)) {} + + private: + PitchLink::DirectionVector update(FlightTwoAxisGimbalSolver& solver) const override { + solver.control_enabled_ = true; + return fast_tf::cast(target_, *solver.tf_); + } + + OdomImu::DirectionVector target_; + }; + + class SetControlShift : public Operation { + public: + SetControlShift(double yaw_shift, double pitch_shift) + : yaw_shift_(yaw_shift) + , pitch_shift_(pitch_shift) {} + + private: + PitchLink::DirectionVector update(FlightTwoAxisGimbalSolver& solver) const override { + PitchLink::DirectionVector direction; + if (!solver.control_enabled_) { + solver.control_enabled_ = true; + direction = PitchLink::DirectionVector{Eigen::Vector3d::UnitX()}; + } else { + direction = fast_tf::cast(solver.control_direction_, *solver.tf_); + } + + auto yaw_transform = Eigen::AngleAxisd{yaw_shift_, Eigen::Vector3d::UnitZ()}; + auto pitch_transform = Eigen::AngleAxisd{pitch_shift_, Eigen::Vector3d::UnitY()}; + return PitchLink::DirectionVector{pitch_transform * (yaw_transform * (*direction))}; + } + + double yaw_shift_; + double pitch_shift_; + }; + + struct AngleError { + double yaw_angle_error; + double pitch_angle_error; + }; + + AngleError update(const Operation& operation) { + update_yaw_axis(); + + auto control_direction = operation.update(*this); + if (!control_enabled_) + return {kNan, kNan}; + + auto [control_direction_yaw_link, pitch] = pitch_link_to_yaw_link(control_direction); + clamp_control_direction(control_direction_yaw_link); + if (!control_enabled_) + return {kNan, kNan}; + + 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); + } + + bool enabled() const { return control_enabled_; } + +private: + void update_yaw_axis() { + auto yaw_axis = + fast_tf::cast(YawLink::DirectionVector{Eigen::Vector3d::UnitZ()}, *tf_); + *yaw_axis_filtered_ += 0.1 * (*fast_tf::cast(yaw_axis, *tf_)); + yaw_axis_filtered_->normalize(); + } + + auto pitch_link_to_yaw_link(const PitchLink::DirectionVector& direction) const + -> std::pair { + std::pair result; + auto& [direction_yaw_link, pitch] = result; + + auto yaw_axis = fast_tf::cast(yaw_axis_filtered_, *tf_); + pitch = {yaw_axis->z(), yaw_axis->x()}; + pitch.normalize(); + + const auto& [x, y, z] = *direction; + direction_yaw_link = {x * pitch.x() - z * pitch.y(), y, x * pitch.y() + z * pitch.x()}; + return result; + } + + static PitchLink::DirectionVector yaw_link_to_pitch_link( + const YawLink::DirectionVector& direction, const Eigen::Vector2d& pitch) { + const auto& [x, y, z] = *direction; + return {x * pitch.x() + z * pitch.y(), y, -x * pitch.y() + z * pitch.x()}; + } + + void clamp_control_direction(YawLink::DirectionVector& control_direction) { + const auto& [x, y, z] = *control_direction; + + Eigen::Vector2d xy_projection{x, y}; + double xy_norm = xy_projection.norm(); + if (xy_norm <= 0.0) { + control_enabled_ = false; + return; + } + xy_projection /= xy_norm; + + if (z > upper_limit_.y()) { + *control_direction << upper_limit_.x() * xy_projection, upper_limit_.y(); + } else if (z < lower_limit_.y()) { + *control_direction << lower_limit_.x() * xy_projection, lower_limit_.y(); + } + + const auto& [yaw_x, yaw_y, yaw_z] = *control_direction; + Eigen::Vector2d xz_projection{yaw_x, yaw_z}; + double xz_norm = xz_projection.norm(); + if (xz_norm <= 0.0) { + control_enabled_ = false; + return; + } + xz_projection /= xz_norm; + + if (yaw_y > yaw_upper_limit_.y()) { + *control_direction << yaw_upper_limit_.x() * xz_projection.x(), yaw_upper_limit_.y(), + yaw_upper_limit_.x() * xz_projection.y(); + } else if (yaw_y < yaw_lower_limit_.y()) { + *control_direction << yaw_lower_limit_.x() * xz_projection.x(), yaw_lower_limit_.y(), + yaw_lower_limit_.x() * xz_projection.y(); + } + } + + static AngleError calculate_control_errors( + const YawLink::DirectionVector& control_direction, const Eigen::Vector2d& pitch) { + const auto& [x, y, z] = *control_direction; + const auto& [c, s] = pitch; + + AngleError result; + result.yaw_angle_error = std::atan2(y, x); + double x_projected = std::sqrt(x * x + y * y); + result.pitch_angle_error = -std::atan2(z * c - x_projected * s, z * s + x_projected * c); + return result; + } + + static constexpr double kNan = std::numeric_limits::quiet_NaN(); + + const Eigen::Vector2d upper_limit_; + const Eigen::Vector2d lower_limit_; + const Eigen::Vector2d yaw_upper_limit_; + const Eigen::Vector2d yaw_lower_limit_; + + rmcs_executor::Component::InputInterface tf_; + + OdomImu::DirectionVector yaw_axis_filtered_{Eigen::Vector3d::UnitZ()}; + bool control_enabled_ = false; + OdomImu::DirectionVector control_direction_; +}; + +class FlightGimbalController + : public rmcs_executor::Component + , public rclcpp::Node { +public: + FlightGimbalController() + : Node( + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) + , hold_level_threshold_(get_parameter("hold_level_threshold").as_double()) + , hold_yaw_kd_(get_parameter("hold_yaw_kd").as_double()) + , hold_pitch_kd_(get_parameter("hold_pitch_kd").as_double()) + , solver_( + *this, get_parameter("upper_limit").as_double(), + get_parameter("lower_limit").as_double(), + get_parameter("yaw_upper_limit").as_double(), + get_parameter("yaw_lower_limit").as_double()) { + register_input("/remote/joystick/left", joystick_left_); + register_input("/remote/switch/right", switch_right_); + register_input("/remote/switch/left", switch_left_); + 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("/gimbal/yaw/velocity_imu", yaw_velocity_imu_); + register_input("/gimbal/pitch/velocity_imu", pitch_velocity_imu_); + register_input("/gimbal/yaw/velocity", yaw_velocity_encoder_); + register_input("/gimbal/pitch/velocity", pitch_velocity_encoder_); + register_input("/gimbal/yaw/angle", gimbal_yaw_angle_); + register_input("/gimbal/pitch/angle", gimbal_pitch_angle_); + + // Optional: when HoldController is not loaded these stay unbound, + // calculate_angle_error() falls through to the pre-hold behavior. + register_input("/gimbal/hold/desired", hold_desired_, false); + + register_output("/gimbal/yaw/control_angle_error", yaw_angle_error_, kNan); + register_output("/gimbal/pitch/control_angle_error", pitch_angle_error_, kNan); + register_output("/gimbal/yaw/hold_feedforward", yaw_hold_feedforward_, 0.0); + register_output("/gimbal/pitch/hold_feedforward", pitch_hold_feedforward_, 0.0); + } + + void update() override { + auto angle_error = calculate_angle_error(); + *yaw_angle_error_ = angle_error.yaw_angle_error; + + if (!std::isfinite(angle_error.pitch_angle_error) || hold_active_) { + *pitch_angle_error_ = angle_error.pitch_angle_error; + } else if (std::abs(angle_error.pitch_angle_error) < kPitchDeadband) { + *pitch_angle_error_ = 0.0; + } else if (angle_error.pitch_angle_error > 0.0) { + *pitch_angle_error_ = angle_error.pitch_angle_error - kPitchDeadband; + } else { + *pitch_angle_error_ = angle_error.pitch_angle_error + kPitchDeadband; + } + + if (hold_active_) { + filtered_yaw_vel_ += kVelFilterAlpha * (*yaw_velocity_imu_ - filtered_yaw_vel_); + filtered_pitch_vel_ += kVelFilterAlpha * (*pitch_velocity_imu_ - filtered_pitch_vel_); + *yaw_hold_feedforward_ = -hold_yaw_kd_ * filtered_yaw_vel_; + *pitch_hold_feedforward_ = -hold_pitch_kd_ * filtered_pitch_vel_; + } else { + filtered_yaw_vel_ = 0.0; + filtered_pitch_vel_ = 0.0; + *yaw_hold_feedforward_ = 0.0; + *pitch_hold_feedforward_ = 0.0; + } + } + +private: + auto calculate_angle_error() -> FlightTwoAxisGimbalSolver::AngleError { + auto switch_right = *switch_right_; + auto switch_left = *switch_left_; + + using namespace rmcs_msgs; // NOLINT(google-build-using-namespace) + if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) + || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) { + level_reached_ = false; + hold_active_ = false; + return solver_.update(FlightTwoAxisGimbalSolver::SetDisabled{}); + } + + if (!solver_.enabled()) { + level_reached_ = false; + hold_active_ = false; + return solver_.update(FlightTwoAxisGimbalSolver::SetToLevel{}); + } + + // Until SetToLevel has converged, hold/auto-aim mode is forbidden so an + // idle input at startup cannot latch a target before leveling finishes. + if (!level_reached_) { + auto err = solver_.update(FlightTwoAxisGimbalSolver::SetToLevel{}); + if (std::isfinite(err.yaw_angle_error) && std::isfinite(err.pitch_angle_error) + && std::abs(err.yaw_angle_error) < hold_level_threshold_ + && std::abs(err.pitch_angle_error) < hold_level_threshold_) { + level_reached_ = true; + } + return err; + } + + if (auto_aim_control_direction_.ready() && (mouse_->right || switch_right == Switch::UP) + && !auto_aim_control_direction_->isZero()) { + hold_active_ = false; + return solver_.update( + FlightTwoAxisGimbalSolver::SetControlDirection( + OdomImu::DirectionVector{*auto_aim_control_direction_})); + } + + const bool want_hold = hold_desired_.ready() && *hold_desired_; + if (want_hold) { + if (!hold_active_) { + hold_active_ = true; + hold_target_yaw_ = *gimbal_yaw_angle_; + hold_target_pitch_ = *gimbal_pitch_angle_; + } + return { + wrap_pi(hold_target_yaw_ - *gimbal_yaw_angle_), + wrap_pi(hold_target_pitch_ - *gimbal_pitch_angle_)}; + } + + if (hold_active_) { + hold_active_ = false; + solver_.anchor_to_current_pose(); + } + + constexpr double joystick_sensitivity = 0.006; + constexpr double mouse_sensitivity = 0.5; + double yaw_shift = + joystick_sensitivity * joystick_left_->y() + mouse_sensitivity * mouse_velocity_->y(); + double pitch_shift = + -joystick_sensitivity * joystick_left_->x() + mouse_sensitivity * mouse_velocity_->x(); + + return solver_.update(FlightTwoAxisGimbalSolver::SetControlShift(yaw_shift, pitch_shift)); + } + + static double wrap_pi(double x) { return std::remainder(x, 2.0 * std::numbers::pi); } + + static constexpr double kNan = std::numeric_limits::quiet_NaN(); + static constexpr double kPitchDeadband = 2e-3; + static constexpr double kVelFilterAlpha = 0.12; + + const double hold_level_threshold_; + const double hold_yaw_kd_; + const double hold_pitch_kd_; + bool level_reached_ = false; + bool hold_active_ = false; + double hold_target_yaw_ = 0.0; + double hold_target_pitch_ = 0.0; + double filtered_yaw_vel_ = 0.0; + double filtered_pitch_vel_ = 0.0; + + InputInterface joystick_left_; + InputInterface switch_right_; + InputInterface switch_left_; + InputInterface mouse_velocity_; + InputInterface mouse_; + InputInterface auto_aim_control_direction_; + InputInterface yaw_velocity_imu_; + InputInterface pitch_velocity_imu_; + InputInterface yaw_velocity_encoder_; + InputInterface pitch_velocity_encoder_; + InputInterface gimbal_yaw_angle_; + InputInterface gimbal_pitch_angle_; + InputInterface hold_desired_; + + OutputInterface yaw_angle_error_; + OutputInterface pitch_angle_error_; + OutputInterface yaw_hold_feedforward_; + OutputInterface pitch_hold_feedforward_; + + FlightTwoAxisGimbalSolver solver_; +}; + +} // namespace rmcs_core::controller::gimbal + +#include + +PLUGINLIB_EXPORT_CLASS( + rmcs_core::controller::gimbal::FlightGimbalController, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/hold_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/hold_controller.cpp new file mode 100644 index 00000000..ce7bfad9 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/hold_controller.cpp @@ -0,0 +1,74 @@ +#include +#include + +#include +#include +#include +#include + +namespace rmcs_core::controller::gimbal { + +class HoldController + : public rmcs_executor::Component + , public rclcpp::Node { +public: + HoldController() + : Node( + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) + , joystick_deadband_(get_parameter("joystick_deadband").as_double()) + , mouse_deadband_(get_parameter("mouse_deadband").as_double()) { + register_input("/remote/joystick/left", joystick_left_); + register_input("/remote/mouse/velocity", mouse_velocity_); + register_input("/remote/switch/left", switch_left_); + register_input("/remote/switch/right", switch_right_); + + register_output("/gimbal/hold/desired", hold_desired_, false); + } + + void update() override { + using namespace rmcs_msgs; // NOLINT(google-build-using-namespace) + const auto switch_left = *switch_left_; + const auto switch_right = *switch_right_; + + const bool gimbal_disabled = switch_left == Switch::UNKNOWN + || switch_right == Switch::UNKNOWN + || (switch_left == Switch::DOWN && switch_right == Switch::DOWN); + + if (gimbal_disabled || !is_idle()) { + idle_counter_ = 0; + *hold_desired_ = false; + return; + } + + idle_counter_ = std::min(idle_counter_ + 1, kEntryTicks); + *hold_desired_ = idle_counter_ >= kEntryTicks; + } + +private: + [[nodiscard]] bool is_idle() const { + const auto joystick_norm = joystick_left_->norm(); + const auto mouse_norm = mouse_velocity_->norm(); + return std::isfinite(joystick_norm) && std::isfinite(mouse_norm) + && joystick_norm < joystick_deadband_ && mouse_norm < mouse_deadband_; + } + + static constexpr int kEntryTicks = 50; + + const double joystick_deadband_; + const double mouse_deadband_; + int idle_counter_ = 0; + + InputInterface joystick_left_; + InputInterface mouse_velocity_; + InputInterface switch_left_; + InputInterface switch_right_; + + OutputInterface hold_desired_; +}; + +} // namespace rmcs_core::controller::gimbal + +#include + +PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::gimbal::HoldController, rmcs_executor::Component) 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 b33baa5f..06081af2 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 @@ -1,6 +1,8 @@ #include #include +#include +#include #include #include #include @@ -8,6 +10,7 @@ #include #include "controller/gimbal/two_axis_gimbal_solver.hpp" +#include "debug/rclcpp_diagnostic_log.hpp" namespace rmcs_core::controller::gimbal { @@ -48,8 +51,12 @@ class SimpleGimbalController *pitch_angle_error_ = angle_error.pitch_angle_error; } - // RCLCPP_INFO(get_logger(), "[gimbal calibration] New pitch offset: %f", - // *pitch_angle_error_); + if constexpr (RMCS_RCLCPP_DIAGNOSTIC_LOGS) { + RCLCPP_INFO_THROTTLE( + get_logger(), *get_clock(), 5000, + "Gimbal controller output: yaw_error=%.6f pitch_error=%.6f", + *yaw_angle_error_, *pitch_angle_error_); + } } TwoAxisGimbalSolver::AngleError calculate_angle_error() { @@ -57,6 +64,17 @@ class SimpleGimbalController auto switch_left = *switch_left_; auto mouse = *mouse_; + if constexpr (RMCS_RCLCPP_DIAGNOSTIC_LOGS) { + RCLCPP_INFO_THROTTLE( + get_logger(), *get_clock(), 5000, + "Gimbal remote input: switch_left=%u switch_right=%u joystick_left=(%.3f, %.3f) " + "mouse_velocity=(%.3f, %.3f) mouse_left=%d mouse_right=%d", + static_cast(std::to_underlying(switch_left)), + static_cast(std::to_underlying(switch_right)), joystick_left_->x(), + joystick_left_->y(), mouse_velocity_->x(), mouse_velocity_->y(), mouse.left, + mouse.right); + } + using namespace rmcs_msgs; if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) @@ -107,4 +125,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/pid/error_pid_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/pid/error_pid_controller.cpp index ac334dab..37eb03e3 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/pid/error_pid_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/pid/error_pid_controller.cpp @@ -5,6 +5,7 @@ #include "controller/pid/pid_calculator.hpp" #include "controller/pid/smart_input.hpp" +#include "debug/rclcpp_diagnostic_log.hpp" namespace rmcs_core::controller::pid { @@ -37,6 +38,12 @@ class ErrorPidController void update() override { auto err = *measurement_; *control_ = *feedforward_ + pid_calculator_.update(err); + if constexpr (RMCS_RCLCPP_DIAGNOSTIC_LOGS) { + RCLCPP_INFO_THROTTLE( + get_logger(), *get_clock(), 5000, + "Error PID output: measurement=%.6f feedforward=%.6f control=%.6f", err, + *feedforward_, *control_); + } } private: @@ -51,4 +58,4 @@ class ErrorPidController #include -PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::pid::ErrorPidController, rmcs_executor::Component) \ No newline at end of file +PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::pid::ErrorPidController, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/pid/pid_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/pid/pid_controller.cpp index 9780edd3..239253b0 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/pid/pid_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/pid/pid_controller.cpp @@ -5,6 +5,7 @@ #include "controller/pid/pid_calculator.hpp" #include "controller/pid/smart_input.hpp" +#include "debug/rclcpp_diagnostic_log.hpp" namespace rmcs_core::controller::pid { @@ -38,6 +39,12 @@ class PidController void update() override { auto err = *setpoint_ - *measurement_; *control_ = *feedforward_ + pid_calculator_.update(err); + if constexpr (RMCS_RCLCPP_DIAGNOSTIC_LOGS) { + RCLCPP_INFO_THROTTLE( + get_logger(), *get_clock(), 5000, + "PID output: measurement=%.6f setpoint=%.6f feedforward=%.6f control=%.6f", + *measurement_, *setpoint_, *feedforward_, *control_); + } } private: diff --git a/rmcs_ws/src/rmcs_core/src/debug/rclcpp_diagnostic_log.hpp b/rmcs_ws/src/rmcs_core/src/debug/rclcpp_diagnostic_log.hpp new file mode 100644 index 00000000..55949da3 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/debug/rclcpp_diagnostic_log.hpp @@ -0,0 +1,3 @@ +#pragma once + +#define RMCS_RCLCPP_DIAGNOSTIC_LOGS 0 diff --git a/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp b/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp index 10a6a12d..2eeb810d 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp @@ -1,13 +1,14 @@ -#include +#include +#include #include #include -#include +#include #include +#include -#include -#include -#include -#include +#include + +#include #include #include @@ -18,6 +19,7 @@ #include +#include "debug/rclcpp_diagnostic_log.hpp" #include "hardware/device/bmi088.hpp" #include "hardware/device/dji_motor.hpp" #include "hardware/device/dr16.hpp" @@ -31,10 +33,10 @@ class Flight , private librmcs::agent::CBoard { public: Flight() - : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} - , librmcs::agent::CBoard{} - , local_position_interface_{*this, px4_ros2::PoseFrame::LocalNED, px4_ros2::VelocityFrame::LocalNED} - , logger_(get_logger()) + : Node{ + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} + , librmcs::agent::CBoard{get_parameter("board_serial").as_string()} , flight_command_( create_partner_component(get_component_name() + "_command", *this)) , gimbal_yaw_motor_(*this, *flight_command_, "/gimbal/yaw") @@ -44,22 +46,6 @@ class Flight , gimbal_bullet_feeder_(*this, *flight_command_, "/gimbal/bullet_feeder") , dr16_(*this) , bmi088_(500.0, 0.3, 0.005) { - - if (!local_position_interface_.doRegister()) { - throw std::runtime_error("Failed to register LocalPositionMeasurementInterface"); - } - RCLCPP_INFO(logger_, "LocalPositionMeasurementInterface registered successfully."); - - auto odin_odom_qos = rclcpp::QoS{1}; - odin_odom_qos.reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE); - odin_odom_qos.durability(RMW_QOS_POLICY_DURABILITY_VOLATILE); - odin_odometry_subscription_ = create_subscription( - "/odin1/odometry", odin_odom_qos, - [this](const nav_msgs::msg::Odometry::ConstSharedPtr& msg) { - odin_odometry_subscription_callback(*msg); - }); - RCLCPP_INFO(logger_, "Subscribed to Odin driver odometry on /odin1/odometry."); - gimbal_yaw_motor_.configure( device::LkMotor::Config{device::LkMotor::Type::kMHF7015} .set_reversed() @@ -118,7 +104,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({ std::span{buffer, size} }); return size; @@ -131,12 +117,13 @@ class Flight update_motors(); update_imu(); dr16_.update_status(); - update_local_position(); } void command_update() { auto yaw_cmd = gimbal_yaw_motor_.generate_command(); auto pitch_cmd = gimbal_pitch_motor_.generate_command(); + const auto yaw_command_count = ++yaw_can_command_count_; + const auto pitch_command_count = ++pitch_can_command_count_; device::CanPacket8 dji_cmds{ gimbal_bullet_feeder_.generate_command(), device::CanPacket8::PaddingQuarter{}, @@ -146,6 +133,22 @@ class Flight .can2_transmit({.can_id = 0x141, .can_data = yaw_cmd.as_bytes()}) .can2_transmit({.can_id = 0x142, .can_data = pitch_cmd.as_bytes()}) .can1_transmit({.can_id = 0x200, .can_data = dji_cmds.as_bytes()}); + + if constexpr (RMCS_RCLCPP_DIAGNOSTIC_LOGS) { + RCLCPP_INFO_THROTTLE( + get_logger(), *get_clock(), 5000, + "Flight CAN command: yaw_ctrl_vel=%.6f yaw_ctrl_torque=%.6f " + "pitch_ctrl_vel=%.6f pitch_ctrl_torque=%.6f cmd_count(yaw=%llu,pitch=%llu) " + "fb_count(yaw=%llu,pitch=%llu)", + gimbal_yaw_motor_.control_velocity(), gimbal_yaw_motor_.control_torque(), + gimbal_pitch_motor_.control_velocity(), gimbal_pitch_motor_.control_torque(), + static_cast(yaw_command_count), + static_cast(pitch_command_count), + static_cast( + yaw_can_feedback_count_.load(std::memory_order_relaxed)), + static_cast( + pitch_can_feedback_count_.load(std::memory_order_relaxed))); + } } private: @@ -160,6 +163,20 @@ class Flight gimbal_bullet_feeder_.update_status(); gimbal_left_friction_.update_status(); gimbal_right_friction_.update_status(); + + if constexpr (RMCS_RCLCPP_DIAGNOSTIC_LOGS) { + RCLCPP_INFO_THROTTLE( + get_logger(), *get_clock(), 5000, + "Flight CAN feedback: yaw(angle=%.6f,velocity=%.6f,torque=%.6f,count=%llu) " + "pitch(angle=%.6f,velocity=%.6f,torque=%.6f,count=%llu)", + gimbal_yaw_motor_.angle(), gimbal_yaw_motor_.velocity(), gimbal_yaw_motor_.torque(), + static_cast( + yaw_can_feedback_count_.load(std::memory_order_relaxed)), + gimbal_pitch_motor_.angle(), gimbal_pitch_motor_.velocity(), + gimbal_pitch_motor_.torque(), + static_cast( + pitch_can_feedback_count_.load(std::memory_order_relaxed))); + } } void update_imu() { @@ -172,148 +189,7 @@ class Flight *gimbal_pitch_velocity_imu_ = bmi088_.gy(); } - void update_local_position() { - // Rate-limit to 50 Hz: skip unless 20 update cycles (20 ms) have elapsed - if (++odom_publish_counter_ < 20) - return; - odom_publish_counter_ = 0; - - OdomSnapshot snap; - { - std::lock_guard lock(odom_mutex_); - if (!odom_ready_) - return; - snap = latest_odom_; - odom_ready_ = false; - } - - // Validate: reject any snapshot containing NaN values - if (std::isnan(snap.pos_x) || std::isnan(snap.pos_y) || std::isnan(snap.pos_z) - || std::isnan(snap.vel_x) || std::isnan(snap.vel_y) || std::isnan(snap.vel_z) - || std::isnan(snap.q_w) || std::isnan(snap.q_x) || std::isnan(snap.q_y) - || std::isnan(snap.q_z)) { - RCLCPP_WARN_THROTTLE( - logger_, *get_clock(), 1000, - "Odometry snapshot contains NaN, skipping publish"); - return; - } - - px4_ros2::LocalPositionMeasurement meas{}; - meas.timestamp_sample = - snap.timestamp_sample.nanoseconds() > 0 ? snap.timestamp_sample : get_clock()->now(); - - // Odin publishes in ENU frame; PX4 expects NED. - // ENU→NED position: (x_ned, y_ned, z_ned) = (y_enu, x_enu, -z_enu) - float ned_pos_x = snap.pos_y; - float ned_pos_y = snap.pos_x; - float ned_pos_z = -snap.pos_z; - - float ned_vel_x = snap.vel_y; - float ned_vel_y = snap.vel_x; - float ned_vel_z = -snap.vel_z; - - meas.position_xy = Eigen::Vector2f{ned_pos_x, ned_pos_y}; - meas.position_z = ned_pos_z; - - // Covariance: only provide if source has valid (>0) values; swap x↔y for NED - bool pos_cov_valid = snap.cov_xx > 0.f && snap.cov_yy > 0.f && snap.cov_zz > 0.f - && !std::isnan(snap.cov_xx) && !std::isnan(snap.cov_yy) && !std::isnan(snap.cov_zz); - if (pos_cov_valid) { - meas.position_xy_variance = Eigen::Vector2f{snap.cov_yy, snap.cov_xx}; - meas.position_z_variance = snap.cov_zz; - } else { - // No valid covariance from source; omit position entirely rather than fabricate - meas.position_xy = std::nullopt; - meas.position_z = std::nullopt; - } - - bool vel_cov_valid = snap.vel_cov_xx > 0.f && snap.vel_cov_yy > 0.f - && snap.vel_cov_zz > 0.f && !std::isnan(snap.vel_cov_xx) - && !std::isnan(snap.vel_cov_yy) && !std::isnan(snap.vel_cov_zz); - if (vel_cov_valid) { - meas.velocity_xy = Eigen::Vector2f{ned_vel_x, ned_vel_y}; - meas.velocity_xy_variance = Eigen::Vector2f{snap.vel_cov_yy, snap.vel_cov_xx}; - meas.velocity_z = ned_vel_z; - meas.velocity_z_variance = snap.vel_cov_zz; - } - // If vel covariance invalid, omit velocity (leave as nullopt) - - // Attitude: convert quaternion from ENU to NED - Eigen::Quaternionf q_enu{snap.q_w, snap.q_x, snap.q_y, snap.q_z}; - q_enu.normalize(); - meas.attitude_quaternion = px4_ros2::attitudeEnuToNed(q_enu); - meas.attitude_variance = Eigen::Vector3f{0.05f, 0.05f, 0.05f}; - - try { - local_position_interface_.update(meas); - RCLCPP_INFO_THROTTLE( - logger_, *get_clock(), 1000, - "Published local position measurement with timestamp %f", - meas.timestamp_sample.seconds()); - } catch (const px4_ros2::NavigationInterfaceInvalidArgument& e) { - RCLCPP_ERROR_THROTTLE( - logger_, *get_clock(), 1000, "Navigation update error: %s", e.what()); - } - } - - 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()); - } - - // ---- Shared odometry snapshot (filled by ROS subscription, read by update()) ---- - struct OdomSnapshot { - rclcpp::Time timestamp_sample{}; - float pos_x{}, pos_y{}, pos_z{}; - float vel_x{}, vel_y{}, vel_z{}; - float ang_x{}, ang_y{}, ang_z{}; - float q_x{}, q_y{}, q_z{}, q_w{1.f}; - float cov_xx{}, cov_yy{}, cov_zz{}; - float vel_cov_xx{}, vel_cov_yy{}, vel_cov_zz{}; - }; - - std::mutex odom_mutex_; - OdomSnapshot latest_odom_; - bool odom_ready_{false}; - uint32_t odom_publish_counter_{0}; - - void odin_odometry_subscription_callback(const nav_msgs::msg::Odometry& msg) { - RCLCPP_INFO_THROTTLE( - logger_, *get_clock(), 1000, "Received odometry message with timestamp %u.%u", - msg.header.stamp.sec, msg.header.stamp.nanosec); - - OdomSnapshot snap{}; - snap.timestamp_sample = rclcpp::Time{msg.header.stamp}; - snap.pos_x = static_cast(msg.pose.pose.position.x); - snap.pos_y = static_cast(msg.pose.pose.position.y); - snap.pos_z = static_cast(msg.pose.pose.position.z); - snap.vel_x = static_cast(msg.twist.twist.linear.x); - snap.vel_y = static_cast(msg.twist.twist.linear.y); - snap.vel_z = static_cast(msg.twist.twist.linear.z); - snap.ang_x = static_cast(msg.twist.twist.angular.x); - snap.ang_y = static_cast(msg.twist.twist.angular.y); - snap.ang_z = static_cast(msg.twist.twist.angular.z); - snap.q_x = static_cast(msg.pose.pose.orientation.x); - snap.q_y = static_cast(msg.pose.pose.orientation.y); - snap.q_z = static_cast(msg.pose.pose.orientation.z); - snap.q_w = static_cast(msg.pose.pose.orientation.w); - snap.cov_xx = static_cast(msg.pose.covariance[0]); - snap.cov_yy = static_cast(msg.pose.covariance[7]); - snap.cov_zz = static_cast(msg.pose.covariance[14]); - snap.vel_cov_xx = static_cast(msg.twist.covariance[0]); - snap.vel_cov_yy = static_cast(msg.twist.covariance[7]); - snap.vel_cov_zz = static_cast(msg.twist.covariance[14]); - - { - std::lock_guard lock(odom_mutex_); - latest_odom_ = snap; - odom_ready_ = true; - } - } + void gimbal_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr) {} protected: void can1_receive_callback(const librmcs::data::CanDataView& data) override { @@ -335,13 +211,15 @@ class Flight [[unlikely]] return; if (data.can_id == 0x142) { + ++pitch_can_feedback_count_; gimbal_pitch_motor_.store_status(data.can_data); } else if (data.can_id == 0x141) { + ++yaw_can_feedback_count_; gimbal_yaw_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++}; }, @@ -361,10 +239,6 @@ class Flight } private: - // ---- Members ---- - px4_ros2::LocalPositionMeasurementInterface local_position_interface_; - rclcpp::Logger logger_; - class FlightCommand : public rmcs_executor::Component { public: explicit FlightCommand(Flight& flight) @@ -375,7 +249,6 @@ class Flight std::shared_ptr flight_command_; rclcpp::Subscription::SharedPtr gimbal_calibrate_subscription_; - rclcpp::Subscription::SharedPtr odin_odometry_subscription_; device::LkMotor gimbal_yaw_motor_; device::LkMotor gimbal_pitch_motor_; @@ -386,6 +259,11 @@ class Flight device::Dr16 dr16_; device::Bmi088 bmi088_; + std::atomic yaw_can_command_count_{0}; + std::atomic pitch_can_command_count_{0}; + std::atomic yaw_can_feedback_count_{0}; + std::atomic pitch_can_feedback_count_{0}; + OutputInterface gimbal_yaw_velocity_imu_; OutputInterface gimbal_pitch_velocity_imu_; From c338b952c980a5525a18acd88884152e14aabaee Mon Sep 17 00:00:00 2001 From: noskillzheng <3515964992@qq.com> Date: Sun, 3 May 2026 06:43:37 +0800 Subject: [PATCH 23/23] docs: add flight self-stabilization dependencies --- README.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/README.md b/README.md index dc365f69..352d85ff 100644 --- a/README.md +++ b/README.md @@ -277,3 +277,5 @@ build-rmcs && wait-sync && attach-remote -r ``` 可以触发 RMCS 构建，`wait-sync` 等待文件同步完成，接下来重启 RMCS 守护进程后，显示实时输出。 + +ps: 无人机自稳需要 `ros-jazzy-mavros` 和 `ros-jazzy-cv-bridge` 这两个包。