diff --git a/.github/workflows/update-image.yml b/.github/workflows/update-image.yml deleted file mode 100644 index 71c5538f..00000000 --- a/.github/workflows/update-image.yml +++ /dev/null @@ -1,49 +0,0 @@ -name: Build and Push RMCS Images - -on: - workflow_dispatch: - push: - paths: - - 'Dockerfile' - branches: - - main - -jobs: - build-and-push: - runs-on: ubuntu-latest - - steps: - - name: Checkout repository - uses: actions/checkout@v4 - - - name: Set up Docker Buildx - uses: docker/setup-buildx-action@v3 - - - name: Log in to Docker Hub - uses: docker/login-action@v3 - with: - username: ${{ secrets.DOCKERHUB_USERNAME }} - password: ${{ secrets.DOCKERHUB_TOKEN }} - - - name: Set up SSH keys - run: | - echo "${{ secrets.CONTAINER_ID_RSA }}" > .ssh/id_rsa - echo "${{ secrets.CONTAINER_ID_RSA_PUB }}" > .ssh/id_rsa.pub - chmod 600 .ssh/id_rsa - chmod 644 .ssh/id_rsa.pub - - - name: Build and push rmcs-develop:latest - uses: docker/build-push-action@v6 - with: - context: . - push: true - target: rmcs-develop - tags: qzhhhi/rmcs-develop:latest - - - name: Build and push rmcs-runtime:latest - uses: docker/build-push-action@v6 - with: - context: . - push: true - target: rmcs-runtime - tags: qzhhhi/rmcs-runtime:latest diff --git a/.script/complete/_play-autoaim b/.script/complete/_play-autoaim new file mode 100644 index 00000000..025a0974 --- /dev/null +++ b/.script/complete/_play-autoaim @@ -0,0 +1,7 @@ +#compdef play-autoaim + +_arguments \ + '--user[指定远程用户名]:用户名:' \ + '--remote[从设备拉取 SDP 到容器]' \ + '--no-copy[跳过容器到宿主机的拷贝]'\ + '--ip[监视端的 IP 地址]' diff --git a/.script/complete/_set-remote b/.script/complete/_set-remote new file mode 100644 index 00000000..e5599c3f --- /dev/null +++ b/.script/complete/_set-remote @@ -0,0 +1,22 @@ +#compdef set-remote + +_remote_hosts() { + local hosts=( + "169.254.233.233" + "alliance-sentry.local" + "alliance-infantry.local" + "alliance-hero.local" + ) + + # 从 ~/.ssh/config 提取最近使用的 HostName + if [[ -f ~/.ssh/config ]]; then + local extracted + extracted=(${(f)"$(grep -A1 'Host remote' ~/.ssh/config | grep HostName | awk '{print $2}')"}) + hosts+=(${extracted}) + fi + + compadd -- $hosts +} + +_arguments \ + '1:远程主机地址:_remote_hosts' diff --git a/.script/foxglove b/.script/foxglove new file mode 100755 index 00000000..ba845e52 --- /dev/null +++ b/.script/foxglove @@ -0,0 +1,5 @@ +#! /bin/bash + +source ~/env_setup.bash + +ros2 launch foxglove_bridge foxglove_bridge_launch.xml port:=8765 diff --git a/.script/play-autoaim b/.script/play-autoaim index 67e86159..b9a33f0f 100755 --- a/.script/play-autoaim +++ b/.script/play-autoaim @@ -1,38 +1,74 @@ #!/bin/sh -if [ "$#" -ne 1 ]; then - echo "用法: $0 <用户名>" - echo "示例: $0 user" - exit 1 -fi +set -e -REMOTE_PLAYER="vlc" +MONITOR_USER="" +MONITOR_PLAYER="vlc" +MONITOR_HOST="localhost" -REMOTE_USER="$1" -REMOTE_HOST="localhost" +SDP_PATH="/tmp/auto_aim.sdp" -REMOTE_PATH="/tmp/auto_aim.sdp" -LOCAL_PATH="/tmp/auto_aim.sdp" +USE_REMOTE=false +SKIP_COPY=false + +# 参数解析 +while [ "$#" -gt 0 ]; do + case "$1" in + --user) + shift + [ -z "$1" ] && echo "❌ --user 参数缺失" && exit 1 + MONITOR_USER="$1" + ;; + --remote) + USE_REMOTE=true + ;; + --no-copy) + SKIP_COPY=true + ;; + --ip) + shift + [ -z "$1" ] && echo "❌ --ip 参数缺失" && exit 1 + MONITOR_HOST="$1" + ;; + *) + echo "未知参数: $1" + echo "用法: $0 --user <用户名> [--remote] [--no-copy]" + exit 1 + ;; + esac + shift +done -scp remote:${REMOTE_PATH} ${REMOTE_PATH} -if [ $? -ne 0 ]; then - echo "⚠️ 从 remote 拷贝失败。是否继续？(y/n)" +if [ -z "$MONITOR_USER" ]; then + echo "❌ 缺少 --user 参数" + echo "用法: play-autoaim --user <用户名> [--remote] [--no-copy]" + exit 1 +fi + +if [ "$USE_REMOTE" = true ]; then + scp remote:${SDP_PATH} ${SDP_PATH} + if [ $? -ne 0 ]; then + echo " 从 remote 拷贝失败。是否继续？(y/n)" read -r answer case "$answer" in - [Yy]*) echo "继续执行后续操作…" ;; + [Yy]*) echo " 继续执行后续操作…" ;; *) - echo "已取消。" - exit 1 - ;; + echo " 已取消。" + exit 1 + ;; esac + fi fi -scp "${REMOTE_PATH}" "${REMOTE_USER}@${REMOTE_HOST}:${LOCAL_PATH}" -if [ $? -ne 0 ]; then - echo "❌ scp 拷贝失败" +if [ "$SKIP_COPY" = false ]; then + scp "${SDP_PATH}" "${MONITOR_USER}@${MONITOR_HOST}:${SDP_PATH}" + if [ $? -ne 0 ]; then + echo " scp 拷贝失败" exit 1 + fi + echo " 文件已拷贝到宿主机：${SDP_PATH}" +else + echo " 跳过拷贝，直接打开远程 SDP" fi -echo "✅ 文件已拷贝到宿主机：${LOCAL_PATH}" - -ssh -f "${REMOTE_USER}@${REMOTE_HOST}" "DISPLAY=:0 ${REMOTE_PLAYER} '${LOCAL_PATH}'" +ssh -f "${MONITOR_USER}@${MONITOR_HOST}" "DISPLAY=:0 ${MONITOR_PLAYER} '${SDP_PATH}' --network-caching=50" diff --git a/.script/template/env_setup.bash b/.script/template/env_setup.bash index f8ab1dd2..341647d3 100644 --- a/.script/template/env_setup.bash +++ b/.script/template/env_setup.bash @@ -1,6 +1,6 @@ #!/bin/bash -export ROS_LOCALHOST_ONLY=1 +export ROS_AUTOMATIC_DISCOVERY_RANGE=LOCALHOST export RCUTILS_COLORIZED_OUTPUT=1 export RMCS_PATH="/workspaces/RMCS" diff --git a/.script/template/env_setup.zsh b/.script/template/env_setup.zsh index 335250c9..fab840df 100644 --- a/.script/template/env_setup.zsh +++ b/.script/template/env_setup.zsh @@ -1,7 +1,7 @@ -#!/bin/bash +#!/bin/zsh +export ROS_AUTOMATIC_DISCOVERY_RANGE=LOCALHOST export RCUTILS_COLORIZED_OUTPUT=1 -export ROS_LOCALHOST_ONLY=1 export RMCS_PATH="/workspaces/RMCS" source /opt/ros/jazzy/setup.zsh @@ -16,3 +16,7 @@ eval "$(register-python-argcomplete ros2)" eval "$(register-python-argcomplete colcon)" export RMCS_ROBOT_TYPE="" + +fpath=(${RMCS_PATH}/.script/complete $fpath) +autoload -Uz compinit +compinit diff --git a/Dockerfile b/Dockerfile index 8e59ba3e..22ba59bd 100644 --- a/Dockerfile +++ b/Dockerfile @@ -88,19 +88,25 @@ RUN apt-get update && apt-get install -y --no-install-recommends \ openssh-client \ lsb-release software-properties-common gnupg sudo \ python3-colorama python3-dpkt && \ - wget -O ./llvm-snapshot.gpg.key https://apt.llvm.org/llvm-snapshot.gpg.key && \ - apt-key add ./llvm-snapshot.gpg.key && \ - rm ./llvm-snapshot.gpg.key && \ - echo "deb https://apt.llvm.org/noble/ llvm-toolchain-noble main" > /etc/apt/sources.list.d/llvm-apt.list && \ - apt-get update && \ - version=`apt-cache search clangd- | grep clangd- | awk -F' ' '{print $1}' | sort -V | tail -1 | cut -d- -f2` && \ - apt-get install -y --no-install-recommends clangd-$version && \ update-alternatives --install /usr/bin/gcc gcc /usr/bin/gcc-14 50 && \ update-alternatives --install /usr/bin/g++ g++ /usr/bin/g++-14 50 && \ - update-alternatives --install /usr/bin/clangd clangd /usr/bin/clangd-$version 50 && \ apt-get autoremove -y && apt-get clean && \ rm -rf /var/lib/apt/lists/* /tmp/* +# Install latest stable llvm-toolchain +RUN mkdir -p /etc/apt/keyrings && \ + wget -O - https://apt.llvm.org/llvm-snapshot.gpg.key | gpg -o /etc/apt/keyrings/llvm-snapshot.gpg --dearmor && \ + echo "deb [signed-by=/etc/apt/keyrings/llvm-snapshot.gpg] https://mirrors.tuna.tsinghua.edu.cn/llvm-apt/noble/ llvm-toolchain-noble-22 main" \ + | tee /etc/apt/sources.list.d/llvm.list && \ + apt-get update && \ + apt-get install -y --no-install-recommends clang-22 clangd-22 clang-format-22 lldb-22 && \ + update-alternatives --install /usr/bin/clang clang /usr/bin/clang-22 100 && \ + update-alternatives --install /usr/bin/clang++ clang++ /usr/bin/clang++-22 100 && \ + update-alternatives --install /usr/bin/clangd clangd /usr/bin/clangd-22 100 && \ + update-alternatives --install /usr/bin/clang-format clang-format /usr/bin/clang-format-22 100 && \ + update-alternatives --install /usr/bin/lldb lldb /usr/bin/lldb-22 100 && \ + apt-get autoremove -y && apt-get clean && rm -rf /var/lib/apt/lists/* /tmp/* + # Generate/load ssh key and setup unison RUN --mount=type=bind,target=/tmp/.ssh,source=.ssh,readonly=false \ cd /home/ubuntu && mkdir -p .ssh && \ @@ -117,6 +123,11 @@ RUN curl -LO https://github.com/neovim/neovim/releases/latest/download/nvim-linu tar -C /opt -xzf nvim-linux-x86_64.tar.gz && \ rm nvim-linux-x86_64.tar.gz +# Install latest stable cmake for user ubuntu +RUN wget https://github.com/kitware/cmake/releases/download/v4.2.3/cmake-4.2.3-linux-x86_64.sh -O install.sh && \ + mkdir -p /opt/cmake/ && bash install.sh --skip-license --prefix=/opt/cmake/ --exclude-subdir && \ + rm install.sh + # Change user RUN chsh -s /bin/zsh ubuntu && \ echo "ubuntu ALL=(ALL:ALL) NOPASSWD:ALL" >> /etc/sudoers @@ -130,12 +141,14 @@ RUN sh -c "$(wget https://raw.githubusercontent.com/ohmyzsh/ohmyzsh/master/tools sed -i 's/ZSH_THEME=\"[a-z0-9\-]*\"/ZSH_THEME="af-magic"/g' ~/.zshrc && \ echo 'source ~/env_setup.zsh' >> ~/.zshrc && \ echo 'export PATH="${PATH}:/opt/nvim-linux-x86_64/bin"' >> ~/.zshrc && \ + # echo 'export PATH="${PATH}:/opt/cmake/bin"' >> ~/.zshrc && \ echo 'export PATH="${PATH}:${RMCS_PATH}/.script"' >> ~/.zshrc # Copy environment setup scripts COPY --chown=1000:1000 .script/template/env_setup.bash env_setup.bash COPY --chown=1000:1000 .script/template/env_setup.zsh env_setup.zsh + # Runtime container, will automatically launch the main program FROM rmcs-base AS rmcs-runtime diff --git a/README.md b/README.md index 1bb950ab..dc365f69 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,8 @@ # RMCS RoboMaster Control System based on ROS2. +快速开始: [quick-start](https://github.com/Alliance-Algorithm/RMCS/wiki/Quick-Start) + ## Development ### Pre-requirements: diff --git a/docker-compose.yml b/docker-compose.yml index d59afc5b..43ec6c74 100644 --- a/docker-compose.yml +++ b/docker-compose.yml @@ -1,17 +1,11 @@ services: rmcs-develop: image: qzhhhi/rmcs-develop:latest - user: "${CONTAINER_USER}" - privileged: true - command: > - bash -c " - sudo chown -R ${CONTAINER_USER}:${CONTAINER_USER} ${CONTAINER_HOME}/.config - exec bash - " + user: "1000:1000" volumes: - /dev:/dev:bind - /tmp/.X11-unix:/tmp/.X11-unix:bind - - /run/user/1000/wayland-0:/run/user/1000/wayland-0 + - /run/user/1000/wayland-0:/run/user/1000/wayland-0:bind - ${HOST_NVIM_DIR}:${CONTAINER_HOME}/.config/nvim/:bind - .:/workspaces/RMCS:bind environment: @@ -19,5 +13,4 @@ services: - WAYLAND_DISPLAY=${WAYLAND_DISPLAY} network_mode: host tty: true - stdin_open: true - + stdin_open: true 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/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..a8f3e7d8 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 @@ -7,8 +8,6 @@ #include #include -#include -#include #include #include #include @@ -26,11 +25,16 @@ class TwoAxisGimbalSolver { }; public: - TwoAxisGimbalSolver(rmcs_executor::Component& component, double upper_limit, double lower_limit) + TwoAxisGimbalSolver( + rmcs_executor::Component& component, double upper_limit, double lower_limit, + double yaw_upper_limit, double yaw_lower_limit) : upper_limit_(std::cos(upper_limit), -std::sin(upper_limit)) - , lower_limit_(std::cos(lower_limit), -std::sin(lower_limit)) { + , lower_limit_(std::cos(lower_limit), -std::sin(lower_limit)) + , yaw_upper_limit_(std::cos(yaw_upper_limit), -std::sin(yaw_upper_limit)) + , yaw_lower_limit_(std::cos(yaw_lower_limit), -std::sin(yaw_lower_limit)) { component.register_input("/gimbal/pitch/angle", gimbal_pitch_angle_); + component.register_input("/gimbal/yaw/angle", gimbal_yaw_angle_); component.register_input("/tf", tf_); } @@ -104,6 +108,7 @@ class TwoAxisGimbalSolver { update_yaw_axis(); PitchLink::DirectionVector control_direction = operation.update(*this); + if (!control_enabled_) return {nan_, nan_}; @@ -167,8 +172,22 @@ class TwoAxisGimbalSolver { *control_direction << upper_limit_.x() * projection, upper_limit_.y(); else if (z < lower_limit_.y()) *control_direction << lower_limit_.x() * projection, lower_limit_.y(); + + const auto& [yaw_x, yaw_y, yaw_z] = *control_direction; + + Eigen::Vector2d xz_projection{yaw_x, yaw_z}; + double xz_norm = xz_projection.norm(); + if (xz_norm > 0) + xz_projection /= xz_norm; + + if (yaw_y > yaw_upper_limit_.y()) + *control_direction << yaw_upper_limit_.x() * xz_projection.x(), yaw_upper_limit_.x(), yaw_upper_limit_.x() * xz_projection.y(); + else if (yaw_y < yaw_lower_limit_.y()) + *control_direction << yaw_lower_limit_.x() * xz_projection.x(), yaw_lower_limit_.x(), yaw_lower_limit_.x() * xz_projection.y(); + } + static AngleError calculate_control_errors( const YawLink::DirectionVector& control_direction, const Eigen::Vector2d& pitch) { const auto& [x, y, z] = *control_direction; @@ -185,8 +204,10 @@ class TwoAxisGimbalSolver { static constexpr double nan_ = std::numeric_limits::quiet_NaN(); const Eigen::Vector2d upper_limit_, lower_limit_; + const Eigen::Vector2d yaw_upper_limit_, yaw_lower_limit_; rmcs_executor::Component::InputInterface gimbal_pitch_angle_; + rmcs_executor::Component::InputInterface gimbal_yaw_angle_; rmcs_executor::Component::InputInterface tf_; OdomImu::DirectionVector yaw_axis_filtered_{Eigen::Vector3d::UnitZ()}; @@ -195,4 +216,4 @@ class TwoAxisGimbalSolver { OdomImu::DirectionVector control_direction_; }; -} // namespace rmcs_core::controller::gimbal \ No newline at end of file +} // namespace rmcs_core::controller::gimbal diff --git a/rmcs_ws/src/rmcs_core/src/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 diff --git a/rmcs_ws/src/rmcs_executor/include/rmcs_executor/component.hpp b/rmcs_ws/src/rmcs_executor/include/rmcs_executor/component.hpp index cba9b55f..9155e7a9 100644 --- a/rmcs_ws/src/rmcs_executor/include/rmcs_executor/component.hpp +++ b/rmcs_ws/src/rmcs_executor/include/rmcs_executor/component.hpp @@ -16,12 +16,12 @@ class Component { public: friend class Executor; - Component(const Component&) = delete; + Component(const Component&) = delete; Component& operator=(const Component&) = delete; - Component(Component&&) = delete; - Component& operator=(Component&&) = delete; + Component(Component&&) = delete; + Component& operator=(Component&&) = delete; - virtual ~Component(){}; + virtual ~Component() = default; virtual void before_pairing(const std::map& output_map) { (void)output_map; @@ -37,10 +37,10 @@ class Component { InputInterface() = default; - InputInterface(const InputInterface&) = delete; + InputInterface(const InputInterface&) = delete; InputInterface& operator=(const InputInterface&) = delete; - InputInterface(InputInterface&&) = delete; - InputInterface& operator=(InputInterface&&) = delete; + InputInterface(InputInterface&&) = delete; + InputInterface& operator=(InputInterface&&) = delete; ~InputInterface() { if (delete_data_when_deconstruct) { @@ -61,7 +61,7 @@ class Component { throw std::runtime_error("The interface has already been bound to somewhere"); data_pointer_ = new T(std::forward(args)...); - activated = true; + activated = true; delete_data_when_deconstruct = true; } @@ -71,7 +71,7 @@ class Component { throw std::runtime_error("The interface has already been bound to somewhere"); data_pointer_ = const_cast(&destination); - activated = true; + activated = true; } const T* operator->() const { return data_pointer_; } @@ -84,7 +84,7 @@ class Component { } T* data_pointer_ = nullptr; - bool activated = false; + bool activated = false; bool delete_data_when_deconstruct = false; }; @@ -96,10 +96,10 @@ class Component { OutputInterface() = default; - OutputInterface(const OutputInterface&) = delete; + OutputInterface(const OutputInterface&) = delete; OutputInterface& operator=(const OutputInterface&) = delete; - OutputInterface(OutputInterface&&) = delete; - OutputInterface& operator=(OutputInterface&&) = delete; + OutputInterface(OutputInterface&&) = delete; + OutputInterface& operator=(OutputInterface&&) = delete; ~OutputInterface() { if (active()) @@ -136,6 +136,7 @@ class Component { } template + requires std::constructible_from void register_output(const std::string& name, OutputInterface& interface, Args&&... args) { if (interface.active()) throw std::runtime_error("The interface has been activated"); @@ -144,6 +145,7 @@ class Component { } template + requires std::constructible_from std::shared_ptr create_partner_component(const std::string& name, Args&&... args) { initializing_component_name = name.c_str(); @@ -182,8 +184,8 @@ class Component { std::vector> partner_component_list_; - size_t dependency_count_ = 0; + std::size_t dependency_count_ = 0; std::unordered_set wanted_by_ = {}; }; -} // namespace rmcs_executor \ No newline at end of file +} // namespace rmcs_executor