diff --git a/ip.conf b/ip.conf new file mode 100644 index 00000000..24dd82fa --- /dev/null +++ b/ip.conf @@ -0,0 +1 @@ +169.254.233.233 diff --git a/rmcs_ws/src/rmcs_bringup/config/flight.yaml b/rmcs_ws/src/rmcs_bringup/config/flight.yaml new file mode 100644 index 00000000..f7d7ac40 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/librmcs b/rmcs_ws/src/rmcs_core/librmcs index b82f2eaf..80444ec9 160000 --- a/rmcs_ws/src/rmcs_core/librmcs +++ b/rmcs_ws/src/rmcs_core/librmcs @@ -1 +1 @@ -Subproject commit b82f2eafd21371a23d046e5f75884fe6c6e49124 +Subproject commit 80444ec9251a83e31bc153fe06cf91ddb5eece37 diff --git a/rmcs_ws/src/rmcs_core/plugins.xml b/rmcs_ws/src/rmcs_core/plugins.xml index ac7d1368..02efadb1 100644 --- a/rmcs_ws/src/rmcs_core/plugins.xml +++ b/rmcs_ws/src/rmcs_core/plugins.xml @@ -8,6 +8,9 @@ Test plugin. + + Test plugin. + Test plugin. @@ -68,6 +71,9 @@ Test plugin. + + Dynamic TF publisher for gimbal coordinate frames + Test plugin. diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/hero_gimbal_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/hero_gimbal_controller.cpp index f20bd98f..65417862 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/hero_gimbal_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/hero_gimbal_controller.cpp @@ -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()) { diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/simple_gimbal_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/simple_gimbal_controller.cpp index d27704d5..517dc0fc 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 @@ -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_); diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/two_axis_gimbal_solver.hpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/two_axis_gimbal_solver.hpp index 5d937aae..b218e27f 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/two_axis_gimbal_solver.hpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/two_axis_gimbal_solver.hpp @@ -1,5 +1,6 @@ #pragma once +#include "librmcs/utility/logging.hpp" #include #include @@ -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(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::quiet_NaN(); const Eigen::Vector2d upper_limit_, lower_limit_; + const Eigen::Vector2d yaw_upper_limit_, yaw_lower_limit_; rmcs_executor::Component::InputInterface gimbal_pitch_angle_; + rmcs_executor::Component::InputInterface gimbal_yaw_angle_; rmcs_executor::Component::InputInterface tf_; OdomImu::DirectionVector yaw_axis_filtered_{Eigen::Vector3d::UnitZ()}; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp b/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp new file mode 100644 index 00000000..5d741135 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp @@ -0,0 +1,248 @@ +#include +#include + +#include +#include + +#include +#include +#include +#include + +#include +#include + +#include "hardware/device/bmi088.hpp" +#include "hardware/device/dji_motor.hpp" +#include "hardware/device/dr16.hpp" +#include "hardware/device/lk_motor.hpp" +#include "librmcs/utility/ring_buffer.hpp" + +namespace rmcs_core::hardware { + +class Flight + : public rmcs_executor::Component + , public rclcpp::Node + , private librmcs::client::CBoard { +public: + Flight() + : Node{ + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} + , librmcs::client::CBoard{static_cast(get_parameter("usb_pid").as_int())} + , logger_(get_logger()) + , flight_command_( + create_partner_component(get_component_name() + "_command", *this)) + , gimbal_yaw_motor_(*this, *flight_command_, "/gimbal/yaw") + , gimbal_pitch_motor_(*this, *flight_command_, "/gimbal/pitch") + , gimbal_left_friction_(*this, *flight_command_, "/gimbal/left_friction") + , gimbal_right_friction_(*this, *flight_command_, "/gimbal/right_friction") + , gimbal_bullet_feeder_(*this, *flight_command_, "/gimbal/bullet_feeder") + , dr16_(*this) + , bmi088_(500.0, 0.3, 0.005) + + , transmit_buffer_(*this, 32) + , event_thread_([this]() { handle_events(); }) { + gimbal_yaw_motor_.configure( + device::LkMotor::Config{device::LkMotor::Type::MHF7015}.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::MG4010E_I10}.set_encoder_zero_point( + static_cast(get_parameter("pitch_motor_zero_point").as_int()))); + gimbal_left_friction_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::M3508} + .set_reversed() + .set_reduction_ratio(1.)); + gimbal_right_friction_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::M3508} + + .set_reduction_ratio(1.)); + gimbal_bullet_feeder_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::M2006}.enable_multi_turn_angle()); + + register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_); + register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_imu_); + register_output("/tf", tf_); + + bmi088_.set_coordinate_mapping( + [](double x, double y, double z) { return std::make_tuple(-x, z, y); }); + + using namespace rmcs_description; + + constexpr double rotor_distance_x = 0.83637; + constexpr double rotor_distance_y = 0.83637; + + tf_->set_transform( + Eigen::Translation3d{rotor_distance_x / 2, rotor_distance_y / 2, 0}); + tf_->set_transform( + Eigen::Translation3d{rotor_distance_x / 2, -rotor_distance_y / 2, 0}); + tf_->set_transform( + Eigen::Translation3d{-rotor_distance_x / 2, rotor_distance_y / 2, 0}); + tf_->set_transform( + Eigen::Translation3d{-rotor_distance_x / 2, -rotor_distance_y / 2, 0}); + + constexpr double gimbal_center_x = 0; + constexpr double gimbal_center_y = 0; + constexpr double gimbal_center_z = -0.20552; + tf_->set_transform( + Eigen::Translation3d{gimbal_center_x, gimbal_center_y, gimbal_center_z}); + + tf_->set_transform(Eigen::Translation3d{0.0557, 0, 0.053}); + + gimbal_calibrate_subscription_ = create_subscription( + "/gimbal/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { + gimbal_calibrate_subscription_callback(std::move(msg)); + }); + register_output("/referee/serial", referee_serial_); + referee_serial_->read = [this](std::byte* buffer, size_t size) { + return referee_ring_buffer_receive_.pop_front_multi( + [&buffer](std::byte byte) { *buffer++ = byte; }, size); + }; + referee_serial_->write = [this](const std::byte* buffer, size_t size) { + transmit_buffer_.add_uart1_transmission(buffer, size); + return size; + }; + } + + ~Flight() override { + stop_handling_events(); + event_thread_.join(); + } + + void update() override { + update_motors(); + update_imu(); + dr16_.update_status(); + + } + + void command_update() { + uint16_t can_commands[4]; + + transmit_buffer_.add_can1_transmission(0x141, gimbal_yaw_motor_.generate_torque_command()); + transmit_buffer_.add_can2_transmission(0x144, gimbal_pitch_motor_.generate_command()); + + can_commands[0] = gimbal_bullet_feeder_.generate_command(); + can_commands[1] = gimbal_right_friction_.generate_command(); + can_commands[2] = gimbal_left_friction_.generate_command(); + can_commands[3] = 0; + transmit_buffer_.add_can2_transmission(0x200, std::bit_cast(can_commands)); + + transmit_buffer_.trigger_transmission(); + } + +private: + void update_motors() { + using namespace rmcs_description; + gimbal_yaw_motor_.update_status(); + tf_->set_state(gimbal_yaw_motor_.angle()); + + gimbal_pitch_motor_.update_status(); + tf_->set_state(gimbal_pitch_motor_.angle()); + + gimbal_bullet_feeder_.update_status(); + gimbal_left_friction_.update_status(); + gimbal_right_friction_.update_status(); + } + + void update_imu() { + bmi088_.update_status(); + Eigen::Quaterniond gimbal_imu_pose{bmi088_.q0(), bmi088_.q1(), bmi088_.q2(), bmi088_.q3()}; + tf_->set_transform( + gimbal_imu_pose.conjugate()); + + *gimbal_yaw_velocity_imu_ = bmi088_.gz(); + *gimbal_pitch_velocity_imu_ = bmi088_.gy(); + } + void gimbal_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr) { + RCLCPP_INFO( + logger_, "[gimbal calibration] New yaw offset: %ld", + gimbal_yaw_motor_.calibrate_zero_point()); + RCLCPP_INFO( + logger_, "[gimbal calibration] New pitch offset: %ld", + gimbal_pitch_motor_.calibrate_zero_point()); + } + +protected: + void can1_receive_callback( + uint32_t can_id, uint64_t can_data, bool is_extended_can_id, bool is_remote_transmission, + uint8_t can_data_length) override { + if (is_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] + return; + + if (can_id == 0x141) { + gimbal_yaw_motor_.store_status(can_data); + } + } + + void can2_receive_callback( + uint32_t can_id, uint64_t can_data, bool is_extended_can_id, bool is_remote_transmission, + uint8_t can_data_length) override { + if (is_remote_transmission || is_extended_can_id || can_data_length < 8) [[unlikely]] + return; + if (can_id == 0x201) { + gimbal_bullet_feeder_.store_status(can_data); + } else if (can_id == 0x203) { + gimbal_left_friction_.store_status(can_data); + } else if (can_id == 0x202) { + gimbal_right_friction_.store_status(can_data); + } else if (can_id == 0x144) { + gimbal_pitch_motor_.store_status(can_data); + } + } + + void uart1_receive_callback(const std::byte* uart_data, uint8_t uart_data_length) override { + referee_ring_buffer_receive_.emplace_back_multi( + [&uart_data](std::byte* storage) { *storage = *uart_data++; }, uart_data_length); + } + + void dbus_receive_callback(const std::byte* uart_data, uint8_t uart_data_length) override { + dr16_.store_status(uart_data, uart_data_length); + } + + void accelerometer_receive_callback(int16_t x, int16_t y, int16_t z) override { + bmi088_.store_accelerometer_status(x, y, z); + } + + void gyroscope_receive_callback(int16_t x, int16_t y, int16_t z) override { + bmi088_.store_gyroscope_status(x, y, z); + } + +private: + rclcpp::Logger logger_; + class FlightCommand : public rmcs_executor::Component { + public: + explicit FlightCommand(Flight& flight) + : flight_(flight) {} + void update() override { flight_.command_update(); } + Flight& flight_; + }; + + std::shared_ptr flight_command_; + rclcpp::Subscription::SharedPtr gimbal_calibrate_subscription_; + device::LkMotor gimbal_yaw_motor_; + device::LkMotor gimbal_pitch_motor_; + + device::DjiMotor gimbal_left_friction_; + device::DjiMotor gimbal_right_friction_; + device::DjiMotor gimbal_bullet_feeder_; + + device::Dr16 dr16_; + device::Bmi088 bmi088_; + + OutputInterface gimbal_yaw_velocity_imu_; + OutputInterface gimbal_pitch_velocity_imu_; + + OutputInterface tf_; + + librmcs::utility::RingBuffer referee_ring_buffer_receive_{256}; + OutputInterface referee_serial_; + + librmcs::client::CBoard::TransmitBuffer transmit_buffer_; + std::thread event_thread_; +}; +} // namespace rmcs_core::hardware + +#include + +PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::Flight, rmcs_executor::Component) \ No newline at end of file