diff --git a/.script/template/entrypoint b/.script/template/entrypoint index e8660e7ac..49290c5c3 100755 --- a/.script/template/entrypoint +++ b/.script/template/entrypoint @@ -1,5 +1,18 @@ #!/usr/bin/bash +USBFS_MEMORY_MB="${USBFS_MEMORY_MB:-2000}" +USBFS_MEMORY_MB_PATH="/sys/module/usbcore/parameters/usbfs_memory_mb" + +if [ -w "$USBFS_MEMORY_MB_PATH" ]; then + if ! printf '%s\n' "$USBFS_MEMORY_MB" > "$USBFS_MEMORY_MB_PATH"; then + echo "Warning: failed to set $USBFS_MEMORY_MB_PATH to $USBFS_MEMORY_MB" >&2 + fi +elif [ -e "$USBFS_MEMORY_MB_PATH" ]; then + echo "Warning: $USBFS_MEMORY_MB_PATH is not writable, skipping usbfs_memory_mb setup" >&2 +else + echo "Warning: $USBFS_MEMORY_MB_PATH not found, skipping usbfs_memory_mb setup" >&2 +fi + # Remove all files in /tmp rm -rf /tmp/* @@ -14,4 +27,4 @@ if [ -f "/etc/avahi/enabled" ]; then service avahi-daemon start fi -sleep infinity \ No newline at end of file +sleep infinity diff --git a/rmcs_ws/src/rmcs_bringup/config/flight.yaml b/rmcs_ws/src/rmcs_bringup/config/flight.yaml new file mode 100644 index 000000000..2291e12ac --- /dev/null +++ b/rmcs_ws/src/rmcs_bringup/config/flight.yaml @@ -0,0 +1,153 @@ +rmcs_executor: + ros__parameters: + update_rate: 1000.0 + components: + - rmcs_core::hardware::Flight -> flight_hardware + + - rmcs_core::controller::gimbal::SimpleGimbalController -> gimbal_controller + - rmcs_core::controller::pid::ErrorPidController -> yaw_angle_pid_controller + - rmcs_core::controller::pid::PidController -> yaw_velocity_pid_controller + - rmcs_core::controller::pid::ErrorPidController -> pitch_angle_pid_controller + + - rmcs_core::controller::shooting::FrictionWheelController -> friction_wheel_controller + - rmcs_core::controller::shooting::HeatController -> heat_controller + - rmcs_core::controller::shooting::BulletFeederController17mm -> bullet_feeder_controller + - rmcs_core::controller::pid::PidController -> left_friction_velocity_pid_controller + - rmcs_core::controller::pid::PidController -> right_friction_velocity_pid_controller + - rmcs_core::controller::pid::PidController -> bullet_feeder_velocity_pid_controller + + - rmcs_core::referee::command::Interaction -> referee_interaction + - rmcs_core::referee::Command -> referee_command + - rmcs_core::referee::Status -> referee_status + - rmcs_core::referee::command::interaction::Ui -> referee_ui + - rmcs_core::referee::app::ui::Flight -> referee_ui_flight + + # - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster + # - rmcs_core::broadcaster::TfBroadcaster -> tf_broadcaster + + - rmcs::AutoAimComponent + +odin_ros_driver: + ros__parameters: + enabled: true + config_file: "/rmcs_install/share/odin_ros_driver/config/control_command.yaml" + node_name: host_sdk_sample + respawn: true + respawn_delay: 1.0 + +value_broadcaster: + ros__parameters: + forward_list: + - /gimbal/pitch/angle + - /gimbal/pitch/velocity + - /gimbal/pitch/torque + - /gimbal/pitch/control_angle_error + - /gimbal/pitch/control_velocity + - /gimbal/pitch/velocity_imu + + - /gimbal/yaw/angle + - /gimbal/yaw/velocity + - /gimbal/yaw/torque + - /gimbal/yaw/control_torque + - /gimbal/yaw/control_angle_error + - /gimbal/yaw/velocity_imu + - /gimbal/bullet_feeder/velocity + +tf_broadcaster: + ros__parameters: + tf: /tf + +flight_hardware: + ros__parameters: + board_serial: "AF-7C58-5458-E731-9F74-1F9C-CAFD-30AF-9C09" + yaw_motor_zero_point: 11720 + pitch_motor_zero_point: 18578 + +referee_status: + ros__parameters: + path: /dev/tty0 + +gimbal_controller: + ros__parameters: + upper_limit: -0.39518 # -0.39518 rad ≈ -22.6° + lower_limit: 0.7 # 0.7 rad ≈ 40.1° + yaw_lower_limit: 0.1745 + yaw_upper_limit: 2.5708 + +yaw_angle_pid_controller: + ros__parameters: + measurement: /gimbal/yaw/control_angle_error + control: /gimbal/yaw/control_velocity + kp: 15.0 + ki: 0.0 + kd: 0.0 + +yaw_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/yaw/velocity_imu + setpoint: /gimbal/yaw/control_velocity + control: /gimbal/yaw/control_torque + kp: 8.0 + ki: 0.0 + kd: 0.0 + +pitch_angle_pid_controller: + ros__parameters: + measurement: /gimbal/pitch/control_angle_error + control: /gimbal/pitch/control_velocity + kp: 10.0 + ki: 0.0 + kd: 0.0 + +friction_wheel_controller: + ros__parameters: + friction_wheels: + - /gimbal/left_friction + - /gimbal/right_friction + friction_velocities: + - 620.0 + - 620.0 + friction_soft_start_stop_time: 1.0 + +heat_controller: + ros__parameters: + heat_per_shot: 10000 + reserved_heat: 15000 + +bullet_feeder_controller: + ros__parameters: + bullets_per_feeder_turn: 8.0 + shot_frequency: 20.0 + safe_shot_frequency: 10.0 + eject_frequency: 10.0 + eject_time: 0.05 + deep_eject_frequency: 5.0 + deep_eject_time: 0.2 + single_shot_max_stop_delay: 2.0 + +left_friction_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/left_friction/velocity + setpoint: /gimbal/left_friction/control_velocity + control: /gimbal/left_friction/control_torque + kp: 0.003436926 + ki: 0.00 + kd: 0.009373434 + +right_friction_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/right_friction/velocity + setpoint: /gimbal/right_friction/control_velocity + control: /gimbal/right_friction/control_torque + kp: 0.003436926 + ki: 0.00 + kd: 0.009373434 + +bullet_feeder_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/bullet_feeder/velocity + setpoint: /gimbal/bullet_feeder/control_velocity + control: /gimbal/bullet_feeder/control_torque + kp: 0.583 + ki: 0.0 + kd: 0.0 diff --git a/rmcs_ws/src/rmcs_core/CMakeLists.txt b/rmcs_ws/src/rmcs_core/CMakeLists.txt index d9504de27..81bf0b613 100644 --- a/rmcs_ws/src/rmcs_core/CMakeLists.txt +++ b/rmcs_ws/src/rmcs_core/CMakeLists.txt @@ -8,29 +8,30 @@ set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-packed-bitfield-compat") if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-O2 -Wall -Wextra -Wpedantic) + add_compile_options(-O2 -Wall -Wextra -Wpedantic) endif() find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() + include(FetchContent) set(BUILD_STATIC_LIBRMCS ON CACHE BOOL "Build static librmcs SDK" FORCE) FetchContent_Declare( - librmcs - URL https://github.com/Alliance-Algorithm/librmcs/releases/download/v3.2.0/librmcs-sdk-src-3.2.0.zip - URL_HASH SHA256=f81c3af7fbcf35727a8a7586200db8e9bf668ee0f448529de4bfd3eb7c36ed6f - DOWNLOAD_EXTRACT_TIMESTAMP TRUE + librmcs + URL https://github.com/Alliance-Algorithm/librmcs/releases/download/v3.2.0/librmcs-sdk-src-3.2.0.zip + URL_HASH SHA256=f81c3af7fbcf35727a8a7586200db8e9bf668ee0f448529de4bfd3eb7c36ed6f + DOWNLOAD_EXTRACT_TIMESTAMP TRUE ) FetchContent_MakeAvailable(librmcs) file(GLOB_RECURSE PROJECT_SOURCE CONFIGURE_DEPENDS - ${PROJECT_SOURCE_DIR}/src/*.cpp - ${PROJECT_SOURCE_DIR}/src/*.c + ${PROJECT_SOURCE_DIR}/src/*.cpp + ${PROJECT_SOURCE_DIR}/src/*.c ) ament_auto_add_library( - ${PROJECT_NAME} SHARED - ${PROJECT_SOURCE} + ${PROJECT_NAME} SHARED + ${PROJECT_SOURCE} ) include_directories(${PROJECT_SOURCE_DIR}/include) diff --git a/rmcs_ws/src/rmcs_core/plugins.xml b/rmcs_ws/src/rmcs_core/plugins.xml index 82ffac1c3..1f46763eb 100644 --- a/rmcs_ws/src/rmcs_core/plugins.xml +++ b/rmcs_ws/src/rmcs_core/plugins.xml @@ -1,5 +1,5 @@ - + @@ -7,6 +7,7 @@ + @@ -20,14 +21,14 @@ - + - + @@ -36,18 +37,23 @@ + + + + + + - - + diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/simple_gimbal_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/simple_gimbal_controller.cpp index d27704d52..dfd361313 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/simple_gimbal_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/simple_gimbal_controller.cpp @@ -20,9 +20,11 @@ class SimpleGimbalController : Node( get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) - , two_axis_gimbal_solver( - *this, get_parameter("upper_limit").as_double(), - get_parameter("lower_limit").as_double()) { + , two_axis_gimbal_solver{ + *this, + get_parameter("upper_limit").as_double(), + get_parameter("lower_limit").as_double(), + } { register_input("/remote/joystick/left", joystick_left_); register_input("/remote/switch/right", switch_right_); @@ -30,10 +32,15 @@ class SimpleGimbalController register_input("/remote/mouse/velocity", mouse_velocity_); register_input("/remote/mouse", mouse_); - register_input("/gimbal/auto_aim/control_direction", auto_aim_control_direction_, false); + register_input("/auto_aim/should_control", auto_aim_should_control_, false); + register_input("/auto_aim/control_direction", auto_aim_control_direction_, false); register_output("/gimbal/yaw/control_angle_error", yaw_angle_error_, nan_); register_output("/gimbal/pitch/control_angle_error", pitch_angle_error_, nan_); + + two_axis_gimbal_solver.enable_yaw_limit( + *this, get_parameter("yaw_upper_limit").as_double(), + get_parameter("yaw_lower_limit").as_double()); } void update() override { @@ -45,18 +52,22 @@ class SimpleGimbalController TwoAxisGimbalSolver::AngleError calculate_angle_error() { auto switch_right = *switch_right_; auto switch_left = *switch_left_; - auto mouse = *mouse_; using namespace rmcs_msgs; if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) return two_axis_gimbal_solver.update(TwoAxisGimbalSolver::SetDisabled()); - if (auto_aim_control_direction_.ready() && (mouse.right || switch_right == Switch::UP) - && !auto_aim_control_direction_->isZero()) + const auto auto_aim_requested = mouse_->right || switch_right == Switch::UP; + const auto should_control = auto_aim_should_control_.ready() && *auto_aim_should_control_; + const auto valid_control = + auto_aim_control_direction_.ready() && auto_aim_control_direction_->allFinite(); + + if (auto_aim_requested && should_control && valid_control) { return two_axis_gimbal_solver.update( TwoAxisGimbalSolver::SetControlDirection( OdomImu::DirectionVector(*auto_aim_control_direction_))); + } if (!two_axis_gimbal_solver.enabled()) return two_axis_gimbal_solver.update(TwoAxisGimbalSolver::SetToLevel()); @@ -67,7 +78,7 @@ class SimpleGimbalController double yaw_shift = joystick_sensitivity * joystick_left_->y() + mouse_sensitivity * mouse_velocity_->y(); double pitch_shift = - -joystick_sensitivity * joystick_left_->x() - mouse_sensitivity * mouse_velocity_->x(); + -joystick_sensitivity * joystick_left_->x() + mouse_sensitivity * mouse_velocity_->x(); return two_axis_gimbal_solver.update( TwoAxisGimbalSolver::SetControlShift(yaw_shift, pitch_shift)); @@ -82,6 +93,7 @@ class SimpleGimbalController InputInterface mouse_velocity_; InputInterface mouse_; + InputInterface auto_aim_should_control_; InputInterface auto_aim_control_direction_; TwoAxisGimbalSolver two_axis_gimbal_solver; @@ -94,4 +106,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/gimbal/two_axis_gimbal_solver.hpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/two_axis_gimbal_solver.hpp index c1fc772f3..038319695 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/two_axis_gimbal_solver.hpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/two_axis_gimbal_solver.hpp @@ -1,7 +1,9 @@ #pragma once +#include #include #include +#include #include #include @@ -32,6 +34,13 @@ class TwoAxisGimbalSolver { component.register_input("/tf", tf_); } + void enable_yaw_limit( + rmcs_executor::Component& component, double yaw_upper_limit, double yaw_lower_limit) { + yaw_cw_max_ = yaw_upper_limit; + yaw_cw_min_ = yaw_lower_limit; + component.register_input("/gimbal/yaw/angle", gimbal_yaw_angle_); + } + class SetDisabled : public Operation { PitchLink::DirectionVector update(TwoAxisGimbalSolver& super) const override { super.control_enabled_ = false; @@ -113,6 +122,8 @@ class TwoAxisGimbalSolver { if (!control_enabled_) return {nan_, nan_}; + clamp_yaw_limit(control_direction_yaw_link); + control_direction_ = fast_tf::cast(yaw_link_to_pitch_link(control_direction_yaw_link, pitch), *tf_); return calculate_control_errors(control_direction_yaw_link, pitch); @@ -188,6 +199,29 @@ class TwoAxisGimbalSolver { *control_direction << lower_limit_.x() * projection, lower_limit_.y(); } + void clamp_yaw_limit(YawLink::DirectionVector& control_direction) { + if (!gimbal_yaw_angle_.ready()) + return; + + constexpr double two_pi = 2 * std::numbers::pi; + double cw = std::fmod(two_pi - *gimbal_yaw_angle_, two_pi); + if (cw < 0) + cw += two_pi; + + const auto& [x, y, z] = *control_direction; + const double err = std::atan2(y, x); + + const double target_cw = cw - err; + const double clamped_cw = std::clamp(target_cw, yaw_cw_min_, yaw_cw_max_); + if (clamped_cw == target_cw) + return; + + // delta = err_new - err = (cw - clamped_cw) - err + const double delta = (cw - clamped_cw) - err; + const double c = std::cos(delta), s = std::sin(delta); + *control_direction << c * x - s * y, s * x + c * y, z; + } + static AngleError calculate_control_errors( const YawLink::DirectionVector& control_direction, const Eigen::Vector2d& pitch) { const auto& [x, y, z] = *control_direction; @@ -209,6 +243,10 @@ class TwoAxisGimbalSolver { rmcs_executor::Component::InputInterface gimbal_pitch_angle_; rmcs_executor::Component::InputInterface tf_; + double yaw_cw_min_ = 0.; + double yaw_cw_max_ = 0.; + rmcs_executor::Component::InputInterface gimbal_yaw_angle_; + OdomImu::DirectionVector yaw_axis_filtered_{Eigen::Vector3d::UnitZ()}; bool control_enabled_ = false; diff --git a/rmcs_ws/src/rmcs_core/src/controller/shooting/bullet_feeder_controller_17mm.cpp b/rmcs_ws/src/rmcs_core/src/controller/shooting/bullet_feeder_controller_17mm.cpp index 0c97c98a0..4b845caae 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/shooting/bullet_feeder_controller_17mm.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/shooting/bullet_feeder_controller_17mm.cpp @@ -1,7 +1,3 @@ -#include - -#include - #include #include #include @@ -53,7 +49,7 @@ class BulletFeederController17mm register_input("/remote/mouse", mouse_); register_input("/remote/keyboard", keyboard_); - register_input("/gimbal/auto_aim/fire_control", fire_control_, false); + register_input("/auto_aim/should_shoot", should_shoot_, false); register_input("/gimbal/bullet_feeder/velocity", bullet_feeder_velocity_); register_output( @@ -63,8 +59,8 @@ class BulletFeederController17mm } void before_updating() override { - if (!fire_control_.ready()) - fire_control_.bind_directly(false); + if (!should_shoot_.ready()) + should_shoot_.bind_directly(false); } void update() override { @@ -80,7 +76,7 @@ class BulletFeederController17mm || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) { reset_all_controls(); } else { - int64_t bullet_allowance = 0; + std::int64_t bullet_allowance = 0; if (switch_right != Switch::DOWN) { shoot_mode = keyboard.f ? ShootMode::SINGLE : ShootMode::AUTOMATIC; @@ -102,12 +98,13 @@ class BulletFeederController17mm if (*friction_ready_) { if (shoot_mode == ShootMode::AUTOMATIC) { - bool triggered = mouse.left || switch_left == Switch::DOWN - || (switch_right == Switch::UP && *fire_control_); + auto aiming_enable = mouse_->right || (switch_right == Switch::UP); + auto attack_intent = mouse_->left || (switch_left == Switch::DOWN); + auto triggered = aiming_enable ? *should_shoot_ : attack_intent; bullet_allowance = triggered ? *control_bullet_allowance_limited_by_heat_ : 0; } else { - bool triggered = single_shot_stop_counter_ > 0; + auto triggered = single_shot_stop_counter_ > 0; bullet_allowance = triggered && (*control_bullet_allowance_limited_by_heat_ > 0); } @@ -209,7 +206,7 @@ class BulletFeederController17mm InputInterface mouse_; InputInterface keyboard_; - InputInterface fire_control_; + InputInterface should_shoot_; rmcs_msgs::Switch last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; rmcs_msgs::Switch last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; @@ -232,4 +229,4 @@ class BulletFeederController17mm #include PLUGINLIB_EXPORT_CLASS( - rmcs_core::controller::shooting::BulletFeederController17mm, rmcs_executor::Component) \ No newline at end of file + rmcs_core::controller::shooting::BulletFeederController17mm, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/shooting/heat_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/shooting/heat_controller.cpp index 3de0960ae..c6901fbcb 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/shooting/heat_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/shooting/heat_controller.cpp @@ -53,4 +53,4 @@ class HeatController #include -PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::shooting::HeatController, rmcs_executor::Component) \ No newline at end of file +PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::shooting::HeatController, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/lk_motor.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/lk_motor.hpp index 1bc0ef8a8..60348bb69 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/lk_motor.hpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/lk_motor.hpp @@ -23,7 +23,14 @@ namespace rmcs_core::hardware::device { class LkMotor { public: - enum class Type : uint8_t { kMG5010Ei10, kMG4010Ei10, kMG6012Ei8, kMG4005Ei10, kMG5010Ei36 }; + enum class Type : uint8_t { + kMG5010Ei10, + kMG4010Ei10, + kMG6012Ei8, + kMG4005Ei10, + kMG5010Ei36, + kMHF7015, + }; struct Config { explicit Config(Type type) @@ -110,6 +117,12 @@ class LkMotor { reduction_ratio = 36.0; max_torque_ = 25.0; break; + case Type::kMHF7015: + raw_angle_modulus_ = 1 << 16; + torque_constant = 0.51; + reduction_ratio = 1.0; + max_torque_ = 2.42; + break; default: std::unreachable(); } diff --git a/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp b/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp new file mode 100644 index 000000000..c35bb8dfe --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp @@ -0,0 +1,274 @@ +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "hardware/device/bmi088.hpp" +#include "hardware/device/can_packet.hpp" +#include "hardware/device/dji_motor.hpp" +#include "hardware/device/dr16.hpp" +#include "hardware/device/lk_motor.hpp" +#include "librmcs/agent/rmcs_board_lite.hpp" + +namespace rmcs_core::hardware { + +class Flight + : public rmcs_executor::Component + , public rclcpp::Node + , private librmcs::agent::RmcsBoardLite { +public: + Flight() + : Node{ + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} + , RmcsBoardLite{get_parameter("board_serial").as_string()} { + + gimbal_yaw_motor_.configure( + device::LkMotor::Config{device::LkMotor::Type::kMHF7015} + .set_reversed() + .set_encoder_zero_point( + static_cast(get_parameter("yaw_motor_zero_point").as_int()))); + gimbal_pitch_motor_.configure( + device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10}.set_encoder_zero_point( + static_cast(get_parameter("pitch_motor_zero_point").as_int()))); + gimbal_left_friction_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reversed() + .set_reduction_ratio(1.0)); + gimbal_right_friction_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(1.0)); + gimbal_bullet_feeder_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM2006}.enable_multi_turn_angle()); + + bmi088_.set_coordinate_mapping( + [](double x, double y, double z) { return std::tuple{y, z, x}; }); + + using namespace rmcs_description; + + constexpr auto kCameraPostionX = 0.10238; + constexpr auto kCameraPostionZ = 0.05286; + tf_->set_transform( + Eigen::Translation3d{kCameraPostionX, 0.0, kCameraPostionZ}); + + register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_); + register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_imu_); + register_output("/tf", tf_); + + register_output("/auto_aim/camera_transform", camera_transform_); + register_output("/auto_aim/barrel_direction", barrel_direction_); + + register_output("/referee/serial", referee_serial_); + referee_serial_->read = [this](std::byte* buffer, size_t size) { + return referee_ring_buffer_receive_.pop_front_n( + [&buffer](std::byte byte) noexcept { *buffer++ = byte; }, size); + }; + referee_serial_->write = [this](const std::byte* buffer, size_t size) { + start_transmit().uart1_transmit( + {.uart_data = std::span{buffer, size}}); + return size; + }; + + status_service_ = create_service( + "/rmcs/service/robot_status", + [this]( + const std_srvs::srv::Trigger::Request::SharedPtr&, + const std_srvs::srv::Trigger::Response::SharedPtr& response) { + status_service_callback(response); + }); + } + + void update() override { + update_motors(); + update_imu(); + dr16_.update_status(); + + using namespace rmcs_description; + *camera_transform_ = fast_tf::lookup_transform(*tf_); + *barrel_direction_ = + *fast_tf::cast(PitchLink::DirectionVector{Eigen::Vector3d::UnitX()}, *tf_); + } + + void command_update() { + auto builder = start_transmit(); + builder + .can0_transmit( + {.can_id = 0x200, + .can_data = + device::CanPacket8{ + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + gimbal_left_friction_.generate_command(), + gimbal_right_friction_.generate_command(), + } + .as_bytes()}) + .can1_transmit( + {.can_id = 0x200, + .can_data = + device::CanPacket8{ + gimbal_bullet_feeder_.generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes()}) + .can2_transmit( + {.can_id = 0x141, + .can_data = gimbal_yaw_motor_.generate_torque_command().as_bytes()}) + .can3_transmit( + {.can_id = 0x142, .can_data = gimbal_pitch_motor_.generate_command().as_bytes()}); + } + +private: + void update_motors() { + gimbal_bullet_feeder_.update_status(); + gimbal_left_friction_.update_status(); + gimbal_right_friction_.update_status(); + + using namespace rmcs_description; + + gimbal_yaw_motor_.update_status(); + tf_->set_state(gimbal_yaw_motor_.angle()); + + gimbal_pitch_motor_.update_status(); + tf_->set_state(gimbal_pitch_motor_.angle()); + } + + void update_imu() { + using namespace rmcs_description; + + bmi088_.update_status(); + const auto gimbal_imu_pose = + Eigen::Quaterniond{bmi088_.q0(), bmi088_.q1(), bmi088_.q2(), bmi088_.q3()}; + tf_->set_transform(gimbal_imu_pose.conjugate()); + + *gimbal_yaw_velocity_imu_ = bmi088_.gz(); + *gimbal_pitch_velocity_imu_ = bmi088_.gy(); + } + + void + status_service_callback(const std::shared_ptr& response) { + 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(" yaw_motor_zero_point: {}", gimbal_yaw_motor_.last_raw_angle()); + text(" pitch_motor_zero_point: {}", gimbal_pitch_motor_.last_raw_angle()); + + response->message = feedback_message.str(); + } + +protected: + void can0_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission || data.can_data.size() < 8) + [[unlikely]] + return; + + if (data.can_id == 0x203) { + gimbal_left_friction_.store_status(data.can_data); + } else if (data.can_id == 0x204) { + gimbal_right_friction_.store_status(data.can_data); + } + } + void can1_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission || data.can_data.size() < 8) + [[unlikely]] + return; + + if (data.can_id == 0x201) { + gimbal_bullet_feeder_.store_status(data.can_data); + } + } + + void can2_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission || data.can_data.size() < 8) + [[unlikely]] + return; + + if (data.can_id == 0x141) { + gimbal_yaw_motor_.store_status(data.can_data); + } + } + void can3_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission || data.can_data.size() < 8) + [[unlikely]] + return; + + if (data.can_id == 0x142) { + gimbal_pitch_motor_.store_status(data.can_data); + } + } + + void 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: + class FlightCommand : public rmcs_executor::Component { + public: + explicit FlightCommand(Flight& flight) + : flight_(flight) {} + + void update() override { flight_.command_update(); } + + private: + Flight& flight_; + }; + std::shared_ptr command_component_{ + create_partner_component(get_component_name() + "_command", *this), + }; + std::shared_ptr> status_service_; + + device::LkMotor gimbal_yaw_motor_{*this, *command_component_, "/gimbal/yaw"}; + device::LkMotor gimbal_pitch_motor_{*this, *command_component_, "/gimbal/pitch"}; + device::DjiMotor gimbal_left_friction_{*this, *command_component_, "/gimbal/left_friction"}; + device::DjiMotor gimbal_right_friction_{*this, *command_component_, "/gimbal/right_friction"}; + device::DjiMotor gimbal_bullet_feeder_{*this, *command_component_, "/gimbal/bullet_feeder"}; + + device::Dr16 dr16_{*this}; + device::Bmi088 bmi088_{1000.0, 0.2, 0.00}; + + OutputInterface gimbal_yaw_velocity_imu_; + OutputInterface gimbal_pitch_velocity_imu_; + OutputInterface tf_; + OutputInterface referee_serial_; + + OutputInterface camera_transform_; + OutputInterface barrel_direction_; + + rmcs_utility::RingBuffer referee_ring_buffer_receive_{256}; +}; + +} // namespace rmcs_core::hardware + +#include + +PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::Flight, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/flight.cpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/flight.cpp new file mode 100644 index 000000000..88367c269 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/flight.cpp @@ -0,0 +1,96 @@ +#include +#include + +#include +#include +#include + +#include "referee/app/ui/shape/shape.hpp" +#include "referee/app/ui/widget/crosshair_circle.hpp" +#include "referee/app/ui/widget/status_ring.hpp" + +namespace rmcs_core::referee::app::ui { +class Flight + : public rmcs_executor::Component + , public rclcpp::Node { +public: + static constexpr uint8_t kUiModeCombat = 0; + static constexpr uint8_t kUiModeOutpostOnly = 1; + static constexpr uint8_t kUiModeEngineer = 2; + + Flight() + : Node{ + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} + , crosshair_circle_(Shape::Color::WHITE, x_center - 2, y_center - 30, 8, 2) + , status_ring_(26.5, 26.5, 600, 300, false, false) + , horizontal_center_guidelines_( + {Shape::Color::WHITE, 2, x_center - 360, y_center, x_center - 110, y_center}, + {Shape::Color::WHITE, 2, x_center + 110, y_center, x_center + 360, y_center}) + , vertical_center_guidelines_( + {Shape::Color::WHITE, 2, x_center, 800, x_center, y_center + 110}, + {Shape::Color::WHITE, 2, x_center, y_center - 110, x_center, 200}) + , mode_indicator_( + Shape::Color::YELLOW, 30, x_center + 360, y_center + 220, x_center + 400, + y_center + 220) { + + register_input("/referee/shooter/bullet_allowance", robot_bullet_allowance_); + + register_input("/gimbal/left_friction/control_velocity", left_friction_control_velocity_); + register_input("/gimbal/left_friction/velocity", left_friction_velocity_); + register_input("/gimbal/right_friction/velocity", right_friction_velocity_); + + register_input("/auto_aim/ui_mode", auto_aim_mode_, false); + + register_input("/remote/mouse", mouse_); + } + + void update() override { + status_ring_.update_bullet_allowance(*robot_bullet_allowance_); + status_ring_.update_friction_wheel_speed( + std::min(*left_friction_velocity_, *right_friction_velocity_), + *left_friction_control_velocity_ > 0); + status_ring_.update_auto_aim_enable(mouse_->right == 1); + + update_mode_indicator(); + } + +private: + static constexpr uint16_t screen_width = 1920, screen_height = 1080; + static constexpr uint16_t x_center = screen_width / 2, y_center = screen_height / 2; + + auto update_mode_indicator() -> void { + auto mode_value = auto_aim_mode_.ready() ? *auto_aim_mode_ : kUiModeCombat; + + switch (mode_value) { + case kUiModeOutpostOnly: mode_indicator_.set_color(Shape::Color::GREEN); break; + case kUiModeEngineer: mode_indicator_.set_color(Shape::Color::PINK); break; + case kUiModeCombat: + default: mode_indicator_.set_color(Shape::Color::YELLOW); break; + } + } + + InputInterface robot_bullet_allowance_; + + InputInterface left_friction_control_velocity_; + InputInterface left_friction_velocity_; + InputInterface right_friction_velocity_; + + InputInterface auto_aim_mode_; + + InputInterface mouse_; + + CrossHairCircle crosshair_circle_; + StatusRing status_ring_; + + Line horizontal_center_guidelines_[2]; + Line vertical_center_guidelines_[2]; + + Line mode_indicator_; +}; + +} // namespace rmcs_core::referee::app::ui + +#include + +PLUGINLIB_EXPORT_CLASS(rmcs_core::referee::app::ui::Flight, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/status_ring.hpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/status_ring.hpp index 8af3d993a..c45eaf932 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/status_ring.hpp +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/status_ring.hpp @@ -1,11 +1,10 @@ #pragma once #include +#include #include #include #include - -#include #include #include @@ -17,7 +16,11 @@ namespace rmcs_core::referee::app::ui { class StatusRing { public: StatusRing( - double supercap_limit, double battery_limit, double friction_limit, int16_t bullet_limit) { + double supercap_limit, double battery_limit, double friction_limit, int16_t bullet_limit, + bool supercap = true, bool battery = true) + : supercap_ui_{supercap} + , battery_ui_{battery} { + supercap_status_.set_x(x_center); supercap_status_.set_y(y_center); supercap_status_.set_r(visible_radius - width_ring); @@ -25,7 +28,7 @@ class StatusRing { supercap_status_.set_angle_end(275 + visible_angle); supercap_status_.set_width(width_ring); supercap_status_.set_color(Shape::Color::PINK); - supercap_status_.set_visible(true); + supercap_status_.set_visible(supercap); battery_status_.set_x(x_center); battery_status_.set_y(y_center); @@ -34,7 +37,7 @@ class StatusRing { battery_status_.set_angle_end(265); battery_status_.set_width(width_ring); battery_status_.set_color(Shape::Color::PINK); - battery_status_.set_visible(true); + battery_status_.set_visible(battery); friction_wheel_speed_.set_x(x_center); friction_wheel_speed_.set_y(y_center); @@ -112,8 +115,8 @@ class StatusRing { void set_visible(bool value) { // Dynamic - supercap_status_.set_visible(value); - battery_status_.set_visible(value); + supercap_status_.set_visible(value && supercap_ui_); + battery_status_.set_visible(value && battery_ui_); friction_wheel_speed_.set_visible(value); bullet_status_.set_visible(value); @@ -358,6 +361,9 @@ class StatusRing { double friction_limit_; int16_t bullet_limit_; + bool supercap_ui_ = true; + bool battery_ui_ = true; + // Dynamic part Arc supercap_status_; @@ -378,4 +384,4 @@ class StatusRing { Arc bullet_scales_[4]; }; -} // namespace rmcs_core::referee::app::ui \ No newline at end of file +} // namespace rmcs_core::referee::app::ui diff --git a/rmcs_ws/src/rmcs_core/src/referee/command/interaction/ui.cpp b/rmcs_ws/src/rmcs_core/src/referee/command/interaction/ui.cpp index cea7953c6..57167144b 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/command/interaction/ui.cpp +++ b/rmcs_ws/src/rmcs_core/src/referee/command/interaction/ui.cpp @@ -165,4 +165,4 @@ class Ui #include -PLUGINLIB_EXPORT_CLASS(rmcs_core::referee::command::interaction::Ui, rmcs_executor::Component) \ No newline at end of file +PLUGINLIB_EXPORT_CLASS(rmcs_core::referee::command::interaction::Ui, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/referee/status.cpp b/rmcs_ws/src/rmcs_core/src/referee/status.cpp index c080a355c..c29bc3c2b 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/status.cpp +++ b/rmcs_ws/src/rmcs_core/src/referee/status.cpp @@ -284,7 +284,8 @@ class Status *map_command_received_timestamp_ = std::chrono::duration(now.time_since_epoch()).count(); - if (has_last_map_command_ && std::memcmp(&last_map_command_, &data, sizeof(data)) == 0) { + if (has_last_map_command_ + && std::memcmp(&last_map_command_, &data, sizeof(data)) == 0) { // NOLINT return; } diff --git a/rmcs_ws/src/rmcs_description/include/rmcs_description/sentry_description.hpp b/rmcs_ws/src/rmcs_description/include/rmcs_description/sentry_description.hpp index 2fc9d17be..1ac5d4eef 100644 --- a/rmcs_ws/src/rmcs_description/include/rmcs_description/sentry_description.hpp +++ b/rmcs_ws/src/rmcs_description/include/rmcs_description/sentry_description.hpp @@ -66,7 +66,7 @@ struct RightFrontWheelLink : fast_tf::Link { template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::BaseLink; + using Parent = rmcs_description::BaseLink; Eigen::Translation3d transform = Eigen::Translation3d::Identity(); }; @@ -114,37 +114,37 @@ struct fast_tf::Joint : fast_tf::ModificationTracka template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::PitchLink; + using Parent = rmcs_description::PitchLink; Eigen::Translation3d transform = Eigen::Translation3d::Identity(); }; template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::PitchLink; + using Parent = rmcs_description::PitchLink; Eigen::Translation3d transform = Eigen::Translation3d::Identity(); }; template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::PitchLink; + using Parent = rmcs_description::PitchLink; Eigen::Isometry3d transform = Eigen::Isometry3d::Identity(); }; template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::BottomYawLink; + using Parent = rmcs_description::BottomYawLink; Eigen::Quaterniond transform = Eigen::Quaterniond::Identity(); }; template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::PitchLink; + using Parent = rmcs_description::PitchLink; Eigen::Quaterniond transform = Eigen::Quaterniond::Identity(); }; template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::BaseLink; + using Parent = rmcs_description::BaseLink; Eigen::Isometry3d transform = Eigen::Isometry3d::Identity(); void set_state(double angle) { auto rotation = Eigen::AngleAxisd{std::numbers::pi / 4, Eigen::Vector3d::UnitZ()} @@ -155,7 +155,7 @@ struct fast_tf::Joint : fast_tf::Modificat template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::BaseLink; + using Parent = rmcs_description::BaseLink; Eigen::Isometry3d transform = Eigen::Isometry3d::Identity(); void set_state(double angle) { auto rotation = Eigen::AngleAxisd{std::numbers::pi / 4 * 3, Eigen::Vector3d::UnitZ()} @@ -166,7 +166,7 @@ struct fast_tf::Joint : fast_tf::Modificati template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::BaseLink; + using Parent = rmcs_description::BaseLink; Eigen::Isometry3d transform = Eigen::Isometry3d::Identity(); void set_state(double angle) { auto rotation = Eigen::AngleAxisd{-std::numbers::pi / 4 * 3, Eigen::Vector3d::UnitZ()} @@ -177,7 +177,7 @@ struct fast_tf::Joint : fast_tf::Modificat template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::BaseLink; + using Parent = rmcs_description::BaseLink; Eigen::Isometry3d transform = Eigen::Isometry3d::Identity(); void set_state(double angle) { auto rotation = Eigen::AngleAxisd{-std::numbers::pi / 4, Eigen::Vector3d::UnitZ()} diff --git a/rmcs_ws/src/rmcs_description/include/rmcs_description/tf_description.hpp b/rmcs_ws/src/rmcs_description/include/rmcs_description/tf_description.hpp index 650987f58..4a46051ca 100644 --- a/rmcs_ws/src/rmcs_description/include/rmcs_description/tf_description.hpp +++ b/rmcs_ws/src/rmcs_description/include/rmcs_description/tf_description.hpp @@ -73,7 +73,7 @@ struct OmniLinkRight : fast_tf::Link { template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::BaseLink; + using Parent = rmcs_description::BaseLink; Eigen::Translation3d transform = Eigen::Translation3d::Identity(); }; @@ -101,25 +101,25 @@ struct fast_tf::Joint : fast_tf::ModificationTracka template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::PitchLink; + using Parent = rmcs_description::PitchLink; Eigen::Translation3d transform = Eigen::Translation3d::Identity(); }; template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::PitchLink; + using Parent = rmcs_description::PitchLink; Eigen::Translation3d transform = Eigen::Translation3d::Identity(); }; template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::PitchLink; + using Parent = rmcs_description::PitchLink; Eigen::Isometry3d transform = Eigen::Isometry3d::Identity(); }; template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::PitchLink; + using Parent = rmcs_description::PitchLink; Eigen::Quaterniond transform = Eigen::Quaterniond::Identity(); }; @@ -135,7 +135,7 @@ struct fast_tf::Joint : fast_tf::ModificationTrack }; template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::BaseLink; + using Parent = rmcs_description::BaseLink; Eigen::Isometry3d transform = Eigen::Isometry3d::Identity(); void set_state(double angle) { auto rotation = Eigen::AngleAxisd{std::numbers::pi / 4, Eigen::Vector3d::UnitZ()} @@ -146,7 +146,7 @@ struct fast_tf::Joint : fast_tf::Modificat template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::BaseLink; + using Parent = rmcs_description::BaseLink; Eigen::Isometry3d transform = Eigen::Isometry3d::Identity(); void set_state(double angle) { auto rotation = Eigen::AngleAxisd{std::numbers::pi / 4 * 3, Eigen::Vector3d::UnitZ()} @@ -157,7 +157,7 @@ struct fast_tf::Joint : fast_tf::Modificati template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::BaseLink; + using Parent = rmcs_description::BaseLink; Eigen::Isometry3d transform = Eigen::Isometry3d::Identity(); void set_state(double angle) { auto rotation = Eigen::AngleAxisd{-std::numbers::pi / 4 * 3, Eigen::Vector3d::UnitZ()} @@ -168,7 +168,7 @@ struct fast_tf::Joint : fast_tf::Modificat template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::BaseLink; + using Parent = rmcs_description::BaseLink; Eigen::Isometry3d transform = Eigen::Isometry3d::Identity(); void set_state(double angle) { auto rotation = Eigen::AngleAxisd{-std::numbers::pi / 4, Eigen::Vector3d::UnitZ()}