From f02275145045d4264a5cd2f687a0020e166dd626 Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Sun, 26 Apr 2026 03:12:54 +0800 Subject: [PATCH 1/9] style: update clang-format config and fix missing newline --- .clang-format | 22 +++++++++---------- .../controller/gimbal/dual_yaw_controller.cpp | 2 +- 2 files changed, 11 insertions(+), 13 deletions(-) diff --git a/.clang-format b/.clang-format index 994474914..5c1eec782 100644 --- a/.clang-format +++ b/.clang-format @@ -13,27 +13,25 @@ MaxEmptyLinesToKeep: 1 AlignAfterOpenBracket: AlwaysBreak -AlignArrayOfStructures: Right - AlignConsecutiveAssignments: - Enabled: false + Enabled: false AlignConsecutiveBitFields: - Enabled: true - AcrossEmptyLines: false - AcrossComments: false + Enabled: true + AcrossEmptyLines: false + AcrossComments: false AlignConsecutiveDeclarations: - Enabled: false + Enabled: false AlignConsecutiveMacros: - Enabled: true - AcrossEmptyLines: false - AcrossComments: false + Enabled: true + AcrossEmptyLines: false + AcrossComments: false # AlignConsecutiveShortCaseStatements: # Enabled: false AlignEscapedNewlines: Left AlignOperands: AlignAfterOperator AlignTrailingComments: - Kind: Always - OverEmptyLines: 64 + Kind: Always + OverEmptyLines: 64 PointerAlignment: Left AllowAllArgumentsOnNextLine: true diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/dual_yaw_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/dual_yaw_controller.cpp index 80d7a72e3..739b58592 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/dual_yaw_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/dual_yaw_controller.cpp @@ -145,4 +145,4 @@ class DualYawController #include -PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::gimbal::DualYawController, rmcs_executor::Component) \ No newline at end of file +PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::gimbal::DualYawController, rmcs_executor::Component) From bae1e39b74cbd74643b85a4edbc7da28d9da8193 Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Sun, 26 Apr 2026 03:13:02 +0800 Subject: [PATCH 2/9] feat(device): add make_pid_calculator() and last_raw_angle() interface - pid_calculator: add make_pid_calculator() factory function with parameter loading - dji_motor: add int last_raw_angle() public getter - lk_motor: add int64_t last_raw_angle() public getter --- .../src/controller/pid/pid_calculator.hpp | 36 +++++++++++++++++-- .../src/hardware/device/dji_motor.hpp | 2 ++ .../src/hardware/device/lk_motor.hpp | 2 ++ 3 files changed, 38 insertions(+), 2 deletions(-) diff --git a/rmcs_ws/src/rmcs_core/src/controller/pid/pid_calculator.hpp b/rmcs_ws/src/rmcs_core/src/controller/pid/pid_calculator.hpp index 4eba4ff4d..2951f53f8 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/pid/pid_calculator.hpp +++ b/rmcs_ws/src/rmcs_core/src/controller/pid/pid_calculator.hpp @@ -4,6 +4,10 @@ #include #include +#include +#include + +#include namespace rmcs_core::controller::pid { @@ -21,7 +25,7 @@ class PidCalculator { virtual ~PidCalculator() = default; void reset() { - last_err_ = nan; + last_err_ = nan; err_integral_ = 0; } @@ -57,4 +61,32 @@ class PidCalculator { double last_err_, err_integral_; }; -} // namespace rmcs_core::controller::pid \ No newline at end of file +inline auto make_pid_calculator( + rclcpp::Node& node, const std::string& prefix, // + std::optional kp_default = std::nullopt, + std::optional ki_default = std::nullopt, + std::optional kd_default = std::nullopt) { + + const auto parameter_or_default = + [&node](const std::string& name, std::optional default_value) { + if (default_value.has_value() && !node.has_parameter(name)) + node.declare_parameter(name, *default_value); + return node.get_parameter(name).as_double(); + }; + + auto calculator = PidCalculator{ + parameter_or_default(prefix + "kp", kp_default), + parameter_or_default(prefix + "ki", ki_default), + parameter_or_default(prefix + "kd", kd_default), + }; + + node.get_parameter(prefix + "integral_min", calculator.integral_min); + node.get_parameter(prefix + "integral_max", calculator.integral_max); + node.get_parameter(prefix + "integral_split_min", calculator.integral_split_min); + node.get_parameter(prefix + "integral_split_max", calculator.integral_split_max); + node.get_parameter(prefix + "output_min", calculator.output_min); + node.get_parameter(prefix + "output_max", calculator.output_max); + return calculator; +} + +} // namespace rmcs_core::controller::pid diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/dji_motor.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/dji_motor.hpp index bb3f1c932..d1c56cbfd 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/dji_motor.hpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/dji_motor.hpp @@ -200,6 +200,8 @@ class DjiMotor { return encoder_zero_point_; } + int last_raw_angle() const { return last_raw_angle_; } + double angle() const { return angle_; } double velocity() const { return velocity_; } double torque() const { return torque_; } 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 68b2e4f65..c11061c23 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 @@ -203,6 +203,8 @@ class LkMotor { return encoder_zero_point_; } + int64_t last_raw_angle() const { return last_raw_angle_; } + double angle() const { return angle_; } double velocity() const { return velocity_; } double torque() const { return torque_; } From 019d59ac41099e34dafd9a8390c097111bc6e57e Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Sun, 26 Apr 2026 03:13:07 +0800 Subject: [PATCH 3/9] feat(description): add sentry TF link tree with dual-yaw gimbal joints --- .../rmcs_description/sentry_description.hpp | 198 ++++++++++++++++++ 1 file changed, 198 insertions(+) create mode 100644 rmcs_ws/src/rmcs_description/include/rmcs_description/sentry_description.hpp diff --git a/rmcs_ws/src/rmcs_description/include/rmcs_description/sentry_description.hpp b/rmcs_ws/src/rmcs_description/include/rmcs_description/sentry_description.hpp new file mode 100644 index 000000000..2fc9d17be --- /dev/null +++ b/rmcs_ws/src/rmcs_description/include/rmcs_description/sentry_description.hpp @@ -0,0 +1,198 @@ +#pragma once + +#include + +#include + +#include + +namespace rmcs_description { + +struct BaseLink : fast_tf::Link { + static constexpr char name[] = "base_link"; +}; + +struct BottomYawLink : fast_tf::Link { + static constexpr char name[] = "bottom_yaw_link"; +}; +using YawLink = BottomYawLink; + +struct TopYawLink : fast_tf::Link { + static constexpr char name[] = "top_yaw_link"; +}; + +struct PitchLink : fast_tf::Link { + static constexpr char name[] = "pitch_link"; +}; + +struct MuzzleLink : fast_tf::Link { + static constexpr char name[] = "muzzle_link"; +}; + +struct CameraLink : fast_tf::Link { + static constexpr char name[] = "camera_link"; +}; + +struct TransmitterLink : fast_tf::Link { + static constexpr char name[] = "transmitter_link"; +}; + +struct OdomImu : fast_tf::Link { + static constexpr char name[] = "odom_imu"; +}; + +struct OdomGimbalImu : fast_tf::Link { + static constexpr char name[] = "odom_gimbal_imu"; +}; + +struct GimbalCenterLink : fast_tf::Link { + static constexpr char name[] = "gimbal_center_link"; +}; + +struct LeftFrontWheelLink : fast_tf::Link { + static constexpr char name[] = "left_front_wheel_link"; +}; +struct LeftBackWheelLink : fast_tf::Link { + static constexpr char name[] = "left_back_wheel_link"; +}; +struct RightBackWheelLink : fast_tf::Link { + static constexpr char name[] = "right_back_wheel_link"; +}; +struct RightFrontWheelLink : fast_tf::Link { + static constexpr char name[] = "right_front_wheel_link"; +}; + +} // namespace rmcs_description + +template <> +struct fast_tf::Joint : fast_tf::ModificationTrackable { + using Parent = rmcs_description::BaseLink; + Eigen::Translation3d transform = Eigen::Translation3d::Identity(); +}; + +template <> +struct fast_tf::Joint : fast_tf::ModificationTrackable { + using Parent = rmcs_description::GimbalCenterLink; + + void set_state(double angle) { angle_ = angle; } + auto get_transform() const { return Eigen::AngleAxisd{angle_, Eigen::Vector3d::UnitZ()}; } + +private: + double angle_ = 0.0; +}; + +template <> +struct fast_tf::Joint : fast_tf::ModificationTrackable { + using Parent = rmcs_description::BottomYawLink; + + void set_transform(const Eigen::Translation3d& translation) { translation_ = translation; } + + void set_state(double angle) { angle_ = angle; } + + auto get_transform() const { + Eigen::Isometry3d transform = Eigen::Isometry3d::Identity(); + transform *= translation_; + transform *= Eigen::AngleAxisd{angle_, Eigen::Vector3d::UnitZ()}; + return transform; + } + +private: + Eigen::Translation3d translation_ = Eigen::Translation3d::Identity(); + double angle_ = 0.0; +}; + +template <> +struct fast_tf::Joint : fast_tf::ModificationTrackable { + using Parent = rmcs_description::TopYawLink; + + void set_state(double angle) { angle_ = angle; } + auto get_transform() const { return Eigen::AngleAxisd{angle_, Eigen::Vector3d::UnitY()}; } + +private: + double angle_ = 0.0; +}; + +template <> +struct fast_tf::Joint : fast_tf::ModificationTrackable { + using Parent = rmcs_description::PitchLink; + Eigen::Translation3d transform = Eigen::Translation3d::Identity(); +}; + +template <> +struct fast_tf::Joint : fast_tf::ModificationTrackable { + using Parent = rmcs_description::PitchLink; + Eigen::Translation3d transform = Eigen::Translation3d::Identity(); +}; + +template <> +struct fast_tf::Joint : fast_tf::ModificationTrackable { + using Parent = rmcs_description::PitchLink; + Eigen::Isometry3d transform = Eigen::Isometry3d::Identity(); +}; + +template <> +struct fast_tf::Joint : fast_tf::ModificationTrackable { + using Parent = rmcs_description::BottomYawLink; + Eigen::Quaterniond transform = Eigen::Quaterniond::Identity(); +}; + +template <> +struct fast_tf::Joint : fast_tf::ModificationTrackable { + using Parent = rmcs_description::PitchLink; + Eigen::Quaterniond transform = Eigen::Quaterniond::Identity(); +}; + +template <> +struct fast_tf::Joint : fast_tf::ModificationTrackable { + using Parent = rmcs_description::BaseLink; + Eigen::Isometry3d transform = Eigen::Isometry3d::Identity(); + void set_state(double angle) { + auto rotation = Eigen::AngleAxisd{std::numbers::pi / 4, Eigen::Vector3d::UnitZ()} + * Eigen::AngleAxisd{angle, Eigen::Vector3d::UnitX()}; + transform.linear() = rotation.matrix(); + } +}; + +template <> +struct fast_tf::Joint : fast_tf::ModificationTrackable { + using Parent = rmcs_description::BaseLink; + Eigen::Isometry3d transform = Eigen::Isometry3d::Identity(); + void set_state(double angle) { + auto rotation = Eigen::AngleAxisd{std::numbers::pi / 4 * 3, Eigen::Vector3d::UnitZ()} + * Eigen::AngleAxisd{angle, Eigen::Vector3d::UnitX()}; + transform.linear() = rotation.matrix(); + } +}; + +template <> +struct fast_tf::Joint : fast_tf::ModificationTrackable { + using Parent = rmcs_description::BaseLink; + Eigen::Isometry3d transform = Eigen::Isometry3d::Identity(); + void set_state(double angle) { + auto rotation = Eigen::AngleAxisd{-std::numbers::pi / 4 * 3, Eigen::Vector3d::UnitZ()} + * Eigen::AngleAxisd{angle, Eigen::Vector3d::UnitX()}; + transform.linear() = rotation.matrix(); + } +}; + +template <> +struct fast_tf::Joint : fast_tf::ModificationTrackable { + using Parent = rmcs_description::BaseLink; + Eigen::Isometry3d transform = Eigen::Isometry3d::Identity(); + void set_state(double angle) { + auto rotation = Eigen::AngleAxisd{-std::numbers::pi / 4, Eigen::Vector3d::UnitZ()} + * Eigen::AngleAxisd{angle, Eigen::Vector3d::UnitX()}; + transform.linear() = rotation.matrix(); + } +}; + +namespace rmcs_description { + +using Tf = fast_tf::JointCollection< + GimbalCenterLink, BottomYawLink, TopYawLink, PitchLink, MuzzleLink, TransmitterLink, CameraLink, + OdomImu, OdomGimbalImu, LeftFrontWheelLink, LeftBackWheelLink, RightBackWheelLink, + RightFrontWheelLink>; + +using SentryTf = Tf; + +} // namespace rmcs_description From 13822656ea95dec866e6f0561b73cc95c54cd944 Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Sun, 26 Apr 2026 03:13:36 +0800 Subject: [PATCH 4/9] feat(sentry): add sentry hardware driver with build dependencies - Add sentry hardware component (GimbalBoard, Topboard, BottomBoard) - Support dual-yaw gimbal, chassis steering, supercap, DR16 remote, referee serial - Update CMakeLists.txt: bump librmcs to v3.1.0 for RmcsBoardLite support - Add std_srvs dependency for robot status service --- rmcs_ws/src/rmcs_core/CMakeLists.txt | 4 +- rmcs_ws/src/rmcs_core/package.xml | 1 + rmcs_ws/src/rmcs_core/plugins.xml | 1 + rmcs_ws/src/rmcs_core/src/hardware/sentry.cpp | 615 ++++++++++++++++++ 4 files changed, 619 insertions(+), 2 deletions(-) create mode 100644 rmcs_ws/src/rmcs_core/src/hardware/sentry.cpp diff --git a/rmcs_ws/src/rmcs_core/CMakeLists.txt b/rmcs_ws/src/rmcs_core/CMakeLists.txt index dd3c85799..36e7e5706 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.0/librmcs-sdk-src-3.1.0.zip + URL_HASH SHA256=07107e251745ddb23f7b3e39edec5d6910be1a197025d167ec9849c5c80dd954 DOWNLOAD_EXTRACT_TIMESTAMP TRUE ) FetchContent_MakeAvailable(librmcs) diff --git a/rmcs_ws/src/rmcs_core/package.xml b/rmcs_ws/src/rmcs_core/package.xml index 410745520..4312d3341 100644 --- a/rmcs_ws/src/rmcs_core/package.xml +++ b/rmcs_ws/src/rmcs_core/package.xml @@ -11,6 +11,7 @@ rclcpp std_msgs + std_srvs pluginlib tf2 tf2_ros diff --git a/rmcs_ws/src/rmcs_core/plugins.xml b/rmcs_ws/src/rmcs_core/plugins.xml index f7847151c..cf92e7336 100644 --- a/rmcs_ws/src/rmcs_core/plugins.xml +++ b/rmcs_ws/src/rmcs_core/plugins.xml @@ -2,6 +2,7 @@ + diff --git a/rmcs_ws/src/rmcs_core/src/hardware/sentry.cpp b/rmcs_ws/src/rmcs_core/src/hardware/sentry.cpp new file mode 100644 index 000000000..91e6e110d --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/sentry.cpp @@ -0,0 +1,615 @@ +#include "hardware/device/bmi088.hpp" +#include "hardware/device/can_packet.hpp" +#include "hardware/device/dji_motor.hpp" +#include "hardware/device/dr16.hpp" +#include "hardware/device/lk_motor.hpp" +#include "hardware/device/supercap.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace rmcs_core::hardware { + +using Clock = std::chrono::steady_clock; + +class Sentry + : public rmcs_executor::Component + , public rclcpp::Node { + class SentryCommand; + class GimbalBoard; + class TopBoard; + class BottomBoard; + +public: + Sentry() + : Node( + get_component_name(), + rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)) + , command_component_( + create_partner_component(get_component_name() + "_command", *this)) { + register_input("/predefined/timestamp", timestamp_); + register_output("/tf", tf_); + + gimbal_calibrate_subscription_ = create_subscription( + "/gimbal/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { + gimbal_calibrate_subscription_callback(std::move(msg)); + }); + steers_calibrate_subscription_ = create_subscription( + "/steers/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { + steers_calibrate_subscription_callback(std::move(msg)); + }); + + // For command: remote-status + status_service_ = Node::create_service( + "/rmcs/service/robot_status", + [this]( + const std_srvs::srv::Trigger::Request::SharedPtr&, + const std_srvs::srv::Trigger::Response::SharedPtr& response) { + status_service_callback(response); + }); + + top_board_ = std::make_unique( + *this, *command_component_, get_parameter("board_serial_top_board").as_string()); + + bottom_board_ = std::make_unique( + *this, *command_component_, get_parameter("board_serial_bottom_board").as_string()); + + gimbal_board_ = + std::make_unique(get_parameter("board_serial_gimbal_board").as_string()); + + tf_->set_transform( + Eigen::Translation3d{0.08, 0.0, 0.0}); + tf_->set_transform( + Eigen::Translation3d{0.07128, 0.0, 0.0481}); + } + + Sentry(const Sentry&) = delete; + Sentry& operator=(const Sentry&) = delete; + Sentry(Sentry&&) = delete; + Sentry& operator=(Sentry&&) = delete; + + ~Sentry() override = default; + + auto update() -> void override { + top_board_->update(); + bottom_board_->update(); + gimbal_board_->update(); + tf_->set_transform( + gimbal_board_->imu_pose().conjugate()); + } + + auto command_update() -> void { + top_board_->command_update(); + bottom_board_->command_update(); + } + +private: + auto status_service_callback(const std::shared_ptr& response) + -> void { + response->success = true; + + auto feedback_message = std::ostringstream{}; + auto text = [&](std::format_string format, Args&&... args) { + std::println(feedback_message, format, std::forward(args)...); + }; + + text("Gimbal Status"); + text("- Bottom Yaw: {}", bottom_board_->gimbal_bottom_yaw_motor_.last_raw_angle()); + text("- Top Yaw: {}", top_board_->gimbal_top_yaw_motor_.last_raw_angle()); + text("- Pitch Angle: {}", top_board_->gimbal_pitch_motor_.last_raw_angle()); + + text("Chassis Status"); + constexpr auto position = + std::array{"right back", "right front", "left front", "left back"}; + constexpr auto max_length = + std::ranges::max_element(position, {}, &std::string_view::size)->size(); + + for (auto&& [index, motor] : + std::views::zip(position, bottom_board_->chassis_steer_motors_)) { + text("- {:{}}: {}", index, max_length, motor.last_raw_angle()); + } + + response->message = feedback_message.str(); + } + + auto gimbal_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr) -> void { + RCLCPP_INFO( + get_logger(), "[gimbal calibration] New yaw offset: %ld", + bottom_board_->gimbal_bottom_yaw_motor_.calibrate_zero_point()); + RCLCPP_INFO( + get_logger(), "[gimbal calibration] New top yaw offset: %ld", + top_board_->gimbal_top_yaw_motor_.calibrate_zero_point()); + RCLCPP_INFO( + get_logger(), "[gimbal calibration] New pitch offset: %ld", + top_board_->gimbal_pitch_motor_.calibrate_zero_point()); + } + + auto steers_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr) -> void { + RCLCPP_INFO( + get_logger(), "[steer calibration] New left front offset: %d", + bottom_board_->chassis_steer_motors_[2].calibrate_zero_point()); + RCLCPP_INFO( + get_logger(), "[steer calibration] New left back offset: %d", + bottom_board_->chassis_steer_motors_[3].calibrate_zero_point()); + RCLCPP_INFO( + get_logger(), "[steer calibration] New right back offset: %d", + bottom_board_->chassis_steer_motors_[0].calibrate_zero_point()); + RCLCPP_INFO( + get_logger(), "[steer calibration] New right front offset: %d", + bottom_board_->chassis_steer_motors_[1].calibrate_zero_point()); + } + + class SentryCommand : public rmcs_executor::Component { + public: + explicit SentryCommand(Sentry& sentry) + : sentry(sentry) {} + + auto update() -> void override { sentry.command_update(); } + + Sentry& sentry; + }; + + class GimbalBoard final : private librmcs::agent::CBoard { + public: + explicit GimbalBoard(std::string_view board_serial = {}) + : librmcs::agent::CBoard(board_serial) { + bmi088_.set_coordinate_mapping( + [](double x, double y, double z) { return std::make_tuple(y, -x, z); }); + } + + GimbalBoard(const GimbalBoard&) = delete; + GimbalBoard& operator=(const GimbalBoard&) = delete; + GimbalBoard(GimbalBoard&&) = delete; + GimbalBoard& operator=(GimbalBoard&&) = delete; + + ~GimbalBoard() override = default; + + auto update() -> void { + bmi088_.update_status(); + imu_pose_ = Eigen::Quaterniond{bmi088_.q0(), bmi088_.q1(), bmi088_.q2(), bmi088_.q3()}; + } + + auto imu_pose() const -> Eigen::Quaterniond { return imu_pose_; } + + private: + auto accelerometer_receive_callback(const librmcs::data::AccelerometerDataView& data) + -> void override { + bmi088_.store_accelerometer_status(data.x, data.y, data.z); + } + + auto gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) + -> void override { + bmi088_.store_gyroscope_status(data.x, data.y, data.z); + } + + device::Bmi088 bmi088_{1000, 0.2, 0.0}; + Eigen::Quaterniond imu_pose_ = Eigen::Quaterniond::Identity(); + }; + + class TopBoard final : private librmcs::agent::RmcsBoardLite { + public: + friend class Sentry; + explicit TopBoard( + Sentry& sentry, SentryCommand& sentry_command, std::string_view board_serial = {}, + librmcs::agent::AdvancedOptions options = {}) + : librmcs::agent::RmcsBoardLite(board_serial, options) + , tf_(sentry.tf_) + , bmi088_(1000, 0.2, 0.0) + , gimbal_pitch_motor_(sentry, sentry_command, "/gimbal/pitch") + , gimbal_top_yaw_motor_(sentry, sentry_command, "/gimbal/top_yaw") + , gimbal_bullet_feeder_(sentry, sentry_command, "/gimbal/bullet_feeder") + , gimbal_left_friction_(sentry, sentry_command, "/gimbal/left_friction") + , gimbal_right_friction_(sentry, sentry_command, "/gimbal/right_friction") { + gimbal_pitch_motor_.configure( + device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10}.set_encoder_zero_point( + static_cast(sentry.get_parameter("pitch_motor_zero_point").as_int()))); + + gimbal_top_yaw_motor_.configure( + device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10}.set_encoder_zero_point( + static_cast(sentry.get_parameter("top_yaw_motor_zero_point").as_int()))); + + gimbal_bullet_feeder_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .enable_multi_turn_angle() + .set_reversed() + .set_reduction_ratio(19 * 2)); + + gimbal_left_friction_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(1.)); + gimbal_right_friction_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reduction_ratio(1.) + .set_reversed()); + + sentry.register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_bmi088_); + sentry.register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_bmi088_); + + bmi088_.set_coordinate_mapping( + [](double x, double y, double z) { return std::make_tuple(-x, -y, z); }); + } + + TopBoard(const TopBoard&) = delete; + TopBoard& operator=(const TopBoard&) = delete; + TopBoard(TopBoard&&) = delete; + TopBoard& operator=(TopBoard&&) = delete; + + ~TopBoard() override = default; + + auto update() -> void { + gimbal_top_yaw_motor_.update_status(); + gimbal_pitch_motor_.update_status(); + + const auto pitch_angle = + std::remainder(gimbal_pitch_motor_.angle(), 2.0 * std::numbers::pi_v); + + bmi088_.update_status(); + const Eigen::Quaterniond gimbal_bmi088_pose{ + bmi088_.q0(), bmi088_.q1(), bmi088_.q2(), bmi088_.q3()}; + + tf_->set_transform( + gimbal_bmi088_pose.conjugate()); + + *gimbal_yaw_velocity_bmi088_ = bmi088_.gz(); + *gimbal_pitch_velocity_bmi088_ = bmi088_.gy(); + + gimbal_bullet_feeder_.update_status(); + gimbal_left_friction_.update_status(); + gimbal_right_friction_.update_status(); + + tf_->set_state( + gimbal_top_yaw_motor_.angle()); + tf_->set_state(pitch_angle); + } + + auto command_update() -> void { + auto builder = start_transmit(); + + builder.can0_transmit({ + .can_id = 0x200, + .can_data = + device::CanPacket8{ + gimbal_right_friction_.generate_command(), + gimbal_left_friction_.generate_command(), + device::CanPacket8::PaddingQuarter{}, + gimbal_bullet_feeder_.generate_command(), + } + .as_bytes(), + }); + + builder.can3_transmit({ + .can_id = 0x141, + .can_data = gimbal_top_yaw_motor_.generate_torque_command().as_bytes(), + }); + + builder.can2_transmit({ + .can_id = 0x141, + .can_data = gimbal_pitch_motor_.generate_torque_command().as_bytes(), + }); + } + + private: + auto can0_receive_callback(const librmcs::data::CanDataView& data) -> void override { + if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] + return; + auto can_id = data.can_id; + if (can_id == 0x202) { + gimbal_left_friction_.store_status(data.can_data); + } else if (can_id == 0x201) { + gimbal_right_friction_.store_status(data.can_data); + } else if (can_id == 0x204) { + gimbal_bullet_feeder_.store_status(data.can_data); + } + } + + auto can2_receive_callback(const librmcs::data::CanDataView& data) -> void override { + if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] + return; + auto can_id = data.can_id; + if (can_id == 0x141) + gimbal_pitch_motor_.store_status(data.can_data); + } + + auto can3_receive_callback(const librmcs::data::CanDataView& data) -> void override { + if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] + return; + auto can_id = data.can_id; + if (can_id == 0x141) + gimbal_top_yaw_motor_.store_status(data.can_data); + } + + auto accelerometer_receive_callback(const librmcs::data::AccelerometerDataView& data) + -> void override { + bmi088_.store_accelerometer_status(data.x, data.y, data.z); + } + + auto gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) + -> void override { + bmi088_.store_gyroscope_status(data.x, data.y, data.z); + } + + OutputInterface& tf_; + + OutputInterface gimbal_yaw_velocity_bmi088_; + OutputInterface gimbal_pitch_velocity_bmi088_; + + device::Bmi088 bmi088_; + device::LkMotor gimbal_pitch_motor_; + device::LkMotor gimbal_top_yaw_motor_; + device::DjiMotor gimbal_bullet_feeder_; + + device::DjiMotor gimbal_left_friction_; + device::DjiMotor gimbal_right_friction_; + }; + + class BottomBoard final : private librmcs::agent::CBoard { + public: + friend class Sentry; + + explicit BottomBoard( + Sentry& sentry, SentryCommand& sentry_command, std::string_view board_serial = {}) + : librmcs::agent::CBoard(board_serial) + , imu_(1000, 0.2, 0.0) + , tf_(sentry.tf_) + , dr16_(sentry) + , gimbal_bottom_yaw_motor_(sentry, sentry_command, "/gimbal/bottom_yaw") + , chassis_wheel_motors_( + {sentry, sentry_command, "/chassis/left_front_wheel"}, + {sentry, sentry_command, "/chassis/left_back_wheel"}, + {sentry, sentry_command, "/chassis/right_back_wheel"}, + {sentry, sentry_command, "/chassis/right_front_wheel"}) + , chassis_steer_motors_( + {sentry, sentry_command, "/chassis/left_front_steering"}, + {sentry, sentry_command, "/chassis/left_back_steering"}, + {sentry, sentry_command, "/chassis/right_back_steering"}, + {sentry, sentry_command, "/chassis/right_front_steering"}) + , supercap_(sentry, sentry_command) { + sentry.register_output("/referee/serial", referee_serial_); + + referee_serial_->read = [this](std::byte* buffer, size_t size) { + return referee_ring_buffer_receive_.pop_front_n( + [&buffer](std::byte byte) noexcept { *buffer++ = byte; }, size); + }; + referee_serial_->write = [this](const std::byte* buffer, size_t size) { + start_transmit().uart1_transmit( + {.uart_data = std::span{buffer, size}}); + return size; + }; + + gimbal_bottom_yaw_motor_.configure( + device::LkMotor::Config{device::LkMotor::Type::kMG6012Ei8} + .set_reversed() + .set_encoder_zero_point( + static_cast( + sentry.get_parameter("bottom_yaw_motor_zero_point").as_int()))); + + for (auto& motor : chassis_wheel_motors_) + motor.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reduction_ratio(11.) + .enable_multi_turn_angle() + .set_reversed()); + chassis_steer_motors_[2].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} + .set_reversed() + .set_encoder_zero_point( + static_cast(sentry.get_parameter("left_front_zero_point").as_int())) + .enable_multi_turn_angle()); + chassis_steer_motors_[3].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} + .set_reversed() + .set_encoder_zero_point( + static_cast(sentry.get_parameter("left_back_zero_point").as_int())) + .enable_multi_turn_angle()); + chassis_steer_motors_[0].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} + .set_reversed() + .set_encoder_zero_point( + static_cast(sentry.get_parameter("right_back_zero_point").as_int())) + .enable_multi_turn_angle()); + chassis_steer_motors_[1].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} + .set_reversed() + .set_encoder_zero_point( + static_cast(sentry.get_parameter("right_front_zero_point").as_int())) + .enable_multi_turn_angle()); + sentry.register_output("/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_, 0); + } + + BottomBoard(const BottomBoard&) = delete; + BottomBoard& operator=(const BottomBoard&) = delete; + BottomBoard(BottomBoard&&) = delete; + BottomBoard& operator=(BottomBoard&&) = delete; + + ~BottomBoard() override = default; + + auto update() -> void { + imu_.update_status(); + *chassis_yaw_velocity_imu_ = imu_.gz(); + supercap_.update_status(); + + for (auto& motor : chassis_wheel_motors_) + motor.update_status(); + for (auto& motor : chassis_steer_motors_) + motor.update_status(); + + dr16_.update_status(); + gimbal_bottom_yaw_motor_.update_status(); + tf_->set_state( + gimbal_bottom_yaw_motor_.angle()); + } + + auto command_update() -> void { + auto builder = start_transmit(); + builder.can1_transmit({ + .can_id = 0x141, + .can_data = gimbal_bottom_yaw_motor_.generate_command().as_bytes(), + }); + + if (can_transmission_mode_) { + builder + .can1_transmit({ + .can_id = 0x200, + .can_data = + device::CanPacket8{ + chassis_wheel_motors_[1].generate_command(), + chassis_wheel_motors_[0].generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }) + .can2_transmit({ + .can_id = 0x200, + .can_data = + device::CanPacket8{ + device::CanPacket8::PaddingQuarter{}, + chassis_wheel_motors_[2].generate_command(), + device::CanPacket8::PaddingQuarter{}, + chassis_wheel_motors_[3].generate_command(), + } + .as_bytes(), + }); + } else { + builder + .can1_transmit({ + .can_id = 0x1FE, + .can_data = + device::CanPacket8{ + chassis_steer_motors_[1].generate_command(), + chassis_steer_motors_[0].generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }) + .can2_transmit({ + .can_id = 0x1FE, + .can_data = + device::CanPacket8{ + chassis_steer_motors_[2].generate_command(), + chassis_steer_motors_[3].generate_command(), + device::CanPacket8::PaddingQuarter{}, + supercap_.generate_command(), + } + .as_bytes(), + }); + } + can_transmission_mode_ = !can_transmission_mode_; + } + + private: + auto dbus_receive_callback(const librmcs::data::UartDataView& data) -> void override { + dr16_.store_status(data.uart_data.data(), data.uart_data.size()); + } + + auto can1_receive_callback(const librmcs::data::CanDataView& data) -> void override { + if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] + return; + auto can_id = data.can_id; + if (can_id == 0x201) + chassis_wheel_motors_[1].store_status(data.can_data); + else if (can_id == 0x202) + chassis_wheel_motors_[0].store_status(data.can_data); + else if (can_id == 0x205) + chassis_steer_motors_[1].store_status(data.can_data); + else if (can_id == 0x206) + chassis_steer_motors_[0].store_status(data.can_data); + else if (can_id == 0x141) + gimbal_bottom_yaw_motor_.store_status(data.can_data); + } + + auto can2_receive_callback(const librmcs::data::CanDataView& data) -> void override { + if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] + return; + auto can_id = data.can_id; + if (can_id == 0x202) + chassis_wheel_motors_[2].store_status(data.can_data); + else if (can_id == 0x204) + chassis_wheel_motors_[3].store_status(data.can_data); + else if (can_id == 0x205) + chassis_steer_motors_[2].store_status(data.can_data); + else if (can_id == 0x206) + chassis_steer_motors_[3].store_status(data.can_data); + else if (can_id == 0x300) + supercap_.store_status(data.can_data); + } + + auto uart1_receive_callback(const librmcs::data::UartDataView& data) -> void override { + const auto* uart_data = data.uart_data.data(); + referee_ring_buffer_receive_.emplace_back_n( + [&uart_data](std::byte* storage) noexcept { *storage = *uart_data++; }, + data.uart_data.size()); + } + + auto accelerometer_receive_callback(const librmcs::data::AccelerometerDataView& data) + -> void override { + imu_.store_accelerometer_status(data.x, data.y, data.z); + } + + auto gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) + -> void override { + imu_.store_gyroscope_status(data.x, data.y, data.z); + } + + bool can_transmission_mode_ = true; + device::Bmi088 imu_; + OutputInterface& tf_; + + device::Dr16 dr16_; + device::LkMotor gimbal_bottom_yaw_motor_; + device::DjiMotor chassis_wheel_motors_[4]; + device::DjiMotor chassis_steer_motors_[4]; + device::Supercap supercap_; + + rmcs_utility::RingBuffer referee_ring_buffer_receive_{256}; + OutputInterface referee_serial_; + OutputInterface chassis_yaw_velocity_imu_; + }; + + InputInterface timestamp_; + OutputInterface tf_; + + std::shared_ptr command_component_; + std::unique_ptr gimbal_board_; + std::unique_ptr top_board_; + std::unique_ptr bottom_board_; + + rclcpp::Subscription::SharedPtr gimbal_calibrate_subscription_; + rclcpp::Subscription::SharedPtr steers_calibrate_subscription_; + + std::shared_ptr> status_service_; +}; + +} // namespace rmcs_core::hardware + +#include + +PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::Sentry, rmcs_executor::Component) From fefee783bdca58e264821e2d5a68c603dd6b9df0 Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Sun, 26 Apr 2026 03:13:45 +0800 Subject: [PATCH 5/9] feat(sentry): add EccentricDualYaw manual gimbal controller and launch config - Add eccentric dual-yaw gimbal controller with manual joystick/mouse input - Cascaded angle-velocity PID with friction and gravity feedforward - Top-bottom yaw coupling compensation via k_top_to_bottom - Launch config with tuned PID/Ff parameters for sentry robot --- rmcs_ws/src/rmcs_bringup/config/sentry.yaml | 164 ++++++++++ rmcs_ws/src/rmcs_core/plugins.xml | 1 + .../controller/gimbal/eccentric_dual_yaw.cpp | 308 ++++++++++++++++++ 3 files changed, 473 insertions(+) create mode 100644 rmcs_ws/src/rmcs_bringup/config/sentry.yaml create mode 100644 rmcs_ws/src/rmcs_core/src/controller/gimbal/eccentric_dual_yaw.cpp diff --git a/rmcs_ws/src/rmcs_bringup/config/sentry.yaml b/rmcs_ws/src/rmcs_bringup/config/sentry.yaml new file mode 100644 index 000000000..7cd0a6799 --- /dev/null +++ b/rmcs_ws/src/rmcs_bringup/config/sentry.yaml @@ -0,0 +1,164 @@ +rmcs_executor: + ros__parameters: + update_rate: 1000.0 + components: + - rmcs_core::hardware::Sentry -> sentry_hardware + + - rmcs_core::referee::Status -> referee_status + - rmcs_core::referee::Command -> referee_command + + - rmcs_core::referee::command::Interaction -> referee_interaction + - rmcs_core::referee::command::interaction::Ui -> referee_ui + + - rmcs_core::controller::gimbal::EccentricDualYaw -> gimbal_controller + + # - rmcs_core::broadcaster::TfBroadcaster -> tf_broadcaster + + - 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::controller::chassis::ChassisController -> chassis_controller + - rmcs_core::controller::chassis::ChassisPowerController -> chassis_power_controller + - rmcs_core::controller::chassis::SteeringWheelController -> steering_wheel_controller + + # - rmcs::navigation::Navigation -> rmcs_navigation + +# The positive direction is the one that battery exists +sentry_hardware: + ros__parameters: + board_serial_top_board: "af-da30" + board_serial_bottom_board: "d4-2184" + board_serial_gimbal_board: "d4-1d2b" + bottom_yaw_motor_zero_point: 15151 + top_yaw_motor_zero_point: 4453 + pitch_motor_zero_point: 1446 + left_front_zero_point: 5768 + left_back_zero_point: 5110 + right_back_zero_point: 2685 + right_front_zero_point: 6434 + +rmcs_navigation: + ros__parameters: + # 策略名称： + # - fast-push-output "速推前哨站" + # - kill-robots "杀伤优先" + decision: "fast-push-output" + command_vel_name: "/cmd_vel" + mock_context: false + endpoint: "test" + enable_goal_topic_forward: true + +gimbal_controller: + ros__parameters: + upper_limit: -0.39518 + lower_limit: 0.36 + + top_yaw_angle_kp: 30.0 + top_yaw_angle_ki: 0.0 + top_yaw_angle_kd: 0.0 + top_yaw_velocity_kp: 2.160 + top_yaw_velocity_ki: 0.0 + top_yaw_velocity_kd: 0.0 + + bottom_yaw_angle_kp: 8.8 + bottom_yaw_angle_ki: 0.0 + bottom_yaw_angle_kd: 0.0 + bottom_yaw_velocity_kp: 22.49 + bottom_yaw_velocity_ki: 0.0 + bottom_yaw_velocity_kd: 0.0 + + pitch_angle_kp: 40.0 + pitch_angle_ki: 0.0 + pitch_angle_kd: 0.01 + pitch_velocity_kp: 2.5 + pitch_velocity_ki: 0.0 + pitch_velocity_kd: 0.0 + + k_top_to_bottom: -1.0 + + bottom_yaw_viscous_ff_gain: 0.002495 + # bottom_yaw_coulomb_ff_gain: 0.457343 + # bottom_yaw_coulomb_ff_tanh_gain: 100.0 + top_yaw_viscous_ff_gain: 0.231 + # top_yaw_coulomb_ff_gain: 1.12 + # top_yaw_coulomb_ff_tanh_gain: 100.0 + pitch_viscous_ff_gain: 0.33 + # pitch_coulomb_ff_gain: 0.95 + # pitch_coulomb_ff_tanh_gain: 100.0 + pitch_gravity_ff_gain: 2.128 + pitch_gravity_ff_phase: 1.438 + +chassis_controller: + ros__parameters: + navigation_velocity_scale: 1.0 + +friction_wheel_controller: + ros__parameters: + friction_wheels: + - /gimbal/left_friction + - /gimbal/right_friction + friction_velocities: + - 630.0 + - 630.0 + friction_soft_start_stop_time: 1.0 + +heat_controller: + ros__parameters: + heat_per_shot: 10000 + reserved_heat: 10000 + +bullet_feeder_controller: + ros__parameters: + bullets_per_feeder_turn: 9.0 + shot_frequency: 28.0 + safe_shot_frequency: 10.0 + eject_frequency: 15.0 + eject_time: 0.15 + deep_eject_frequency: 15.0 + deep_eject_time: 0.20 + single_shot_max_stop_delay: 2.0 + +left_friction_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/left_friction/velocity + setpoint: /gimbal/left_friction/control_velocity + control: /gimbal/left_friction/control_torque + kp: 0.003436926 + ki: 0.00 + kd: 0.009373434 + +right_friction_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/right_friction/velocity + setpoint: /gimbal/right_friction/control_velocity + control: /gimbal/right_friction/control_torque + kp: 0.003436926 + ki: 0.00 + kd: 0.009373434 + +bullet_feeder_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/bullet_feeder/velocity + setpoint: /gimbal/bullet_feeder/control_velocity + control: /gimbal/bullet_feeder/control_torque + kp: 0.283 + ki: 0.0 + kd: 0.0 + +steering_wheel_controller: + ros__parameters: + mess: 22.0 + moment_of_inertia: 0.77852676 + vehicle_radius: 0.26870058 + wheel_radius: 0.055 + friction_coefficient: 0.666 + k1: 2.958580e+00 + k2: 3.082190e-03 + no_load_power: 11.37 + chassis_translation_kp: 8.0 + chassis_translation_ki: 0.0 + chassis_translation_kd: 0.0 diff --git a/rmcs_ws/src/rmcs_core/plugins.xml b/rmcs_ws/src/rmcs_core/plugins.xml index cf92e7336..db9ff7c45 100644 --- a/rmcs_ws/src/rmcs_core/plugins.xml +++ b/rmcs_ws/src/rmcs_core/plugins.xml @@ -11,6 +11,7 @@ + diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/eccentric_dual_yaw.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/eccentric_dual_yaw.cpp new file mode 100644 index 000000000..4e6c4e72d --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/eccentric_dual_yaw.cpp @@ -0,0 +1,308 @@ +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "controller/pid/pid_calculator.hpp" + +namespace rmcs_core::controller::gimbal { +using namespace rmcs_description; + +class EccentricDualYaw + : public rmcs_executor::Component + , public rclcpp::Node { +public: + EccentricDualYaw() + : Node{ + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} {} + + auto before_updating() -> void override { + enter_disabled_state(); + previous_actual_yaw_ = current_barrel_yaw_pitch().first; + previous_yaw_timestamp_ = *input_.timestamp; + } + + auto update() -> void override { + const auto actual_yaw_pitch = current_barrel_yaw_pitch(); + *output_.yaw_angle = *input_.bottom_yaw_angle; + *output_.yaw_velocity = compute_actual_yaw_velocity(actual_yaw_pitch.first); + + if (!input_.enable_control()) { + enter_disabled_state(); + return; + } + + const double yaw_shift = kJoystickSensitivity * input_.joystick_left->y() + + kMouseSensitivity * input_.mouse_velocity->y(); + const double pitch_shift = -kJoystickSensitivity * input_.joystick_left->x() + - kMouseSensitivity * input_.mouse_velocity->x(); + + manual_bottom_yaw_target_ = limit_rad(manual_bottom_yaw_target_ + yaw_shift); + manual_pitch_target_ = + std::clamp(manual_pitch_target_ + pitch_shift, upper_limit_, lower_limit_); + + const auto manual_target = ControlTarget{ + .bottom_yaw = {.target = manual_bottom_yaw_target_}, + .top_yaw = {.target = 0.0}, + .pitch = {.target = manual_pitch_target_}, + }; + apply_control(manual_target); + } + +private: + static constexpr double kNaN = std::numeric_limits::quiet_NaN(); + static constexpr double kJoystickSensitivity = 0.006; + static constexpr double kMouseSensitivity = 0.5; + + const double upper_limit_{get_parameter("upper_limit").as_double()}; + const double lower_limit_{get_parameter("lower_limit").as_double()}; + + const double bottom_yaw_viscous_ff_gain_{get_parameter_or("bottom_yaw_viscous_ff_gain", 0.0)}; + const double bottom_yaw_coulomb_ff_gain_{get_parameter_or("bottom_yaw_coulomb_ff_gain", 0.0)}; + const double bottom_yaw_coulomb_ff_tanh_gain_{ + get_parameter_or("bottom_yaw_coulomb_ff_tanh_gain", 100.0)}; + const double k_top_to_bottom_{get_parameter_or("k_top_to_bottom", 0.0)}; + const double top_yaw_viscous_ff_gain_{get_parameter_or("top_yaw_viscous_ff_gain", 0.0)}; + const double top_yaw_coulomb_ff_gain_{get_parameter_or("top_yaw_coulomb_ff_gain", 0.0)}; + const double top_yaw_coulomb_ff_tanh_gain_{ + get_parameter_or("top_yaw_coulomb_ff_tanh_gain", 100.0)}; + const double pitch_viscous_ff_gain_{get_parameter_or("pitch_viscous_ff_gain", 0.0)}; + const double pitch_coulomb_ff_gain_{get_parameter_or("pitch_coulomb_ff_gain", 0.0)}; + const double pitch_coulomb_ff_tanh_gain_{get_parameter_or("pitch_coulomb_ff_tanh_gain", 100.0)}; + const double pitch_gravity_ff_gain_{get_parameter_or("pitch_gravity_ff_gain", 0.0)}; + const double pitch_gravity_ff_phase_{get_parameter_or("pitch_gravity_ff_phase", 0.0)}; + + struct AxisCommand { + double target = 0.0; + double velocity_ff = 0.0; + double acceleration_ff = 0.0; + }; + struct ControlTarget { + AxisCommand bottom_yaw; + AxisCommand top_yaw; + AxisCommand pitch; + }; + + struct Input { + explicit Input(rmcs_executor::Component& component) { + component.register_input("/remote/joystick/left", joystick_left); + component.register_input("/remote/switch/right", switch_right); + component.register_input("/remote/switch/left", switch_left); + component.register_input("/remote/mouse/velocity", mouse_velocity); + + component.register_input("/predefined/timestamp", timestamp); + component.register_input("/tf", tf); + + component.register_input("/gimbal/top_yaw/angle", top_yaw_angle); + component.register_input("/gimbal/top_yaw/velocity", top_yaw_velocity); + component.register_input("/gimbal/bottom_yaw/angle", bottom_yaw_angle); + component.register_input("/gimbal/bottom_yaw/velocity", bottom_yaw_velocity); + component.register_input("/gimbal/pitch/angle", pitch_angle); + component.register_input("/gimbal/pitch/velocity", pitch_velocity); + component.register_input("/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu); + } + + auto enable_control() const noexcept -> bool { + using namespace rmcs_msgs; + if ((*switch_left == Switch::UNKNOWN || *switch_right == Switch::UNKNOWN) + || (*switch_left == Switch::DOWN && *switch_right == Switch::DOWN)) { + return false; + } + return true; + } + + InputInterface joystick_left; + InputInterface switch_right; + InputInterface switch_left; + InputInterface mouse_velocity; + + InputInterface timestamp; + InputInterface tf; + + InputInterface top_yaw_angle; + InputInterface top_yaw_velocity; + InputInterface bottom_yaw_angle; + InputInterface bottom_yaw_velocity; + InputInterface pitch_angle; + InputInterface pitch_velocity; + InputInterface chassis_yaw_velocity_imu; + } input_{*this}; + + struct Output { + explicit Output(rmcs_executor::Component& component) { + component.register_output( + "/gimbal/top_yaw/control_torque", top_yaw_control_torque, kNaN); + component.register_output( + "/gimbal/bottom_yaw/control_torque", bottom_yaw_control_torque, kNaN); + component.register_output("/gimbal/pitch/control_torque", pitch_control_torque, kNaN); + + component.register_output( + "/gimbal/yaw/control_angle_error", yaw_control_angle_error, kNaN); + component.register_output("/gimbal/yaw/angle", yaw_angle, 0.0); + component.register_output("/gimbal/yaw/velocity", yaw_velocity, 0.0); + } + + OutputInterface top_yaw_control_torque; + OutputInterface bottom_yaw_control_torque; + OutputInterface pitch_control_torque; + + OutputInterface yaw_control_angle_error; + OutputInterface yaw_angle; + OutputInterface yaw_velocity; + } output_{*this}; + + pid::PidCalculator top_yaw_angle_pid_{pid::make_pid_calculator(*this, "top_yaw_angle_")}; + pid::PidCalculator top_yaw_velocity_pid_{pid::make_pid_calculator(*this, "top_yaw_velocity_")}; + pid::PidCalculator bottom_yaw_angle_pid_{pid::make_pid_calculator(*this, "bottom_yaw_angle_")}; + pid::PidCalculator bottom_yaw_velocity_pid_{ + pid::make_pid_calculator(*this, "bottom_yaw_velocity_")}; + pid::PidCalculator pitch_angle_pid_{pid::make_pid_calculator(*this, "pitch_angle_")}; + pid::PidCalculator pitch_velocity_pid_{pid::make_pid_calculator(*this, "pitch_velocity_")}; + + double manual_bottom_yaw_target_ = 0.0; + double manual_pitch_target_ = 0.0; + double previous_actual_yaw_ = 0.0; + std::chrono::steady_clock::time_point previous_yaw_timestamp_{}; + + static constexpr auto limit_rad(double angle) -> double { + constexpr double kPi = std::numbers::pi_v; + while (angle > kPi) + angle -= 2.0 * kPi; + while (angle <= -kPi) + angle += 2.0 * kPi; + return angle; + } + + auto reset_all_controls() -> void { + top_yaw_angle_pid_.reset(); + top_yaw_velocity_pid_.reset(); + bottom_yaw_angle_pid_.reset(); + bottom_yaw_velocity_pid_.reset(); + pitch_angle_pid_.reset(); + pitch_velocity_pid_.reset(); + + *output_.top_yaw_control_torque = kNaN; + *output_.bottom_yaw_control_torque = kNaN; + *output_.pitch_control_torque = kNaN; + } + + auto enter_disabled_state() -> void { + reset_all_controls(); + + manual_bottom_yaw_target_ = current_bottom_world_yaw(); + manual_pitch_target_ = + std::clamp(limit_rad(*input_.pitch_angle), upper_limit_, lower_limit_); + + *output_.yaw_control_angle_error = kNaN; + } + + auto compute_actual_yaw_velocity(double actual_yaw) -> double { + const auto now = *input_.timestamp; + const double dt = std::chrono::duration(now - previous_yaw_timestamp_).count(); + double velocity = 0.0; + if (dt > 1e-6) + velocity = limit_rad(actual_yaw - previous_actual_yaw_) / dt; + previous_actual_yaw_ = actual_yaw; + previous_yaw_timestamp_ = now; + return velocity; + } + + auto current_barrel_yaw_pitch() const -> std::pair { + auto direction = fast_tf::cast( + PitchLink::DirectionVector{Eigen::Vector3d::UnitX()}, *input_.tf); + Eigen::Vector3d vector = *direction; + if (vector.norm() > 1e-9) + vector.normalize(); + else + vector = Eigen::Vector3d::UnitX(); + const double xy_norm = std::hypot(vector.x(), vector.y()); + return {std::atan2(vector.y(), vector.x()), std::atan2(-vector.z(), xy_norm)}; + } + + auto current_bottom_world_yaw() const -> double { + auto direction = fast_tf::cast( + BottomYawLink::DirectionVector{Eigen::Vector3d::UnitX()}, *input_.tf); + Eigen::Vector3d vector = *direction; + vector.z() = 0.0; + if (vector.norm() > 1e-9) + vector.normalize(); + else + vector = Eigen::Vector3d::UnitX(); + return std::atan2(vector.y(), vector.x()); + } + + auto apply_control(const ControlTarget& target) -> void { + + constexpr auto friction_feedforward = [](double viscous_gain, double coulomb_gain, + double tanh_gain, double velocity) -> double { + return (viscous_gain * velocity) + (coulomb_gain * std::tanh(tanh_gain * velocity)); + }; + + const double current_bottom_angle = current_bottom_world_yaw(); + const double current_bottom_velocity = + *input_.bottom_yaw_velocity + *input_.chassis_yaw_velocity_imu; + const double current_top_angle = limit_rad(*input_.top_yaw_angle); + const double current_pitch_angle = limit_rad(*input_.pitch_angle); + + const double bottom_yaw_error = limit_rad(target.bottom_yaw.target - current_bottom_angle); + const double top_yaw_error = limit_rad(target.top_yaw.target - current_top_angle); + const double pitch_error = limit_rad(target.pitch.target - current_pitch_angle); + + const double bottom_velocity_ref = + bottom_yaw_angle_pid_.update(bottom_yaw_error) + target.bottom_yaw.velocity_ff; + const double top_velocity_ref = + top_yaw_angle_pid_.update(top_yaw_error) + target.top_yaw.velocity_ff; + const double pitch_velocity_ref = + pitch_angle_pid_.update(pitch_error) + target.pitch.velocity_ff; + + const double bottom_world_velocity_ff = + target.bottom_yaw.velocity_ff + *input_.chassis_yaw_velocity_imu; + const double top_yaw_continuous_torque_ff = + target.top_yaw.acceleration_ff + top_yaw_viscous_ff_gain_ * target.top_yaw.velocity_ff; + const double bottom_yaw_torque_ff = + target.bottom_yaw.acceleration_ff + + friction_feedforward( + bottom_yaw_viscous_ff_gain_, bottom_yaw_coulomb_ff_gain_, + bottom_yaw_coulomb_ff_tanh_gain_, bottom_world_velocity_ff) + - k_top_to_bottom_ * top_yaw_continuous_torque_ff; + + const double top_yaw_torque_ff = target.top_yaw.acceleration_ff + + friction_feedforward( + top_yaw_viscous_ff_gain_, top_yaw_coulomb_ff_gain_, + top_yaw_coulomb_ff_tanh_gain_, top_velocity_ref); + const double pitch_torque_ff = + target.pitch.acceleration_ff + + friction_feedforward( + pitch_viscous_ff_gain_, pitch_coulomb_ff_gain_, pitch_coulomb_ff_tanh_gain_, + pitch_velocity_ref) + + pitch_gravity_ff_gain_ * std::sin(current_pitch_angle - pitch_gravity_ff_phase_); + + *output_.bottom_yaw_control_torque = + bottom_yaw_velocity_pid_.update(bottom_velocity_ref - current_bottom_velocity) + + bottom_yaw_torque_ff; + *output_.top_yaw_control_torque = + top_yaw_velocity_pid_.update(top_velocity_ref - *input_.top_yaw_velocity) + + top_yaw_torque_ff; + *output_.pitch_control_torque = + pitch_velocity_pid_.update(pitch_velocity_ref - *input_.pitch_velocity) + + pitch_torque_ff; + + *output_.yaw_control_angle_error = bottom_yaw_error; + } +}; + +} // namespace rmcs_core::controller::gimbal + +#include + +PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::gimbal::EccentricDualYaw, rmcs_executor::Component) From ba3bc226fc44361d7c92977978ea8e8378792168 Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Sun, 26 Apr 2026 03:43:13 +0800 Subject: [PATCH 6/9] build(docker): add navigation2 and lua deps for rmcs-navigation, remove dotnet --- Dockerfile | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/Dockerfile b/Dockerfile index 5fce6b00f..f35ba3ae3 100644 --- a/Dockerfile +++ b/Dockerfile @@ -30,8 +30,9 @@ RUN apt-get update && apt-get install -y --no-install-recommends \ libceres-dev \ ros-$ROS_DISTRO-rviz2 \ ros-$ROS_DISTRO-foxglove-bridge \ - dotnet-sdk-8.0 \ ros-$ROS_DISTRO-pcl-ros ros-$ROS_DISTRO-pcl-conversions ros-$ROS_DISTRO-pcl-msgs && \ + ros-$ROS_DISTRO-navigation2 ros-$ROS_DISTRO-nav2-msgs \ + lua5.4 liblua5.4-0 liblua5.4-dev && \ apt-get autoremove -y && apt-get clean && \ rm -rf /var/lib/apt/lists/* /tmp/* From 8a2f80849e240127debeb8be8720e991539020f4 Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Sun, 26 Apr 2026 06:57:34 +0800 Subject: [PATCH 7/9] refactor(sentry): streamline command component and can packet assembly --- rmcs_ws/src/rmcs_core/src/hardware/sentry.cpp | 317 ++++++------------ 1 file changed, 101 insertions(+), 216 deletions(-) diff --git a/rmcs_ws/src/rmcs_core/src/hardware/sentry.cpp b/rmcs_ws/src/rmcs_core/src/hardware/sentry.cpp index 91e6e110d..9d917a2b1 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/sentry.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/sentry.cpp @@ -5,74 +5,38 @@ #include "hardware/device/lk_motor.hpp" #include "hardware/device/supercap.hpp" -#include -#include -#include -#include -#include -#include -#include #include -#include -#include -#include -#include -#include #include #include #include -#include -#include -#include #include -#include -#include #include #include #include #include #include -#include #include namespace rmcs_core::hardware { -using Clock = std::chrono::steady_clock; - class Sentry : public rmcs_executor::Component , public rclcpp::Node { - class SentryCommand; - class GimbalBoard; - class TopBoard; - class BottomBoard; - public: Sentry() : Node( get_component_name(), - rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)) - , command_component_( - create_partner_component(get_component_name() + "_command", *this)) { + rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)) { + register_input("/predefined/timestamp", timestamp_); register_output("/tf", tf_); - gimbal_calibrate_subscription_ = create_subscription( - "/gimbal/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { - gimbal_calibrate_subscription_callback(std::move(msg)); - }); - steers_calibrate_subscription_ = create_subscription( - "/steers/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { - steers_calibrate_subscription_callback(std::move(msg)); - }); - // For command: remote-status - status_service_ = Node::create_service( + using Srv = std_srvs::srv::Trigger; + status_service_ = create_service( "/rmcs/service/robot_status", - [this]( - const std_srvs::srv::Trigger::Request::SharedPtr&, - const std_srvs::srv::Trigger::Response::SharedPtr& response) { + [this](const Srv::Request::SharedPtr&, const Srv::Response::SharedPtr& response) { status_service_callback(response); }); @@ -91,13 +55,6 @@ class Sentry Eigen::Translation3d{0.07128, 0.0, 0.0481}); } - Sentry(const Sentry&) = delete; - Sentry& operator=(const Sentry&) = delete; - Sentry(Sentry&&) = delete; - Sentry& operator=(Sentry&&) = delete; - - ~Sentry() override = default; - auto update() -> void override { top_board_->update(); bottom_board_->update(); @@ -106,77 +63,7 @@ class Sentry gimbal_board_->imu_pose().conjugate()); } - auto command_update() -> void { - top_board_->command_update(); - bottom_board_->command_update(); - } - private: - auto status_service_callback(const std::shared_ptr& response) - -> void { - response->success = true; - - auto feedback_message = std::ostringstream{}; - auto text = [&](std::format_string format, Args&&... args) { - std::println(feedback_message, format, std::forward(args)...); - }; - - text("Gimbal Status"); - text("- Bottom Yaw: {}", bottom_board_->gimbal_bottom_yaw_motor_.last_raw_angle()); - text("- Top Yaw: {}", top_board_->gimbal_top_yaw_motor_.last_raw_angle()); - text("- Pitch Angle: {}", top_board_->gimbal_pitch_motor_.last_raw_angle()); - - text("Chassis Status"); - constexpr auto position = - std::array{"right back", "right front", "left front", "left back"}; - constexpr auto max_length = - std::ranges::max_element(position, {}, &std::string_view::size)->size(); - - for (auto&& [index, motor] : - std::views::zip(position, bottom_board_->chassis_steer_motors_)) { - text("- {:{}}: {}", index, max_length, motor.last_raw_angle()); - } - - response->message = feedback_message.str(); - } - - auto gimbal_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr) -> void { - RCLCPP_INFO( - get_logger(), "[gimbal calibration] New yaw offset: %ld", - bottom_board_->gimbal_bottom_yaw_motor_.calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[gimbal calibration] New top yaw offset: %ld", - top_board_->gimbal_top_yaw_motor_.calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[gimbal calibration] New pitch offset: %ld", - top_board_->gimbal_pitch_motor_.calibrate_zero_point()); - } - - auto steers_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr) -> void { - RCLCPP_INFO( - get_logger(), "[steer calibration] New left front offset: %d", - bottom_board_->chassis_steer_motors_[2].calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[steer calibration] New left back offset: %d", - bottom_board_->chassis_steer_motors_[3].calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[steer calibration] New right back offset: %d", - bottom_board_->chassis_steer_motors_[0].calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[steer calibration] New right front offset: %d", - bottom_board_->chassis_steer_motors_[1].calibrate_zero_point()); - } - - class SentryCommand : public rmcs_executor::Component { - public: - explicit SentryCommand(Sentry& sentry) - : sentry(sentry) {} - - auto update() -> void override { sentry.command_update(); } - - Sentry& sentry; - }; - class GimbalBoard final : private librmcs::agent::CBoard { public: explicit GimbalBoard(std::string_view board_serial = {}) @@ -215,11 +102,12 @@ class Sentry }; class TopBoard final : private librmcs::agent::RmcsBoardLite { - public: friend class Sentry; + + public: explicit TopBoard( - Sentry& sentry, SentryCommand& sentry_command, std::string_view board_serial = {}, - librmcs::agent::AdvancedOptions options = {}) + Sentry& sentry, rmcs_executor::Component& sentry_command, + std::string_view board_serial = {}, librmcs::agent::AdvancedOptions options = {}) : librmcs::agent::RmcsBoardLite(board_serial, options) , tf_(sentry.tf_) , bmi088_(1000, 0.2, 0.0) @@ -256,13 +144,6 @@ class Sentry [](double x, double y, double z) { return std::make_tuple(-x, -y, z); }); } - TopBoard(const TopBoard&) = delete; - TopBoard& operator=(const TopBoard&) = delete; - TopBoard(TopBoard&&) = delete; - TopBoard& operator=(TopBoard&&) = delete; - - ~TopBoard() override = default; - auto update() -> void { gimbal_top_yaw_motor_.update_status(); gimbal_pitch_motor_.update_status(); @@ -370,11 +251,12 @@ class Sentry }; class BottomBoard final : private librmcs::agent::CBoard { - public: friend class Sentry; + public: explicit BottomBoard( - Sentry& sentry, SentryCommand& sentry_command, std::string_view board_serial = {}) + Sentry& sentry, rmcs_executor::Component& sentry_command, + std::string_view board_serial = {}) : librmcs::agent::CBoard(board_serial) , imu_(1000, 0.2, 0.0) , tf_(sentry.tf_) @@ -403,52 +285,37 @@ class Sentry return size; }; + const auto zero_point = sentry.get_parameter("bottom_yaw_motor_zero_point").as_int(); gimbal_bottom_yaw_motor_.configure( device::LkMotor::Config{device::LkMotor::Type::kMG6012Ei8} .set_reversed() - .set_encoder_zero_point( - static_cast( - sentry.get_parameter("bottom_yaw_motor_zero_point").as_int()))); + .set_encoder_zero_point(static_cast(zero_point))); - for (auto& motor : chassis_wheel_motors_) + for (auto& motor : chassis_wheel_motors_) { motor.configure( device::DjiMotor::Config{device::DjiMotor::Type::kM3508} .set_reduction_ratio(11.) .enable_multi_turn_angle() .set_reversed()); - chassis_steer_motors_[2].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} - .set_reversed() - .set_encoder_zero_point( - static_cast(sentry.get_parameter("left_front_zero_point").as_int())) - .enable_multi_turn_angle()); - chassis_steer_motors_[3].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} - .set_reversed() - .set_encoder_zero_point( - static_cast(sentry.get_parameter("left_back_zero_point").as_int())) - .enable_multi_turn_angle()); - chassis_steer_motors_[0].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} - .set_reversed() - .set_encoder_zero_point( - static_cast(sentry.get_parameter("right_back_zero_point").as_int())) - .enable_multi_turn_angle()); - chassis_steer_motors_[1].configure( - device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} - .set_reversed() - .set_encoder_zero_point( - static_cast(sentry.get_parameter("right_front_zero_point").as_int())) - .enable_multi_turn_angle()); - sentry.register_output("/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_, 0); - } + } - BottomBoard(const BottomBoard&) = delete; - BottomBoard& operator=(const BottomBoard&) = delete; - BottomBoard(BottomBoard&&) = delete; - BottomBoard& operator=(BottomBoard&&) = delete; + constexpr auto kSteerNames = std::array{ + "right_back_zero_point", + "right_front_zero_point", + "left_front_zero_point", + "left_back_zero_point", + }; + for (auto&& [motor, name] : std::views::zip(chassis_steer_motors_, kSteerNames)) { + const auto zero_point = sentry.get_parameter(name).as_int(); + motor.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} + .set_reversed() + .set_encoder_zero_point(static_cast(zero_point)) + .enable_multi_turn_angle()); + } - ~BottomBoard() override = default; + sentry.register_output("/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_, 0); + } auto update() -> void { imu_.update_status(); @@ -467,60 +334,36 @@ class Sentry } auto command_update() -> void { + using namespace device; + auto builder = start_transmit(); builder.can1_transmit({ .can_id = 0x141, .can_data = gimbal_bottom_yaw_motor_.generate_command().as_bytes(), }); + auto cache = CanPacket8{}; + auto generate = [&](std::uint32_t id, std::ranges::range auto& motors, auto... args) { + auto command = [&](T arg) { + if constexpr (std::same_as) { + return arg; + } else { + const auto valid = arg >= 0 && arg < 4; + return valid ? motors[arg].generate_command() + : CanPacket8::PaddingQuarter{}; + } + }; + cache = CanPacket8{command(args)...}; + return librmcs::data::CanDataView{.can_id = id, .can_data = cache.as_bytes()}; + }; + if (can_transmission_mode_) { - builder - .can1_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - chassis_wheel_motors_[1].generate_command(), - chassis_wheel_motors_[0].generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }) - .can2_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - device::CanPacket8::PaddingQuarter{}, - chassis_wheel_motors_[2].generate_command(), - device::CanPacket8::PaddingQuarter{}, - chassis_wheel_motors_[3].generate_command(), - } - .as_bytes(), - }); + builder.can1_transmit(generate(0x200, chassis_wheel_motors_, 1, 0, -1, -1)) + .can2_transmit(generate(0x200, chassis_wheel_motors_, -1, 2, -1, 3)); } else { - builder - .can1_transmit({ - .can_id = 0x1FE, - .can_data = - device::CanPacket8{ - chassis_steer_motors_[1].generate_command(), - chassis_steer_motors_[0].generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }) - .can2_transmit({ - .can_id = 0x1FE, - .can_data = - device::CanPacket8{ - chassis_steer_motors_[2].generate_command(), - chassis_steer_motors_[3].generate_command(), - device::CanPacket8::PaddingQuarter{}, - supercap_.generate_command(), - } - .as_bytes(), - }); + builder.can1_transmit(generate(0x1FE, chassis_steer_motors_, 1, 0, -1, -1)) + .can2_transmit(generate( + 0x1FE, chassis_steer_motors_, 2, 3, -1, supercap_.generate_command())); } can_transmission_mode_ = !can_transmission_mode_; } @@ -594,17 +437,59 @@ class Sentry OutputInterface chassis_yaw_velocity_imu_; }; - InputInterface timestamp_; + struct CommandTransmitter : public rmcs_executor::Component { + std::function fn; + + template + explicit CommandTransmitter(Fn&& fn) + : fn{std::forward(fn)} {} + + auto update() -> void override { fn(); } + }; + + auto status_service_callback(const std::shared_ptr& response) + -> void { + response->success = true; + + auto feedback_message = std::ostringstream{}; + auto text = [&](std::format_string format, Args&&... args) { + std::println(feedback_message, format, std::forward(args)...); + }; + + text("Gimbal Status"); + text("- Bottom Yaw: {}", bottom_board_->gimbal_bottom_yaw_motor_.last_raw_angle()); + text("- Top Yaw: {}", top_board_->gimbal_top_yaw_motor_.last_raw_angle()); + text("- Pitch Angle: {}", top_board_->gimbal_pitch_motor_.last_raw_angle()); + + text("Chassis Status"); + constexpr auto kPosition = + std::array{"right back", "right front", "left front", "left back"}; + constexpr auto kMaxLength = + std::ranges::max_element(kPosition, {}, &std::string_view::size)->size(); + + for (auto&& [index, motor] : + std::views::zip(kPosition, bottom_board_->chassis_steer_motors_)) { + text("- {:{}}: {}", index, kMaxLength, motor.last_raw_angle()); + } + + response->message = feedback_message.str(); + } + + auto command_update() -> void { + top_board_->command_update(); + bottom_board_->command_update(); + } + std::shared_ptr command_component_{ + create_partner_component( + get_component_name() + "_command", [this] { command_update(); })}; + + InputInterface timestamp_; OutputInterface tf_; - std::shared_ptr command_component_; std::unique_ptr gimbal_board_; std::unique_ptr top_board_; std::unique_ptr bottom_board_; - rclcpp::Subscription::SharedPtr gimbal_calibrate_subscription_; - rclcpp::Subscription::SharedPtr steers_calibrate_subscription_; - std::shared_ptr> status_service_; }; From 54799155d0f267573ff64e86fdc656b32f7b6c44 Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Sun, 26 Apr 2026 07:17:14 +0800 Subject: [PATCH 8/9] feat: tool script for status checking --- .script/remote-status | 65 +++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 65 insertions(+) create mode 100755 .script/remote-status diff --git a/.script/remote-status b/.script/remote-status new file mode 100755 index 000000000..f5805e05c --- /dev/null +++ b/.script/remote-status @@ -0,0 +1,65 @@ +#!/bin/env bash + +set -euo pipefail + +# 定义需要查询的服务列表 +services=( + "/rmcs/service/robot_status" +) + +# 使用 bash 远程执行，并传递服务列表作为参数 +ssh remote bash -s -- "${services[@]}" <<'EOF' +set -euo pipefail + +# 1. 环境准备 +set +u +if [ -f ~/env_setup.bash ]; then + source ~/env_setup.bash +fi +set -u + +# 获取所有可用服务列表 +all_services="$(ros2 service list 2>/dev/null || true)" +input_services=("${@}") + +# 2. 定义单个服务查询函数 +call_status_service() { + local service="$1" + + if ! printf "%s\n" "$all_services" | grep -Fxq "$service"; then + printf "[warn] service not found: %s\n\n" "$service" + return + fi + + printf "=== %s ===\n" "$service" + local raw + raw="$(ros2 service call "$service" std_srvs/srv/Trigger "{}" 2>&1 || true)" + + local msg + msg="$(printf "%s\n" "$raw" | sed -n "s/.*message='\(.*\)'.*/\1/p")" + + if [[ -z "$msg" ]]; then + printf "[warn] failed to parse message: %s\n%s\n" "$service" "$raw" + else + printf "%b\n" "$msg" + fi + printf "\n" +} + +# 3. 并发执行逻辑 +tmp_dir=$(mktemp -d) +trap 'rm -rf "${tmp_dir}"' EXIT + +for i in "${!input_services[@]}"; do + call_status_service "${input_services[$i]}" > "${tmp_dir}/${i}" 2>&1 & +done + +wait + +# 4. 按原始顺序汇总输出 +for i in "${!input_services[@]}"; do + if [ -f "${tmp_dir}/${i}" ]; then + cat "${tmp_dir}/${i}" + fi +done +EOF From 7978980ad78f6166d4476d1f75f7830cc2985f56 Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Sun, 26 Apr 2026 07:21:57 +0800 Subject: [PATCH 9/9] chore: update sentry config --- rmcs_ws/src/rmcs_bringup/config/sentry.yaml | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/sentry.yaml b/rmcs_ws/src/rmcs_bringup/config/sentry.yaml index 7cd0a6799..85e636c1c 100644 --- a/rmcs_ws/src/rmcs_bringup/config/sentry.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/sentry.yaml @@ -33,13 +33,13 @@ sentry_hardware: board_serial_top_board: "af-da30" board_serial_bottom_board: "d4-2184" board_serial_gimbal_board: "d4-1d2b" - bottom_yaw_motor_zero_point: 15151 - top_yaw_motor_zero_point: 4453 + bottom_yaw_motor_zero_point: 26163 + top_yaw_motor_zero_point: 4540 pitch_motor_zero_point: 1446 - left_front_zero_point: 5768 - left_back_zero_point: 5110 - right_back_zero_point: 2685 - right_front_zero_point: 6434 + left_front_zero_point: 7131 + left_back_zero_point: 3740 + right_back_zero_point: 1327 + right_front_zero_point: 5147 rmcs_navigation: ros__parameters: