Add relevant code for drone gimbal control

ip.conf

@@ -0,0 +1 @@
+169.254.233.233

rmcs_ws/src/rmcs_bringup/config/flight.yaml

@@ -0,0 +1,192 @@
+rmcs_executor:
+  ros__parameters:
+    update_rate: 1000.0
+    components:
+      - rmcs_core::hardware::Flight -> flight_hardware
+      - rmcs_core::referee::Status -> referee_status
+
+      - rmcs_core::controller::gimbal::SimpleGimbalController -> gimbal_controller
+      - rmcs_core::controller::pid::ErrorPidController -> yaw_angle_pid_controller
+      - rmcs_core::controller::pid::PidController -> yaw_velocity_pid_controller
+      - rmcs_core::controller::pid::ErrorPidController -> pitch_angle_pid_controller
+
+      - rmcs_core::controller::shooting::FrictionWheelController -> friction_wheel_controller
+      - rmcs_core::controller::shooting::HeatController -> heat_controller
+      - rmcs_core::controller::shooting::BulletFeederController17mm -> bullet_feeder_controller
+      - rmcs_core::controller::pid::PidController -> left_friction_velocity_pid_controller
+      - rmcs_core::controller::pid::PidController -> right_friction_velocity_pid_controller
+      - rmcs_core::controller::pid::PidController -> bullet_feeder_velocity_pid_controller
+
+      - rmcs_core::referee::command::Interaction -> referee_interaction
+      #- rmcs_core::referee::command::interaction::Ui -> referee_ui
+      #- rmcs_core::referee::app::ui::Flight -> referee_ui_flight
+
+      - rmcs_core::referee::Command -> referee_command
+
+      # - rmcs_auto_aim::AutoAimInitializer -> auto_aim_initializer
+      # - rmcs_auto_aim::AutoAimController -> auto_aim_controller
+
+      - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster
+      - rmcs_core::broadcaster::TfBroadcaster -> tf_broadcaster
+
+value_broadcaster:
+  ros__parameters:
+    forward_list:
+      - /gimbal/pitch/angle
+      - /gimbal/pitch/velocity
+      - /gimbal/pitch/torque
+      - /gimbal/pitch/control_torque
+      - /gimbal/pitch/control_angle_error
+      - /gimbal/pitch/control_velocity
+      - /gimbal/pitch/velocity_imu
+
+      - /gimbal/yaw/angle
+      - /gimbal/yaw/velocity
+      - /gimbal/yaw/torque
+      - /gimbal/yaw/control_torque
+      - /gimbal/yaw/control_angle_error
+      - /gimbal/yaw/control_velocity
+
+tf_broadcaster:
+  ros__parameters:
+    tf: /tf
+
+flight_hardware:
+  ros__parameters:
+    usb_pid: -1
+    yaw_motor_zero_point: 4608
+    pitch_motor_zero_point: 51629
+
+referee_status:
+  ros__parameters:
+    path: /dev/tty0
+
+gimbal_controller:
+  ros__parameters:
+    upper_limit: -0.149
+    lower_limit: 0.149
+    yaw_upper_limit: -0.349
+    yaw_lower_limit: 0.349
+
+yaw_angle_pid_controller:
+  ros__parameters:
+    measurement: /gimbal/yaw/control_angle_error
+    control: /gimbal/yaw/control_velocity
+    kp: 8.0
+    ki: 0.0
+    kd: 0.0003
+
+yaw_velocity_pid_controller:
+  ros__parameters:
+    measurement: /gimbal/yaw/velocity_imu
+    setpoint: /gimbal/yaw/control_velocity
+    control: /gimbal/yaw/control_torque
+    kp: 1.5
+    ki: 0.0
+    kd: 0.01
+
+pitch_angle_pid_controller:
+  ros__parameters:
+    measurement: /gimbal/pitch/control_angle_error
+    control: /gimbal/pitch/control_velocity
+    kp: 16.0
+    ki: 0.0
+    kd: 0.0
+
+friction_wheel_controller:
+  ros__parameters:
+    friction_wheels:
+      - /gimbal/left_friction
+      - /gimbal/right_friction
+    friction_velocities:
+      - 740.0
+      - 740.0
+    friction_soft_start_stop_time: 1.0
+
+heat_controller:
+  ros__parameters:
+    heat_per_shot: 10000
+    reserved_heat: 0
+
+bullet_feeder_controller:
+  ros__parameters:
+    bullets_per_feeder_turn: 8.0
+    shot_frequency: 20.0
+    safe_shot_frequency: 10.0
+    eject_frequency: 10.0
+    eject_time: 0.05
+    deep_eject_frequency: 5.0
+    deep_eject_time: 0.2
+    single_shot_max_stop_delay: 2.0
+
+shooting_recorder:
+  ros__parameters:
+    friction_wheel_count: 2
+    # 1: trigger, 2: timing
+    log_mode: 1
+
+left_friction_velocity_pid_controller:
+  ros__parameters:
+    measurement: /gimbal/left_friction/velocity
+    setpoint: /gimbal/left_friction/control_velocity
+    control: /gimbal/left_friction/control_torque
+    kp: 0.003436926
+    ki: 0.00
+    kd: 0.009373434
+
+right_friction_velocity_pid_controller:
+  ros__parameters:
+    measurement: /gimbal/right_friction/velocity
+    setpoint: /gimbal/right_friction/control_velocity
+    control: /gimbal/right_friction/control_torque
+    kp: 0.003436926
+    ki: 0.00
+    kd: 0.009373434
+
+bullet_feeder_velocity_pid_controller:
+  ros__parameters:
+    measurement: /gimbal/bullet_feeder/velocity
+    setpoint: /gimbal/bullet_feeder/control_velocity
+    control: /gimbal/bullet_feeder/control_torque
+    kp: 0.583
+    ki: 0.0
+    kd: 0.0
+
+auto_aim_controller:
+  ros__parameters:
+    # capture
+    use_video: false # If true, use video stream instead of camera.
+    video_path: "/workspaces/RMCS/rmcs_ws/resources/1.avi"
+    exposure_time: 3
+    invert_image: false
+    # identifier
+    armor_model_path: "/models/mlp.onnx"
+    # pnp
+    fx: 1.722231837421459e+03
+    fy: 1.724876404292754e+03
+    cx: 7.013056440882832e+02
+    cy: 5.645821718351237e+02
+    k1: -0.064232403853946
+    k2: -0.087667493884102
+    k3: 0.792381808294582
+
+    # tracker
+    armor_predict_duration: 500
+    # controller
+    gimbal_predict_duration: 100
+    yaw_error: 0.02
+    pitch_error: -0.01
+    shoot_velocity: 28.0
+    predict_sec: 0.095
+    # etc
+    buff_predict_duration: 200
+    buff_model_path: "/models/buff_nocolor_v6.onnx"
+    omni_exposure: 1000.0
+    record_fps: 120
+    debug: false # Setup in actual using.Debug mode is used when referee is not ready
+    debug_color: 0 # 0 For blue while 1 for red. mine
+    debug_robot_id: 4
+    debug_buff_mode: false
+    record: true
+    raw_img_pub: false # Set false in actual use
+    image_viewer_type: 2

rmcs_ws/src/rmcs_core/plugins.xml

@@ -8,6 +8,9 @@
   <class type="rmcs_core::hardware::Hero" base_class_type="rmcs_executor::Component">
     <description>Test plugin.</description>
   </class>
+  <class type="rmcs_core::hardware::Flight" base_class_type="rmcs_executor::Component">
+    <description>Test plugin.</description>
+  </class>
   <class type="rmcs_core::hardware::SteeringHero" base_class_type="rmcs_executor::Component">
     <description>Test plugin.</description>
   </class>
@@ -68,6 +71,9 @@
   <class type="rmcs_core::broadcaster::TfBroadcaster" base_class_type="rmcs_executor::Component">
     <description>Test plugin.</description>
   </class>
+  <class type="GimbalTfPublisher" base_class_type="rmcs_executor::Component">
+    <description>Dynamic TF publisher for gimbal coordinate frames</description>
+  </class>
   <class type="rmcs_core::broadcaster::ValueBroadcaster" base_class_type="rmcs_executor::Component">
     <description>Test plugin.</description>
   </class>

rmcs_ws/src/rmcs_core/src/controller/gimbal/hero_gimbal_controller.cpp

@@ -25,7 +25,8 @@ class HeroGimbalController
               rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true))
         , imu_gimbal_solver(
               *this, get_parameter("upper_limit").as_double(),
-              get_parameter("lower_limit").as_double())
+              get_parameter("lower_limit").as_double(),get_parameter("yaw_upper_limit").as_double()
+            ,get_parameter("yaw_lower_limit").as_double())
         , encoder_gimbal_solver(
               *this, get_parameter("upper_limit").as_double(),
               get_parameter("lower_limit").as_double()) {

rmcs_ws/src/rmcs_core/src/controller/gimbal/simple_gimbal_controller.cpp

@@ -22,7 +22,8 @@ class SimpleGimbalController
               rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true))
         , two_axis_gimbal_solver(
               *this, get_parameter("upper_limit").as_double(),
-              get_parameter("lower_limit").as_double()) {
+              get_parameter("lower_limit").as_double(),get_parameter("yaw_upper_limit").as_double()
+            ,get_parameter("yaw_lower_limit").as_double()) {
 
         register_input("/remote/joystick/left", joystick_left_);
         register_input("/remote/switch/right", switch_right_);

rmcs_ws/src/rmcs_core/src/controller/gimbal/two_axis_gimbal_solver.hpp

@@ -1,5 +1,6 @@
 #pragma once
 
+#include "librmcs/utility/logging.hpp"
 #include <cmath>
 
 #include <iostream>
@@ -26,11 +27,16 @@ class TwoAxisGimbalSolver {
     };
 
 public:
-    TwoAxisGimbalSolver(rmcs_executor::Component& component, double upper_limit, double lower_limit)
+    TwoAxisGimbalSolver(
+        rmcs_executor::Component& component, double upper_limit, double lower_limit,
+        double yaw_upper_limit, double yaw_lower_limit)
         : upper_limit_(std::cos(upper_limit), -std::sin(upper_limit))
-        , lower_limit_(std::cos(lower_limit), -std::sin(lower_limit)) {
+        , lower_limit_(std::cos(lower_limit), -std::sin(lower_limit))
+        , yaw_upper_limit_(std::cos(yaw_upper_limit), -std::sin(yaw_upper_limit))
+        , yaw_lower_limit_(std::cos(yaw_lower_limit), -std::sin(yaw_lower_limit)) {
 
         component.register_input("/gimbal/pitch/angle", gimbal_pitch_angle_);
+        component.register_input("/gimbal/yaw/angle", gimbal_yaw_angle_);
         component.register_input("/tf", tf_);
     }
 
@@ -104,6 +110,11 @@ class TwoAxisGimbalSolver {
         update_yaw_axis();
 
         PitchLink::DirectionVector control_direction = operation.update(*this);
+        // LOG_INFO(
+        //     "x: %f, y: %f, z: %f", control_direction->x(), control_direction->y(),
+        //     control_direction->z());
+
+
         if (!control_enabled_)
             return {nan_, nan_};
 
@@ -113,6 +124,7 @@ class TwoAxisGimbalSolver {
         if (!control_enabled_)
             return {nan_, nan_};
 
+
         control_direction_ =
             fast_tf::cast<OdomImu>(yaw_link_to_pitch_link(control_direction_yaw_link, pitch), *tf_);
         return calculate_control_errors(control_direction_yaw_link, pitch);
@@ -167,8 +179,26 @@ class TwoAxisGimbalSolver {
             *control_direction << upper_limit_.x() * projection, upper_limit_.y();
         else if (z < lower_limit_.y())
             *control_direction << lower_limit_.x() * projection, lower_limit_.y();
+
+       const auto& [x_,y_,z_] = *control_direction;
+
+        Eigen::Vector2d yaw_projection{x_,y_};
+        double yaw_norm = yaw_projection.norm();
+        if (yaw_norm > 0)
+            yaw_projection /=yaw_norm;
+        else{
+            control_enabled_ =false;
+            return;
+        }
+
+        if (y_ > yaw_upper_limit_.y())
+            *control_direction << yaw_upper_limit_.x() * yaw_projection.x(), yaw_upper_limit_.y(), z_;
+        else if (y_ < yaw_lower_limit_.y())
+            *control_direction << yaw_lower_limit_.x() * yaw_projection.x(), yaw_lower_limit_.y(), z_;
+
     }
 
+
     static AngleError calculate_control_errors(
         const YawLink::DirectionVector& control_direction, const Eigen::Vector2d& pitch) {
         const auto& [x, y, z] = *control_direction;
@@ -179,14 +209,19 @@ class TwoAxisGimbalSolver {
         double x_projected = std::sqrt(x * x + y * y);
         result.pitch_angle_error = -std::atan2(z * c - x_projected * s, z * s + x_projected * c);
 
+        // LOG_INFO("x: %f, y: %f, z: %f, yaw err: %f, pitch err: %f",x, y,
+        // z,result.yaw_angle_error,result.pitch_angle_error);
+
         return result;
     }
 
     static constexpr double nan_ = std::numeric_limits<double>::quiet_NaN();
 
     const Eigen::Vector2d upper_limit_, lower_limit_;
+    const Eigen::Vector2d yaw_upper_limit_, yaw_lower_limit_;
 
     rmcs_executor::Component::InputInterface<double> gimbal_pitch_angle_;
+    rmcs_executor::Component::InputInterface<double> gimbal_yaw_angle_;
     rmcs_executor::Component::InputInterface<Tf> tf_;
 
     OdomImu::DirectionVector yaw_axis_filtered_{Eigen::Vector3d::UnitZ()};